From df371e6e933658170a72edcc9a85f4cca80cd4c8 Mon Sep 17 00:00:00 2001 From: ashwinsushil Date: Fri, 15 Nov 2019 13:46:32 +0100 Subject: [PATCH 001/861] Append the device only if the manufacturer name is 'Bitcraze AB' When devices with the same VendorID and ProductID as the roadrunner/bitcraze is connected, the cflib fails to connect to the device. A quick fix for this is done by checking if the manufacturer name is 'Bitcraze AB' --- cflib/drivers/cfusb.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index c0452e9e0..07d25d67f 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -65,7 +65,8 @@ def _find_devices(): if pyusb1: for d in usb.core.find(idVendor=USB_VID, idProduct=USB_PID, find_all=1, backend=pyusb_backend): - ret.append(d) + if d.manufacturer == 'Bitcraze AB': + ret.append(d) else: busses = usb.busses() for bus in busses: From ceff1f237aef2e7e625dae0bcd2394fa3c6ab2db Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 9 Dec 2019 16:52:40 +0100 Subject: [PATCH 002/861] #132 Added example for connecting to a Qualisys system --- examples/qualisys/qualisys_hl_commander.py | 284 +++++++++++++++++++++ 1 file changed, 284 insertions(+) create mode 100644 examples/qualisys/qualisys_hl_commander.py diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py new file mode 100644 index 000000000..13c783103 --- /dev/null +++ b/examples/qualisys/qualisys_hl_commander.py @@ -0,0 +1,284 @@ +# -*- 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 +ridgid_body_name to match the name of the Crazyflie in QTM. + +Limitations: This script does not support full pose and the Crazyflie +must be started facing positive X. +""" +import asyncio +import math +import time +import xml.etree.cElementTree as ET +from threading import Thread + +import qtm + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.mem import Poly4D +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.syncLogger import SyncLogger + + +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M' + +# The name of the rigid body in QTM that represents the Crazyflie +ridgid_body_name = 'cf' + +# 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.connect(host) + + params = await self.connection.get_parameters(parameters=['6d']) + xml = ET.fromstring(params) + self.qtm_6DoF_labels = [label.text for label in xml.iter('Name')] + print(self.qtm_6DoF_labels) + + await self.connection.stream_frames( + components=['6deuler'], + on_packet=self._on_packet) + + async def _discover(self): + async for qtm_instance in qtm.Discover('0.0.0.0'): + return qtm_instance + + def _on_packet(self, packet): + header, bodies = packet.get_6d_euler() + + 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 + + # Note: This is roll/pitch/yaw with Qualisys standard settings + # which is different from Crazyflie standard. Roll/pich/yaw is + # not applied in the same order in the two systems. + roll = math.radians(temp_cf_pos[1][0]) + pitch = math.radians(temp_cf_pos[1][1]) + yaw = math.radians(temp_cf_pos[1][2]) + + if self.on_pose: + if math.isnan(x): + return + + self.on_pose([x, y, z, roll, pitch, yaw]) + + async def _close(self): + await self.connection.stream_frames_stop() + self.connection.disconnect() + + +class Uploader: + def __init__(self): + self._is_done = False + + def upload(self, trajectory_mem): + print('Uploading data') + trajectory_mem.write_data(self._upload_done) + + while not self._is_done: + time.sleep(0.2) + + def _upload_done(self, mem, addr): + print('Data uploaded') + self._is_done = True + + +def wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +def reset_estimator(cf): + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + time.sleep(1) + # wait_for_position_estimator(cf) + + +def activate_kalman_estimator(cf): + cf.param.set_value('stabilizer.estimator', '2') + + +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] + + 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.poly4Ds.append(Poly4D(duration, x, y, z, yaw)) + total_duration += duration + + Uploader().upload(trajectory_mem) + cf.high_level_commander.define_trajectory(trajectory_id, 0, + len(trajectory_mem.poly4Ds)) + 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() + + +cflib.crtp.init_drivers(enable_debug_driver=False) + +# Connect to QTM +qtm_wrapper = QtmWrapper(ridgid_body_name) + +with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + trajectory_id = 1 + + activate_kalman_estimator(cf) + + # Set up a callback to handle data from QTM + qtm_wrapper.on_pose = lambda pose: cf.extpos.send_extpos( + pose[0], pose[1], pose[2]) + + activate_high_level_commander(cf) + activate_mellinger_controller(cf) + duration = upload_trajectory(cf, trajectory_id, figure8) + print('The sequence is {:.1f} seconds long'.format(duration)) + reset_estimator(cf) + run_sequence(cf, trajectory_id, duration) + +qtm_wrapper.close() From 87a6769ac92fde0082e443d0c2b35993c8274208 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 11 Dec 2019 07:47:27 +0100 Subject: [PATCH 003/861] Closes #131: Update radio-test.py with info about required nrf build flags --- examples/radio-test.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/examples/radio-test.py b/examples/radio-test.py index d4c90e284..8779c4be4 100644 --- a/examples/radio-test.py +++ b/examples/radio-test.py @@ -8,8 +8,11 @@ It finally sets the Crazyflie channel back to default, plots link quality data, and offers good channel suggestion. - Better used when the Crazyflie2-nrf-firmware is compiled with bluetooth - disabled. + 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 ```make BLE=0 CONFIG=-DRSSI_ACK_PACKET```. + See https://github.com/bitcraze/crazyflie-lib-python/issues/131 for more + informations. ''' import argparse From 22b7d57e14b91040d167872b15a40981392170bb Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 11 Dec 2019 07:54:30 +0100 Subject: [PATCH 004/861] Fix flake8 --- examples/radio-test.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/radio-test.py b/examples/radio-test.py index 8779c4be4..9e4d811e3 100644 --- a/examples/radio-test.py +++ b/examples/radio-test.py @@ -9,8 +9,9 @@ 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 ```make BLE=0 CONFIG=-DRSSI_ACK_PACKET```. + ack packet enabled to get RSSI feedback. To configure the Crazyflie in this + mode build the crazyflie2-nrf-firmware with + ```make BLE=0 CONFIG=-DRSSI_ACK_PACKET```. See https://github.com/bitcraze/crazyflie-lib-python/issues/131 for more informations. ''' From 08fa149a96765fa123f21ab9ceabd9e48b9bdf5c Mon Sep 17 00:00:00 2001 From: Tamas Nepusz Date: Tue, 17 Dec 2019 17:16:39 +0100 Subject: [PATCH 005/861] increase write flash request timeout to 2.5s, fixes #98 --- cflib/bootloader/cloader.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 60b178233..cdea76eaf 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -381,7 +381,14 @@ def write_flash(self, addr, page_buffer, target_page, page_count): pk.data = struct.pack(' Date: Tue, 7 Jan 2020 16:28:29 +0100 Subject: [PATCH 006/861] USB: remove calls to USB reset(), fixes #130 --- cflib/drivers/cfusb.py | 3 +-- cflib/drivers/crazyradio.py | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index 07d25d67f..3fd6c72cb 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -121,10 +121,9 @@ def close(self): if (pyusb1 is False): if self.handle: self.handle.releaseInterface() - self.handle.reset() else: if self.dev: - self.dev.reset() + usb.util.dispose_resources(self.dev) self.handle = None self.dev = None diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 6668fa27a..9622f4977 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -169,10 +169,9 @@ def close(self): if (pyusb1 is False): if self.handle: self.handle.releaseInterface() - self.handle.reset() else: if self.dev: - self.dev.reset() + usb.util.dispose_resources(self.dev) self.handle = None self.dev = None From 754280f6de7b6f11ea5dd05eadd9d67c6749fccc Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 18 Jan 2020 11:33:06 +0100 Subject: [PATCH 007/861] #134 Added yaw to take off and landing commands in high level commander --- cflib/crazyflie/high_level_commander.py | 38 +++++++++++++++++++------ 1 file changed, 29 insertions(+), 9 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 68b63cf44..e36b2b2bc 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -7,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2018 Bitcraze AB +# Copyright (C) 2018-2020 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -42,12 +42,12 @@ class HighLevelCommander(): """ COMMAND_SET_GROUP_MASK = 0 - COMMAND_TAKEOFF = 1 - COMMAND_LAND = 2 COMMAND_STOP = 3 COMMAND_GO_TO = 4 COMMAND_START_TRAJECTORY = 5 COMMAND_DEFINE_TRAJECTORY = 6 + COMMAND_TAKEOFF_2 = 7 + COMMAND_LAND_2 = 8 ALL_GROUPS = 0 @@ -70,7 +70,8 @@ def set_group_mask(self, group_mask=ALL_GROUPS): self.COMMAND_SET_GROUP_MASK, group_mask)) - def takeoff(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS): + def takeoff(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS, + yaw=0.0): """ vertical takeoff from current x-y position to given height @@ -78,14 +79,24 @@ def takeoff(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS): :param duration_s: time it should take until target height is reached (s) :param group_mask: mask for which CFs this should apply to + :param yaw: yaw (rad). Use current yaw if set to None. """ - self._send_packet(struct.pack(' Date: Fri, 31 Jan 2020 13:23:41 +0100 Subject: [PATCH 008/861] #136 Add support for sending external pose data to the Crazyflie estimator --- cflib/crazyflie/extpos.py | 10 +++- cflib/crazyflie/localization.py | 30 +++++++++-- examples/qualisys/qualisys_hl_commander.py | 61 ++++++++++++++++------ 3 files changed, 81 insertions(+), 20 deletions(-) diff --git a/cflib/crazyflie/extpos.py b/cflib/crazyflie/extpos.py index 53486dff0..cf4ec4900 100644 --- a/cflib/crazyflie/extpos.py +++ b/cflib/crazyflie/extpos.py @@ -7,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2011-2013 Bitcraze AB +# Copyright (C) 2011-2020 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -50,3 +50,11 @@ def send_extpos(self, x, y, z): """ self._cf.loc.send_extpos([x, y, z]) + + def send_extpose(self, x, y, z, qx, qy, qz, qw): + """ + Send the current Crazyflie X, Y, Z position and attitude as a + normalized quaternion. This is going to be forwarded to the + Crazyflie's position estimator. + """ + self._cf.loc.send_extpose([x, y, z], [qx, qy, qz, qw]) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 7b3bd053d..dabd1be8c 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -7,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2017 Bitcraze AB +# Copyright (C) 2017-2020 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -56,9 +56,15 @@ class Localization(): GENERIC_CH = 1 # Location message types for generig channel - RANGE_STREAM_REPORT = 0x00 - RANGE_STREAM_REPORT_FP16 = 0x01 - LPS_SHORT_LPP_PACKET = 0x02 + RANGE_STREAM_REPORT = 0 + RANGE_STREAM_REPORT_FP16 = 1 + LPS_SHORT_LPP_PACKET = 2 + EMERGENCY_STOP = 3 + EMERGENCY_STOP_WATCHDOG = 4 + COMM_GNSS_NMEA = 6 + COMM_GNSS_PROPRIETARY = 7 + EXT_POSE = 8 + EXT_POSE_PACKED = 9 def __init__(self, crazyflie=None): """ @@ -110,6 +116,22 @@ def send_extpos(self, pos): pk.data = struct.pack(' Date: Tue, 4 Feb 2020 14:15:48 +0100 Subject: [PATCH 009/861] Corrected path to sys tests --- tools/build/sys-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/build/sys-test b/tools/build/sys-test index 924d6abdc..fd61ee99e 100755 --- a/tools/build/sys-test +++ b/tools/build/sys-test @@ -5,7 +5,7 @@ import subprocess try: script_dir = os.path.dirname(os.path.realpath(__file__)) - root = _path.normpath(_path.join(script_dir, '../../sys-test')) + root = _path.normpath(_path.join(script_dir, '../../sys_test')) print('**** Running sys tests ****') subprocess.check_call(['python3', '-m', 'unittest', 'discover', root]) From be3fd3f045cf451660b4014ef162c21550b30f62 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 6 Feb 2020 11:22:02 +0100 Subject: [PATCH 010/861] Bumped version to 0.1.9 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 7a2e8c933..1c986c3bf 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.8', + version='0.1.9', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 81a61537a653ede2646f0b01a3021e3e2e407400 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 20 Feb 2020 15:36:58 +0100 Subject: [PATCH 011/861] Add suport for muliple log configs in SyncLogger. Closes #137 --- cflib/crazyflie/syncLogger.py | 29 ++++++++++++-------- examples/basiclogSync.py | 3 +++ test/crazyflie/test_syncLogger.py | 45 ++++++++++++++++++++++++++++++- 3 files changed, 65 insertions(+), 12 deletions(-) diff --git a/cflib/crazyflie/syncLogger.py b/cflib/crazyflie/syncLogger.py index ccd41de00..a2a0b232d 100644 --- a/cflib/crazyflie/syncLogger.py +++ b/cflib/crazyflie/syncLogger.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2016 Bitcraze AB +# Copyright (C) 2016-2020 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -27,7 +27,7 @@ This class provides synchronous access to log data from the Crazyflie. It acts as an iterator and returns the next value on each iteration. -If no value is available it blocks until log data is available again. +If no value is available it blocks until log data is received again. """ import sys @@ -46,15 +46,18 @@ def __init__(self, crazyflie, log_config): """ Construct an instance of a SyncLogger - Takes an Crazyflie or SyncCrazyflie instance and a log configuration + Takes an Crazyflie or SyncCrazyflie instance and one log configuration + or an array of log configurations """ if isinstance(crazyflie, SyncCrazyflie): self._cf = crazyflie.cf else: self._cf = crazyflie - self._log_config = log_config - self._cf.log.add_config(self._log_config) + if isinstance(log_config, list): + self._log_config = log_config + else: + self._log_config = [log_config] self._queue = Queue() @@ -65,18 +68,22 @@ def connect(self): raise Exception('Already connected') self._cf.disconnected.add_callback(self._disconnected) - self._log_config.data_received_cb.add_callback(self._log_callback) - self._log_config.start() + for config in self._log_config: + self._cf.log.add_config(config) + config.data_received_cb.add_callback(self._log_callback) + config.start() self._is_connected = True def disconnect(self): if self._is_connected: - self._log_config.stop() - self._log_config.delete() + for config in self._log_config: + config.stop() + config.delete() + + config.data_received_cb.remove_callback( + self._log_callback) - self._log_config.data_received_cb.remove_callback( - self._log_callback) self._cf.disconnected.remove_callback(self._disconnected) self._queue.empty() diff --git a/examples/basiclogSync.py b/examples/basiclogSync.py index cf10f01a3..e24765ef6 100644 --- a/examples/basiclogSync.py +++ b/examples/basiclogSync.py @@ -62,6 +62,9 @@ cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(available[0][0], cf=cf) as scf: + # Note: it is possible to add more than one log config using an + # array. + # with SyncLogger(scf, [lg_stab, other_conf]) as logger: with SyncLogger(scf, lg_stab) as logger: endTime = time.time() + 10 diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index 0f5fe8bdc..64d2baeea 100644 --- a/test/crazyflie/test_syncLogger.py +++ b/test/crazyflie/test_syncLogger.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2016 Bitcraze AB +# Copyright (C) 2016-2020 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -36,8 +36,10 @@ if sys.version_info < (3, 3): from mock import MagicMock + from mock import call else: from unittest.mock import MagicMock + from unittest.mock import call class SyncLoggerTest(unittest.TestCase): @@ -52,7 +54,12 @@ def setUp(self): self.log_config_mock = MagicMock(spec=LogConfig) self.log_config_mock.data_received_cb = Caller() + self.log_config_mock2 = MagicMock(spec=LogConfig) + self.log_config_mock2.data_received_cb = Caller() + self.sut = SyncLogger(self.cf_mock, self.log_config_mock) + self.sut_multi = SyncLogger( + self.cf_mock, [self.log_config_mock, self.log_config_mock2]) def test_that_log_configuration_is_added_on_connect(self): # Fixture @@ -63,6 +70,18 @@ def test_that_log_configuration_is_added_on_connect(self): # Assert self.log_mock.add_config.assert_called_once_with(self.log_config_mock) + def test_that_multiple_log_configurations_are_added_on_connect(self): + # Fixture + + # Test + self.sut_multi.connect() + + # Assert + self.log_mock.add_config.assert_has_calls([ + call(self.log_config_mock), + call(self.log_config_mock2) + ]) + def test_that_logging_is_started_on_connect(self): # Fixture @@ -72,6 +91,16 @@ def test_that_logging_is_started_on_connect(self): # Assert self.log_config_mock.start.assert_called_once_with() + def test_that_logging_is_started_on_connect_for_multiple_log_confs(self): + # Fixture + + # Test + self.sut_multi.connect() + + # Assert + self.log_config_mock.start.assert_called_once_with() + self.log_config_mock2.start.assert_called_once_with() + def test_connection_status_after_connect(self): # Fixture self.sut.connect() @@ -105,6 +134,20 @@ def test_that_log_config_is_stopped_on_disconnect(self): self.log_config_mock.stop.assert_called_once_with() self.log_config_mock.delete.assert_called_once_with() + def test_that_multiple_log_configs_are_stopped_on_disconnect(self): + # Fixture + self.sut_multi.connect() + + # Test + self.sut_multi.disconnect() + + # Assert + self.log_config_mock.stop.assert_called_once_with() + self.log_config_mock.delete.assert_called_once_with() + + self.log_config_mock2.stop.assert_called_once_with() + self.log_config_mock2.delete.assert_called_once_with() + def test_that_data_is_received(self): # Fixture self.sut.connect() From 7a5588848cc1669805403d4c61e248444ccce736 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 20 Feb 2020 16:39:28 +0100 Subject: [PATCH 012/861] #138 Make a copy of the callback list before calling them, to avoid problems when one of the callbacks modifies the list by adding/removing callbacks. --- cflib/utils/callbacks.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/utils/callbacks.py b/cflib/utils/callbacks.py index 3102bc630..1bfc52ef4 100644 --- a/cflib/utils/callbacks.py +++ b/cflib/utils/callbacks.py @@ -50,5 +50,6 @@ def remove_callback(self, cb): def call(self, *args): """ Call the callbacks registered with the arguments args """ - for cb in self.callbacks: + copy_of_callbacks = list(self.callbacks) + for cb in copy_of_callbacks: cb(*args) From b5306d6a0e481deeefdb5df423962c2ccfe640a8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 20 Feb 2020 16:54:14 +0100 Subject: [PATCH 013/861] #138 Make sure the SyncLogger is cleaning up correctly --- cflib/crazyflie/syncLogger.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/syncLogger.py b/cflib/crazyflie/syncLogger.py index a2a0b232d..5fc95888d 100644 --- a/cflib/crazyflie/syncLogger.py +++ b/cflib/crazyflie/syncLogger.py @@ -86,8 +86,6 @@ def disconnect(self): self._cf.disconnected.remove_callback(self._disconnected) - self._queue.empty() - self._is_connected = False def is_connected(self): @@ -106,6 +104,7 @@ def __next__(self): data = self._queue.get() if data == self.DISCONNECT_EVENT: + self._queue.empty() raise StopIteration return data @@ -116,10 +115,11 @@ def __enter__(self): def __exit__(self, exc_type, exc_val, exc_tb): self.disconnect() + self._queue.empty() def _log_callback(self, ts, data, logblock): self._queue.put((ts, data, logblock)) def _disconnected(self, link_uri): - self._queue.put(self.DISCONNECT_EVENT) self.disconnect() + self._queue.put(self.DISCONNECT_EVENT) From d461330d7368af884e2851081965604479b868b6 Mon Sep 17 00:00:00 2001 From: Marlene Boehmer Date: Mon, 24 Feb 2020 15:34:29 +0100 Subject: [PATCH 014/861] uart functionality --- cflib/crtp/serialdriver.py | 222 ++++++++++++++++++++++++++++++++++++- 1 file changed, 218 insertions(+), 4 deletions(-) diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index 34c54036f..37689f32d 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -28,19 +28,57 @@ An early serial link driver. This could still be used (after some fixing) to run high-speed CRTP with the Crazyflie. The UART can be run at 2Mbit. """ +import logging import re +import sys +import threading -from .crtpdriver import CRTPDriver +from .crtpstack import CRTPPacket from .exceptions import WrongUriType +from cflib.crtp.crtpdriver import CRTPDriver + +if sys.version_info < (3,): + import Queue as queue +else: + import queue + +found_serial = True +try: + import serial +except ImportError: + found_serial = False __author__ = 'Bitcraze AB' __all__ = ['SerialDriver'] +logger = logging.getLogger(__name__) + +MTU = 32 +START_BYTE1 = 0xbc +START_BYTE2 = 0xcf +SYSLINK_RADIO_RAW = 0x00 + + +def compute_cksum(list): + cksum0, cksum1 = 0, 0 + for i in range(len(list)): + cksum0 = (cksum0 + list[i]) & 0xff + cksum1 = (cksum1 + cksum0) & 0xff + return bytearray([cksum0, cksum1]) + class SerialDriver(CRTPDriver): def __init__(self): - None + CRTPDriver.__init__(self) + self.ser = None + self.uri = '' + self.link_error_callback = None + self.in_queue = None + self.out_queue = None + self._receive_thread = None + self._send_thread = None + logger.info('Initialized serial driver.') def connect(self, uri, linkQualityCallback, linkErrorCallback): # check if the URI is a serial URI @@ -48,12 +86,188 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): raise WrongUriType('Not a serial URI') # Check if it is a valid serial URI - uriRe = re.search('^serial://([a-z A-Z 0-9]+)/?([0-9]+)?$', uri) + uriRe = re.search('^serial://pi$', uri) if not uriRe: raise Exception('Invalid serial URI') + if not found_serial: + raise Exception('PySerial package is missing') + + self.uri = uri + + self.link_error_callback = linkErrorCallback + + # Prepare the inter-thread communication queue + self.in_queue = queue.Queue() + self.out_queue = queue.Queue(1) + + self.ser = serial.Serial('/dev/ttyAMA0', 512000, timeout=1) + + # Launch the comm thread + self._receive_thread = _SerialReceiveThread( + self.ser, self.in_queue, linkQualityCallback, linkErrorCallback) + self._receive_thread.start() + self._send_thread = _SerialSendThread( + self.ser, self.out_queue, linkQualityCallback, linkErrorCallback) + self._send_thread.start() + + def send_packet(self, pk): + try: + self.out_queue.put(pk, True, 2) + except queue.Full: + if self.link_error_callback: + self.link_error_callback( + 'RadioDriver: Could not send packet to copter') + + def receive_packet(self, wait=0): + try: + if wait == 0: + pk = self.in_queue.get(False) + elif wait < 0: + pk = self.in_queue.get(True) + else: + pk = self.in_queue.get(True, wait) + except queue.Empty: + return None + return pk + + def get_status(self): + return 'No information available' + def get_name(self): return 'serial' def scan_interface(self, address): - return [] + if found_serial: + return [['serial://pi', ''], ] + else: + return [] + + def close(self): + self._receive_thread.stop() + self._send_thread.stop() + try: + self._receive_thread.join() + self._send_thread.join() + except Exception: + pass + self.ser.close() + + +class _SerialReceiveThread(threading.Thread): + + def __init__(self, ser, inQueue, link_quality_callback, + link_error_callback): + """ Create the object """ + threading.Thread.__init__(self) + self.ser = ser + self.in_queue = inQueue + self._stop = False + self.link_error_callback = link_error_callback + + def stop(self): + """ Stop the thread """ + self._stop = True + + def run(self): + """ Run the receiver thread """ + READ_END = bytes([START_BYTE1, START_BYTE2]) + received = bytearray(MTU + 4) + received_header = memoryview(received)[0:2] + while not self._stop: + try: + r = self.ser.read_until(READ_END)[-2:] + if len(r) != 2: + continue + + if r[0] != START_BYTE1 or r[1] != START_BYTE2: + continue + + r = self.ser.readinto(received_header) + if r != 2: + continue + + if not (0 < received_header[1] <= MTU): + continue + + expected = received_header[1] + 2 + + received_data_chk = memoryview(received)[2:2+expected] + r = self.ser.readinto(received_data_chk) + if r != expected: + continue + + # NOTE: end is (expected - 2) as the lenght of the data +2 for + # the header bytes + cksum = compute_cksum(memoryview(received)[:expected]) + if cksum[0] != received_data_chk[-2] or \ + cksum[1] != received_data_chk[-1]: + continue + + pk = CRTPPacket(received[2], received[3:expected]) + self.in_queue.put(pk) + + except Exception as e: + import traceback + if self.link_error_callback: + self.link_error_callback( + 'Error communicating with the Crazyflie!\n' + 'Exception:%s\n\n%s' % (e, traceback.format_exc())) + + +class _SerialSendThread(threading.Thread): + + def __init__(self, ser, outQueue, link_quality_callback, + link_error_callback): + """ Create the object """ + threading.Thread.__init__(self) + self.ser = ser + self.out_queue = outQueue + self._stop = False + self.link_error_callback = link_error_callback + + def stop(self): + """ Stop the thread """ + self._stop = True + + def run(self): + """ Run the sender thread """ + out_data = bytearray(MTU + 6) + out_data[0:3] = bytearray( + [START_BYTE1, START_BYTE2, SYSLINK_RADIO_RAW]) + + empty_packet = CRTPPacket(header=0xFF) + empty_packet_data_length = 0 + empty_packet_data = bytearray(7) + empty_packet_data[0:5] = bytearray( + [START_BYTE1, START_BYTE2, SYSLINK_RADIO_RAW, 0x01, + empty_packet.header]) + empty_packet_data[5:7] = compute_cksum(empty_packet_data[2:5]) + + while not self._stop: + try: + pk = self.out_queue.get(True, timeout=0.0003) + data = pk.data + len_data = len(data) + end_of_payload = 5 + len_data + + out_data[3] = len_data + 1 + out_data[4] = pk.header + out_data[5:end_of_payload] = data + out_data[end_of_payload:end_of_payload + + 2] = compute_cksum(out_data[2:end_of_payload]) + + written = self.ser.write(out_data[0:end_of_payload + 2]) + + except queue.Empty: + pk = empty_packet + len_data = empty_packet_data_length + written = self.ser.write(empty_packet_data) + + if written != len_data + 7: + if self.link_error_callback: + self.link_error_callback( + 'SerialDriver: Could only send {:d}B bytes of {:d}B ' + 'packet to Crazyflie'.format( + written, len_data + 7) + ) From 0ddc07604b5f2fbeeb2ff1d8e829bfe15c89d9c2 Mon Sep 17 00:00:00 2001 From: Marlene Boehmer Date: Wed, 26 Feb 2020 00:50:05 +0100 Subject: [PATCH 015/861] prrt functionality --- cflib/crazyflie/__init__.py | 5 +- cflib/crtp/__init__.py | 4 +- cflib/crtp/prrtdriver.py | 104 ++++++++++++++++++++++++++++++++++++ 3 files changed, 111 insertions(+), 2 deletions(-) create mode 100644 cflib/crtp/prrtdriver.py diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 66de9e1a6..b504735b4 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -106,7 +106,8 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.incoming = _IncomingPacketHandler(self) self.incoming.setDaemon(True) - self.incoming.start() + if self.link: + self.incoming.start() self.commander = Commander(self) self.high_level_commander = HighLevelCommander(self) @@ -228,6 +229,8 @@ def open_link(self, link_uri): logger.warning(message) self.connection_failed.call(link_uri, message) else: + if not self.incoming.isAlive(): + self.incoming.start() # Add a callback so we can check that any data is coming # back from the copter self.packet_received.add_callback( diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 9f7ce32e7..33758bde5 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -29,6 +29,7 @@ from .debugdriver import DebugDriver from .exceptions import WrongUriType +from .prrtdriver import PrrtDriver from .radiodriver import RadioDriver from .serialdriver import SerialDriver from .udpdriver import UdpDriver @@ -40,7 +41,8 @@ logger = logging.getLogger(__name__) -DRIVERS = [RadioDriver, SerialDriver, UdpDriver, DebugDriver, UsbDriver] +DRIVERS = [RadioDriver, SerialDriver, UdpDriver, + DebugDriver, UsbDriver, PrrtDriver] CLASSES = [] diff --git a/cflib/crtp/prrtdriver.py b/cflib/crtp/prrtdriver.py new file mode 100644 index 000000000..81e4b9293 --- /dev/null +++ b/cflib/crtp/prrtdriver.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +import datetime +import logging +import re + +from .crtpstack import CRTPPacket +from .exceptions import WrongUriType +from cflib.crtp.crtpdriver import CRTPDriver + +prrt_installed = True +try: + import prrt +except ImportError: + prrt_installed = False + +__author__ = 'Bitcraze AB' +__all__ = ['PrrtDriver'] + +logger = logging.getLogger(__name__) + +MAX_PAYLOAD = 32 +DEFAULT_TARGET_DELAY = 0.05 # unit: s +PRRT_LOCAL_PORT = 5000 + + +class PrrtDriver(CRTPDriver): + + def __init__(self): + CRTPDriver.__init__(self) + self.prrt_socket = None + self.uri = '' + self.link_error_callback = None + self.packet_log = None + self.log_index = 0 + logger.info('Initialized PRRT driver.') + + def connect(self, uri, linkQualityCallback, linkErrorCallback): + # check if the URI is a PRRT URI + if not re.search('^prrt://', uri): + raise WrongUriType('Not a prrt URI') + + # Check if it is a valid PRRT URI + uri_match = re.search(r'^prrt://((?:[\d]{1,3})\.(?:[\d]{1,3})\.' + r'(?:[\d]{1,3})\.(?:[\d]{1,3})):([\d]{1,5})' + r'(?:/([\d]{1,6}))?$', uri) + if not uri_match: + raise Exception('Invalid PRRT URI') + + if not prrt_installed: + raise Exception('PRRT is missing') + + self.uri = uri + + self.link_error_callback = linkErrorCallback + + address = uri_match.group(1) + port = int(uri_match.group(2)) + target_delay_s = DEFAULT_TARGET_DELAY + if uri_match.group(3): + target_delay_s = int(uri_match.group(3)) / 1000 + + self.prrt_socket = prrt.PrrtSocket(('0.0.0.0', PRRT_LOCAL_PORT), + maximum_payload_size=MAX_PAYLOAD, + target_delay=target_delay_s) + self.prrt_socket.connect((address, port)) + + def send_packet(self, pk): + pk_bytes = bytearray([pk.get_header()]) + pk.data + self.prrt_socket.send(pk_bytes) + + def receive_packet(self, wait=0): + try: + if wait == 0: + pk_bytes, _ = self.prrt_socket.receive_asap() + elif wait < 0: + pk_bytes, _ = self.prrt_socket.receive_asap_wait() + else: + deadline = datetime.datetime.utcnow() + datetime.timedelta( + seconds=wait) + pk_bytes, _ = self.prrt_socket.receive_asap_timedwait(deadline) + except prrt.TimeoutException: + return None + + if len(pk_bytes) <= 0: + return None + + pk = CRTPPacket(pk_bytes[0], pk_bytes[1:]) + return pk + + def get_status(self): + return 'No information available' + + def get_name(self): + return 'prrt' + + def scan_interface(self, address): + default_uri = 'prrt://10.8.0.208:5000' + if prrt_installed: + return [[default_uri, ''], ] + return [] + + def close(self): + return From 0696c087fa9cef61ae3a4372a7dd6bbe015810c8 Mon Sep 17 00:00:00 2001 From: Marlene Boehmer Date: Wed, 4 Mar 2020 16:47:14 +0100 Subject: [PATCH 016/861] Specify device in serial Uri --- cflib/crtp/serialdriver.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index 37689f32d..2d7f36fce 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -45,6 +45,7 @@ found_serial = True try: import serial + import serial.tools.list_ports as list_ports except ImportError: found_serial = False @@ -86,10 +87,16 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): raise WrongUriType('Not a serial URI') # Check if it is a valid serial URI - uriRe = re.search('^serial://pi$', uri) - if not uriRe: + uri_data = re.search('^serial://([a-zA-Z0-9]+)$', uri) + if not uri_data: raise Exception('Invalid serial URI') + devices = [x.device for x in list_ports.comports() + if x.name == uri_data.group(1)] + if not len(devices) == 1: + raise Exception('Could not identify device') + device = devices[0] + if not found_serial: raise Exception('PySerial package is missing') @@ -101,7 +108,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): self.in_queue = queue.Queue() self.out_queue = queue.Queue(1) - self.ser = serial.Serial('/dev/ttyAMA0', 512000, timeout=1) + self.ser = serial.Serial(device, 512000, timeout=1) # Launch the comm thread self._receive_thread = _SerialReceiveThread( @@ -139,7 +146,8 @@ def get_name(self): def scan_interface(self, address): if found_serial: - return [['serial://pi', ''], ] + devices_names = [x.name for x in list_ports.comports()] + return [('serial://' + x, '') for x in devices_names] else: return [] From e7376f7bff60153e7fea340f4ff019f9c12ba9e0 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 13 Mar 2020 09:25:44 +0100 Subject: [PATCH 017/861] Update version to 0.1.10 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 1c986c3bf..ee6f22f12 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.9', + version='0.1.10', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From bcef1b914df7164da6f5a8ddec289c84cf45f95d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 19 Mar 2020 14:21:50 +0100 Subject: [PATCH 018/861] #143 Updated urls to new url structure --- docs/python_api.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/python_api.md b/docs/python_api.md index 0c9e3c4a3..e81e1b529 100644 --- a/docs/python_api.md +++ b/docs/python_api.md @@ -1,6 +1,6 @@ --- title: The Crazyflie Python API -page_id: python_api +page_id: python_api --- In order to easily use and control the Crazyflie there\'s an library @@ -11,8 +11,8 @@ the API that it implements. If you are interested in more details look in the PyDoc in the code or: - Communication protocol for - [logging](https://www.bitcraze.io/docs/crazyflie-firmware/master/ctrp_log/) or - [parameters](https://www.bitcraze.io/docs/crazyflie-firmware/master/ctrp_parameters/) + [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/ctrp_log/) or + [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/ctrp_parameters/) Structure of the library ======================== @@ -403,7 +403,7 @@ The logging cannot be started until your are connected to a Crazyflie: def data_received_callback(timestamp, data, logconf): print "[%d][%s]: %s" % (timestamp, logconf.name, data) - + def logging_error(logconf, msg): print "Error when logging %s" % logconf.name ``` From 777ce18a5832f32fc2ad36f6b6143a920710c856 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 19 Mar 2020 15:37:27 +0100 Subject: [PATCH 019/861] #143 More link updates --- docs/python_api.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/python_api.md b/docs/python_api.md index e81e1b529..b22c9025e 100644 --- a/docs/python_api.md +++ b/docs/python_api.md @@ -11,8 +11,8 @@ the API that it implements. If you are interested in more details look in the PyDoc in the code or: - Communication protocol for - [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/ctrp_log/) or - [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/ctrp_parameters/) + [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/ctrp_log/) or + [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/ctrp_parameters/) Structure of the library ======================== From 6530e41c3cf6fca67dbd40cf25cf16ecce29165c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 19 Mar 2020 15:58:03 +0100 Subject: [PATCH 020/861] #143 More link updates --- docs/python_api.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/python_api.md b/docs/python_api.md index b22c9025e..85beed309 100644 --- a/docs/python_api.md +++ b/docs/python_api.md @@ -11,8 +11,8 @@ the API that it implements. If you are interested in more details look in the PyDoc in the code or: - Communication protocol for - [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/ctrp_log/) or - [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/ctrp_parameters/) + [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/ctrp_log/) or + [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/ctrp_parameters/) Structure of the library ======================== From a195a902278b02ea1eda70d628e50edbcd84c3bc Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 22 Mar 2020 07:58:19 +0100 Subject: [PATCH 021/861] #143 Converted documentation to tree structure --- docs/_data/menu.yml | 4 ++-- docs/{ => development}/eeprom.md | 0 docs/{ => development}/matlab.md | 0 docs/{ => development}/testing.md | 0 docs/{ => functonal-areas}/crazyradio_lib.md | 0 docs/{ => installation}/install_from_source.md | 0 docs/{ => installation}/usb_permissions.md | 0 docs/{ => user-guides}/python_api.md | 0 8 files changed, 2 insertions(+), 2 deletions(-) rename docs/{ => development}/eeprom.md (100%) rename docs/{ => development}/matlab.md (100%) rename docs/{ => development}/testing.md (100%) rename docs/{ => functonal-areas}/crazyradio_lib.md (100%) rename docs/{ => installation}/install_from_source.md (100%) rename docs/{ => installation}/usb_permissions.md (100%) rename docs/{ => user-guides}/python_api.md (100%) diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index 28e1aff22..8cc9f204e 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -1,12 +1,12 @@ - page_id: home -- title: Install Instructions +- title: Installation subs: - {page_id: install_from_source} - {page_id: usd_permissions} - title: User guides subs: - {page_id: python_api} -- title: Explanation Functionalities +- title: Functional areas subs: - {page_id: crazyradio_lib} - title: Development diff --git a/docs/eeprom.md b/docs/development/eeprom.md similarity index 100% rename from docs/eeprom.md rename to docs/development/eeprom.md diff --git a/docs/matlab.md b/docs/development/matlab.md similarity index 100% rename from docs/matlab.md rename to docs/development/matlab.md diff --git a/docs/testing.md b/docs/development/testing.md similarity index 100% rename from docs/testing.md rename to docs/development/testing.md diff --git a/docs/crazyradio_lib.md b/docs/functonal-areas/crazyradio_lib.md similarity index 100% rename from docs/crazyradio_lib.md rename to docs/functonal-areas/crazyradio_lib.md diff --git a/docs/install_from_source.md b/docs/installation/install_from_source.md similarity index 100% rename from docs/install_from_source.md rename to docs/installation/install_from_source.md diff --git a/docs/usb_permissions.md b/docs/installation/usb_permissions.md similarity index 100% rename from docs/usb_permissions.md rename to docs/installation/usb_permissions.md diff --git a/docs/python_api.md b/docs/user-guides/python_api.md similarity index 100% rename from docs/python_api.md rename to docs/user-guides/python_api.md From 21d0e4a0f3411c2631c78537a3f19e019cba013a Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 27 Mar 2020 11:55:57 +0100 Subject: [PATCH 022/861] #144 Use device for serial port identifier in URI if name is empty. --- cflib/crtp/serialdriver.py | 28 ++++++++++++++++++++-------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index 2d7f36fce..bb2f608de 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -87,19 +87,19 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): raise WrongUriType('Not a serial URI') # Check if it is a valid serial URI - uri_data = re.search('^serial://([a-zA-Z0-9]+)$', uri) + uri_data = re.search('^serial://([-a-zA-Z0-9/.]+)$', uri) if not uri_data: raise Exception('Invalid serial URI') - devices = [x.device for x in list_ports.comports() - if x.name == uri_data.group(1)] - if not len(devices) == 1: - raise Exception('Could not identify device') - device = devices[0] - if not found_serial: raise Exception('PySerial package is missing') + device_name = uri_data.group(1) + devices = self.get_devices() + if device_name not in devices: + raise Exception('Could not identify device') + device = devices[device_name] + self.uri = uri self.link_error_callback = linkErrorCallback @@ -146,7 +146,7 @@ def get_name(self): def scan_interface(self, address): if found_serial: - devices_names = [x.name for x in list_ports.comports()] + devices_names = self.get_devices().keys() return [('serial://' + x, '') for x in devices_names] else: return [] @@ -161,6 +161,18 @@ def close(self): pass self.ser.close() + def get_devices(self): + result = {} + for port in list_ports.comports(): + name = port.name + # Name is not populated on all systems, fall back on the device + if not name: + name = port.device + + result[name] = port.device + + return result + class _SerialReceiveThread(threading.Thread): From 58bb5159d9e71f9c19914e7aae44a015ac025f9d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 27 Mar 2020 11:56:51 +0100 Subject: [PATCH 023/861] #144 Added parameter enable_serial_driver to init_drivers(). Use to enable serial support, off by default. --- cflib/crtp/__init__.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 33758bde5..0e509c365 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -46,11 +46,17 @@ CLASSES = [] -def init_drivers(enable_debug_driver=False): +def init_drivers(enable_debug_driver=False, enable_serial_driver=False): """Initialize all the drivers.""" for driver in DRIVERS: try: - if driver != DebugDriver or enable_debug_driver: + enable = True + if driver == DebugDriver: + enable = enable_debug_driver + elif driver == SerialDriver: + enable = enable_serial_driver + + if enable: CLASSES.append(driver) except Exception: # pylint: disable=W0703 continue From 9c002bd85fbbadd8c9c7a530f92ce8e8e84e9fbf Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 30 Mar 2020 17:18:10 +0200 Subject: [PATCH 024/861] Add script for PID tuning #146 --- examples/tuning/PID_controller_tuner.py | 298 ++++++++++++++++++++++++ 1 file changed, 298 insertions(+) create mode 100644 examples/tuning/PID_controller_tuner.py diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py new file mode 100644 index 000000000..a2388073a --- /dev/null +++ b/examples/tuning/PID_controller_tuner.py @@ -0,0 +1,298 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2011-2013 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +""" +Gui une the PID controller of the crazyflie + +""" + +import tkinter as tk +import logging +import sys +import time +import matplotlib.pyplot as plt +import numpy as np +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger +from cflib.positioning.position_hl_commander import PositionHlCommander + +URI = 'radio://0/80/2M/E7E7E7E7E7' +STANDARD_HEIGHT = 1.2 +STEP_RESPONSE_TIME = 3.0 + +if len(sys.argv) > 1: + URI = sys.argv[1] +elif len(sys.argv) > 2: + STANDARD_HEIGHT = sys.argv[2] + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + + + +class TunerGUI: + def __init__(self, master): + self.master = master + self.master.title("PID tuner Crazyflie") + + self.figplot = plt.Figure(figsize=(5,4), dpi=100) + self.ax2 = self.figplot.add_subplot(111) + self.line2 = FigureCanvasTkAgg(self.figplot, self.master) + self.line2.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=1) + + self.scale_Kp = tk.Scale(master, label='scale_Kp', from_=0, to=20, length=600,tickinterval=2, resolution=0.1, orient=tk.HORIZONTAL) + self.scale_Ki = tk.Scale(master, label='scale_Ki', from_=0, to=10, length=600,tickinterval=1, resolution=0.1, orient=tk.HORIZONTAL) + self.scale_Kd = tk.Scale(master, label='scale_Kd', from_=0, to=10, length=600,tickinterval=1, resolution=0.1, orient=tk.HORIZONTAL) + + self.scale_Kp.pack() + self.scale_Ki.pack() + self.scale_Kd.pack() + + self.pos_array_prev = [] + self.sp_array_prev = [] + self.time_array_prev =[] + + def draw_plot(self,time_array,pos_array,sp_array): + + self.ax2.clear() + self.ax2.plot(self.time_array_prev, self.pos_array_prev, label='pos_z', color='red', alpha=0.5) + + self.ax2.plot(time_array, pos_array, label='pos',color='red') + self.ax2.plot(time_array, sp_array, label='sp',color='blue') + self.line2.draw() + self.pos_array_prev= pos_array + self.sp_array_prev= sp_array + self.time_array_prev= time_array + + def clear_plot(self): + self.ax2.clear() + self.line2.draw() + self.time_array_prev=[] + self.pos_array_prev=[] + self.sp_array_prev=[] + + + +class TunerControlCF: + def __init__(self, pid_gui, scf): + self.cf = scf + self.pid_gui = pid_gui + + self.label = tk.Label(self.pid_gui.master, text="Choose an axis!") + self.label.pack() + variable = tk.StringVar(self.pid_gui.master) + variable.set("z") + self.dropdown = tk.OptionMenu(self.pid_gui.master, variable, "x", "y", "z",command=self.change_param_callback) + self.dropdown.pack() + self.axis_choice = "z" + + self.button_send =tk.Button(self.pid_gui.master, text='SEND PID GAINS', command=self.send_pid_gains).pack() + self.button_step=tk.Button(self.pid_gui.master, text='DO STEP', command=self.do_step).pack() + self.button_quit=tk.Button(self.pid_gui.master, text='STOP', command=self.stop_gui).pack() + + self.cf.param.add_update_callback(group="posCtlPid", name="zKp", cb=self.param_updated_callback_Kp) + self.cf.param.add_update_callback(group="posCtlPid", name="zKi", cb=self.param_updated_callback_Ki) + self.cf.param.add_update_callback(group="posCtlPid", name="zKd", cb=self.param_updated_callback_Kd) + + self.current_value_kp = 0 + self.current_value_kd = 0 + self.current_value_ki = 0 + + self.cf.param.request_param_update("posCtlPid.zKp") + self.cf.param.request_param_update("posCtlPid.zKi") + self.cf.param.request_param_update("posCtlPid.zKd") + + time.sleep(0.1) + + self.update_scale_info() + + self.commander = cf.high_level_commander + self.cf.param.set_value('commander.enHighLevel', '1') + self.take_off(STANDARD_HEIGHT) + + + + def update_scale_info(self): + print('update info') + self.pid_gui.scale_Kp.set(self.current_value_kp) + self.pid_gui.scale_Kd.set(self.current_value_kd) + self.pid_gui.scale_Ki.set(self.current_value_ki) + + # Buttons + def send_pid_gains(self): + print("Sending to the " + self.axis_choice + "position PID controller: Kp: " + + str(self.pid_gui.scale_Kp.get()) + ", Ki: " + str(self.pid_gui.scale_Ki.get()) + ", Kd: "+str(self.pid_gui.scale_Ki.get())) + cf.param.set_value("posCtlPid."+self.axis_choice +"Kp", self.pid_gui.scale_Kp.get()) + cf.param.set_value("posCtlPid."+self.axis_choice +"Ki", self.pid_gui.scale_Ki.get()) + cf.param.set_value("posCtlPid."+self.axis_choice +"Kd", self.pid_gui.scale_Kd.get()) + + time.sleep(0.1) + + self.update_scale_info() + + def do_step(self): + print('do step') + log_config = LogConfig(name='Position setpoint', period_in_ms=10) + log_config.add_variable("stateEstimate." + self.axis_choice, 'float') + log_config.add_variable("ctrltarget." + self.axis_choice, 'float') + + if self.axis_choice is "z": + self.commander.go_to(0,0,0.2,0,0.3,relative=True) + elif self.axis_choice is "x": + self.commander.go_to(0.2,0,0,0,0.3,relative=True) + elif self.axis_choice is "y": + self.commander.go_to(0,0.2,0,0,0.3,relative=True) + else: + print('WRONG CHOICE?!?!') + self.stop_gui() + + current_time=time.time() + + pos_history = [] + sp_history = [] + time_history = [] + with SyncLogger(self.cf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + pos_history.append(data["stateEstimate." + self.axis_choice]) + sp_history.append(data["ctrltarget." + self.axis_choice]) + time_history.append(time.time()- current_time) + + if ((time.time() - current_time) > STEP_RESPONSE_TIME): + break + #print(pos_history) + #print(sp_history) + self.pid_gui.draw_plot(time_history,pos_history,sp_history) + if self.axis_choice is "z": + self.commander.go_to(0,0,-0.2,0,0.3,relative=True) + elif self.axis_choice is "x": + self.commander.go_to(-0.2,0,0,0,0.3,relative=True) + elif self.axis_choice is "y": + self.commander.go_to(0,-0.2,0,0,0.3,relative=True) + else: + print('WRONG CHOICE?!?!') + self.stop_gui() + + def stop_gui(self): + self.pid_gui.master.quit() + self.land_and_stop() + + #parameter update + def change_param_callback(self,value): + # + print(value) + self.cf.param.remove_update_callback("posCtlPid",name=self.axis_choice +"Kp") + self.cf.param.remove_update_callback("posCtlPid",name=self.axis_choice +"Ki") + self.cf.param.remove_update_callback("posCtlPid",name=self.axis_choice +"Kd") + + time.sleep(0.1) + self.cf.param.add_update_callback(group="posCtlPid", name=value +"Kp", cb=self.param_updated_callback_Kp) + self.cf.param.add_update_callback(group="posCtlPid", name=value +"Ki", cb=self.param_updated_callback_Ki) + self.cf.param.add_update_callback(group="posCtlPid", name=value +"Kd", cb=self.param_updated_callback_Kd) + + self.cf.param.request_param_update("posCtlPid."+value+"Kp") + self.cf.param.request_param_update("posCtlPid."+value+"Ki") + self.cf.param.request_param_update("posCtlPid."+value+"Kd") + time.sleep(0.1) + + self.update_scale_info() + self.pid_gui.clear_plot() + self.axis_choice=value + + def param_updated_callback_Kp(self, name, value): + self.current_value_kp = float(value) + def param_updated_callback_Ki(self, name, value): + self.current_value_ki = float(value) + def param_updated_callback_Kd(self, name, value): + self.current_value_kd = float(value) + + def take_off(self, height): + self.commander.takeoff(height, 1.0) + def land_and_stop(self): + self.commander.land(0.0, 2.0) + time.sleep(2) + self.commander.stop() + + +def wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +if __name__ == '__main__': + + root = tk.Tk() + pid_gui= TunerGUI(root) + + cflib.crtp.init_drivers(enable_debug_driver=False) + cf=Crazyflie(rw_cache='./cache') + with SyncCrazyflie(URI, cf) as scf: + wait_for_position_estimator(scf) + cf_ctrl= TunerControlCF(pid_gui,cf) + tk.mainloop() From d77c1d7179046f51aba19828bc9c0dcb45570fc0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 31 Mar 2020 14:50:39 +0200 Subject: [PATCH 025/861] Fixed styling issue #146 --- examples/tuning/PID_controller_tuner.py | 201 ++++++++++++++---------- 1 file changed, 115 insertions(+), 86 deletions(-) diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index a2388073a..d0e5ab46d 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -28,22 +28,19 @@ Gui une the PID controller of the crazyflie """ - -import tkinter as tk import logging import sys import time + import matplotlib.pyplot as plt -import numpy as np +import tkinter as tk from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg import cflib.crtp from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.positioning.motion_commander import MotionCommander from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger -from cflib.positioning.position_hl_commander import PositionHlCommander URI = 'radio://0/80/2M/E7E7E7E7E7' STANDARD_HEIGHT = 1.2 @@ -58,20 +55,25 @@ logging.basicConfig(level=logging.ERROR) - class TunerGUI: def __init__(self, master): self.master = master - self.master.title("PID tuner Crazyflie") + self.master.title('PID tuner Crazyflie') - self.figplot = plt.Figure(figsize=(5,4), dpi=100) + self.figplot = plt.Figure(figsize=(5, 4), dpi=100) self.ax2 = self.figplot.add_subplot(111) self.line2 = FigureCanvasTkAgg(self.figplot, self.master) self.line2.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=1) - self.scale_Kp = tk.Scale(master, label='scale_Kp', from_=0, to=20, length=600,tickinterval=2, resolution=0.1, orient=tk.HORIZONTAL) - self.scale_Ki = tk.Scale(master, label='scale_Ki', from_=0, to=10, length=600,tickinterval=1, resolution=0.1, orient=tk.HORIZONTAL) - self.scale_Kd = tk.Scale(master, label='scale_Kd', from_=0, to=10, length=600,tickinterval=1, resolution=0.1, orient=tk.HORIZONTAL) + self.scale_Kp = tk.Scale(master, label='scale_Kp', from_=0, to=20, + length=600, tickinterval=2, resolution=0.1, + orient=tk.HORIZONTAL) + self.scale_Ki = tk.Scale(master, label='scale_Ki', from_=0, to=10, + length=600, tickinterval=1, resolution=0.1, + orient=tk.HORIZONTAL) + self.scale_Kd = tk.Scale(master, label='scale_Kd', from_=0, to=10, + length=600, tickinterval=1, resolution=0.1, + orient=tk.HORIZONTAL) self.scale_Kp.pack() self.scale_Ki.pack() @@ -79,27 +81,27 @@ def __init__(self, master): self.pos_array_prev = [] self.sp_array_prev = [] - self.time_array_prev =[] + self.time_array_prev = [] - def draw_plot(self,time_array,pos_array,sp_array): + def draw_plot(self, time_array, pos_array, sp_array): self.ax2.clear() - self.ax2.plot(self.time_array_prev, self.pos_array_prev, label='pos_z', color='red', alpha=0.5) + self.ax2.plot(self.time_array_prev, self.pos_array_prev, + label='pos_z', color='red', alpha=0.5) - self.ax2.plot(time_array, pos_array, label='pos',color='red') - self.ax2.plot(time_array, sp_array, label='sp',color='blue') - self.line2.draw() - self.pos_array_prev= pos_array - self.sp_array_prev= sp_array - self.time_array_prev= time_array + self.ax2.plot(time_array, pos_array, label='pos', color='red') + self.ax2.plot(time_array, sp_array, label='sp', color='blue') + self.line2.draw() + self.pos_array_prev = pos_array + self.sp_array_prev = sp_array + self.time_array_prev = time_array def clear_plot(self): self.ax2.clear() self.line2.draw() - self.time_array_prev=[] - self.pos_array_prev=[] - self.sp_array_prev=[] - + self.time_array_prev = [] + self.pos_array_prev = [] + self.sp_array_prev = [] class TunerControlCF: @@ -107,29 +109,40 @@ def __init__(self, pid_gui, scf): self.cf = scf self.pid_gui = pid_gui - self.label = tk.Label(self.pid_gui.master, text="Choose an axis!") + self.label = tk.Label(self.pid_gui.master, text='Choose an axis!') self.label.pack() variable = tk.StringVar(self.pid_gui.master) - variable.set("z") - self.dropdown = tk.OptionMenu(self.pid_gui.master, variable, "x", "y", "z",command=self.change_param_callback) + variable.set('z') + self.dropdown = tk.OptionMenu( + self.pid_gui.master, variable, 'x', 'y', 'z', + command=self.change_param_callback) self.dropdown.pack() - self.axis_choice = "z" - - self.button_send =tk.Button(self.pid_gui.master, text='SEND PID GAINS', command=self.send_pid_gains).pack() - self.button_step=tk.Button(self.pid_gui.master, text='DO STEP', command=self.do_step).pack() - self.button_quit=tk.Button(self.pid_gui.master, text='STOP', command=self.stop_gui).pack() - - self.cf.param.add_update_callback(group="posCtlPid", name="zKp", cb=self.param_updated_callback_Kp) - self.cf.param.add_update_callback(group="posCtlPid", name="zKi", cb=self.param_updated_callback_Ki) - self.cf.param.add_update_callback(group="posCtlPid", name="zKd", cb=self.param_updated_callback_Kd) + self.axis_choice = 'z' + + self.button_send = tk.Button( + self.pid_gui.master, text='SEND PID GAINS', + command=self.send_pid_gains).pack() + self.button_step = tk.Button( + self.pid_gui.master, text='DO STEP', + command=self.do_step).pack() + self.button_quit = tk.Button( + self.pid_gui.master, text='STOP', + command=self.stop_gui).pack() + + self.cf.param.add_update_callback( + group='posCtlPid', name='zKp', cb=self.param_updated_callback_Kp) + self.cf.param.add_update_callback( + group='posCtlPid', name='zKi', cb=self.param_updated_callback_Ki) + self.cf.param.add_update_callback( + group='posCtlPid', name='zKd', cb=self.param_updated_callback_Kd) self.current_value_kp = 0 self.current_value_kd = 0 self.current_value_ki = 0 - self.cf.param.request_param_update("posCtlPid.zKp") - self.cf.param.request_param_update("posCtlPid.zKi") - self.cf.param.request_param_update("posCtlPid.zKd") + self.cf.param.request_param_update('posCtlPid.zKp') + self.cf.param.request_param_update('posCtlPid.zKi') + self.cf.param.request_param_update('posCtlPid.zKd') time.sleep(0.1) @@ -139,8 +152,6 @@ def __init__(self, pid_gui, scf): self.cf.param.set_value('commander.enHighLevel', '1') self.take_off(STANDARD_HEIGHT) - - def update_scale_info(self): print('update info') self.pid_gui.scale_Kp.set(self.current_value_kp) @@ -149,11 +160,17 @@ def update_scale_info(self): # Buttons def send_pid_gains(self): - print("Sending to the " + self.axis_choice + "position PID controller: Kp: " - + str(self.pid_gui.scale_Kp.get()) + ", Ki: " + str(self.pid_gui.scale_Ki.get()) + ", Kd: "+str(self.pid_gui.scale_Ki.get())) - cf.param.set_value("posCtlPid."+self.axis_choice +"Kp", self.pid_gui.scale_Kp.get()) - cf.param.set_value("posCtlPid."+self.axis_choice +"Ki", self.pid_gui.scale_Ki.get()) - cf.param.set_value("posCtlPid."+self.axis_choice +"Kd", self.pid_gui.scale_Kd.get()) + print('Sending to the ' + self.axis_choice + + 'position PID controller: Kp: ' + + str(self.pid_gui.scale_Kp.get()) + + ', Ki: ' + str(self.pid_gui.scale_Ki.get()) + + ', Kd: '+str(self.pid_gui.scale_Ki.get())) + cf.param.set_value('posCtlPid.'+self.axis_choice + + 'Kp', self.pid_gui.scale_Kp.get()) + cf.param.set_value('posCtlPid.'+self.axis_choice + + 'Ki', self.pid_gui.scale_Ki.get()) + cf.param.set_value('posCtlPid.'+self.axis_choice + + 'Kd', self.pid_gui.scale_Kd.get()) time.sleep(0.1) @@ -162,20 +179,20 @@ def send_pid_gains(self): def do_step(self): print('do step') log_config = LogConfig(name='Position setpoint', period_in_ms=10) - log_config.add_variable("stateEstimate." + self.axis_choice, 'float') - log_config.add_variable("ctrltarget." + self.axis_choice, 'float') - - if self.axis_choice is "z": - self.commander.go_to(0,0,0.2,0,0.3,relative=True) - elif self.axis_choice is "x": - self.commander.go_to(0.2,0,0,0,0.3,relative=True) - elif self.axis_choice is "y": - self.commander.go_to(0,0.2,0,0,0.3,relative=True) + log_config.add_variable('stateEstimate.' + self.axis_choice, 'float') + log_config.add_variable('ctrltarget.' + self.axis_choice, 'float') + + if self.axis_choice == 'z': + self.commander.go_to(0, 0, 0.2, 0, 0.3, relative=True) + elif self.axis_choice == 'x': + self.commander.go_to(0.2, 0, 0, 0, 0.3, relative=True) + elif self.axis_choice == 'y': + self.commander.go_to(0, 0.2, 0, 0, 0.3, relative=True) else: print('WRONG CHOICE?!?!') self.stop_gui() - current_time=time.time() + current_time = time.time() pos_history = [] sp_history = [] @@ -184,60 +201,72 @@ def do_step(self): for log_entry in logger: data = log_entry[1] - pos_history.append(data["stateEstimate." + self.axis_choice]) - sp_history.append(data["ctrltarget." + self.axis_choice]) - time_history.append(time.time()- current_time) + pos_history.append(data['stateEstimate.' + self.axis_choice]) + sp_history.append(data['ctrltarget.' + self.axis_choice]) + time_history.append(time.time() - current_time) if ((time.time() - current_time) > STEP_RESPONSE_TIME): break - #print(pos_history) - #print(sp_history) - self.pid_gui.draw_plot(time_history,pos_history,sp_history) - if self.axis_choice is "z": - self.commander.go_to(0,0,-0.2,0,0.3,relative=True) - elif self.axis_choice is "x": - self.commander.go_to(-0.2,0,0,0,0.3,relative=True) - elif self.axis_choice is "y": - self.commander.go_to(0,-0.2,0,0,0.3,relative=True) + # print(pos_history) + # print(sp_history) + self.pid_gui.draw_plot(time_history, pos_history, sp_history) + if self.axis_choice == 'z': + self.commander.go_to(0, 0, -0.2, 0, 0.3, relative=True) + elif self.axis_choice == 'x': + self.commander.go_to(-0.2, 0, 0, 0, 0.3, relative=True) + elif self.axis_choice == 'y': + self.commander.go_to(0, -0.2, 0, 0, 0.3, relative=True) else: print('WRONG CHOICE?!?!') self.stop_gui() - + def stop_gui(self): self.pid_gui.master.quit() self.land_and_stop() - #parameter update - def change_param_callback(self,value): + # parameter update + def change_param_callback(self, value): # print(value) - self.cf.param.remove_update_callback("posCtlPid",name=self.axis_choice +"Kp") - self.cf.param.remove_update_callback("posCtlPid",name=self.axis_choice +"Ki") - self.cf.param.remove_update_callback("posCtlPid",name=self.axis_choice +"Kd") + self.cf.param.remove_update_callback( + 'posCtlPid', name=self.axis_choice + 'Kp') + self.cf.param.remove_update_callback( + 'posCtlPid', name=self.axis_choice + 'Ki') + self.cf.param.remove_update_callback( + 'posCtlPid', name=self.axis_choice + 'Kd') time.sleep(0.1) - self.cf.param.add_update_callback(group="posCtlPid", name=value +"Kp", cb=self.param_updated_callback_Kp) - self.cf.param.add_update_callback(group="posCtlPid", name=value +"Ki", cb=self.param_updated_callback_Ki) - self.cf.param.add_update_callback(group="posCtlPid", name=value +"Kd", cb=self.param_updated_callback_Kd) - - self.cf.param.request_param_update("posCtlPid."+value+"Kp") - self.cf.param.request_param_update("posCtlPid."+value+"Ki") - self.cf.param.request_param_update("posCtlPid."+value+"Kd") + self.cf.param.add_update_callback( + group='posCtlPid', name=value + + 'Kp', cb=self.param_updated_callback_Kp) + self.cf.param.add_update_callback( + group='posCtlPid', name=value + + 'Ki', cb=self.param_updated_callback_Ki) + self.cf.param.add_update_callback( + group='posCtlPid', name=value + + 'Kd', cb=self.param_updated_callback_Kd) + + self.cf.param.request_param_update('posCtlPid.'+value+'Kp') + self.cf.param.request_param_update('posCtlPid.'+value+'Ki') + self.cf.param.request_param_update('posCtlPid.'+value+'Kd') time.sleep(0.1) self.update_scale_info() self.pid_gui.clear_plot() - self.axis_choice=value + self.axis_choice = value def param_updated_callback_Kp(self, name, value): self.current_value_kp = float(value) + def param_updated_callback_Ki(self, name, value): self.current_value_ki = float(value) + def param_updated_callback_Kd(self, name, value): self.current_value_kd = float(value) def take_off(self, height): self.commander.takeoff(height, 1.0) + def land_and_stop(self): self.commander.land(0.0, 2.0) time.sleep(2) @@ -288,11 +317,11 @@ def wait_for_position_estimator(scf): if __name__ == '__main__': root = tk.Tk() - pid_gui= TunerGUI(root) + pid_gui = TunerGUI(root) cflib.crtp.init_drivers(enable_debug_driver=False) - cf=Crazyflie(rw_cache='./cache') + cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf) as scf: wait_for_position_estimator(scf) - cf_ctrl= TunerControlCF(pid_gui,cf) + cf_ctrl = TunerControlCF(pid_gui, cf) tk.mainloop() From 21145a2c7e3143ca48b57214d151751998b7dfda Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 12 May 2020 15:04:22 +0200 Subject: [PATCH 026/861] update readme with links to documentation #143 --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 48e2db631..31f6e6789 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ and Crazyflie 2.0 quadcopters. It is intended to be used by client software to communicate with and control a Crazyflie quadcopter. For instance the [cfclient][cfclient] Crazyflie PC client uses the cflib. See [below](#platform-notes) for platform specific instruction. -For more info see our [wiki](http://wiki.bitcraze.se/ "Bitcraze Wiki"). +For more info see our [documentation](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/). ## Development From 5878d49ac43b700d201cd53947e89cedceff6772 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 15 May 2020 12:07:52 +0200 Subject: [PATCH 027/861] #153 Made SyncCrazyflie.close_link() synchronous --- cflib/crazyflie/syncCrazyflie.py | 25 +++++++++++++++++------- test/crazyflie/test_syncCrazyflie.py | 29 +++++++++++++++++++++++++--- test/support/asyncCallbackCaller.py | 2 ++ 3 files changed, 46 insertions(+), 10 deletions(-) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index cfd958906..bb6a02857 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -46,7 +46,8 @@ def __init__(self, link_uri, cf=None): self.cf = Crazyflie() self._link_uri = link_uri - self._connect_event = Event() + self._connect_event = None + self._disconnect_event = None self._is_link_open = False self._error_message = None @@ -57,8 +58,12 @@ def open_link(self): self._add_callbacks() print('Connecting to %s' % self._link_uri) + + self._connect_event = Event() self.cf.open_link(self._link_uri) self._connect_event.wait() + self._connect_event = None + if not self._is_link_open: self._remove_callbacks() raise Exception(self._error_message) @@ -68,9 +73,11 @@ def __enter__(self): return self def close_link(self): - self.cf.close_link() - self._remove_callbacks() - self._is_link_open = False + if (self.is_link_open()): + self._disconnect_event = Event() + self.cf.close_link() + self._disconnect_event.wait() + self._disconnect_event = None def __exit__(self, exc_type, exc_val, exc_tb): self.close_link() @@ -83,19 +90,23 @@ def _connected(self, link_uri): has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) self._is_link_open = True - self._connect_event.set() + if self._connect_event: + self._connect_event.set() def _connection_failed(self, link_uri, msg): - """Callback when connection initial connection fails (i.e no Crazyflie + """Callback when initial connection fails (i.e no Crazyflie at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self._is_link_open = False self._error_message = msg - self._connect_event.set() + if self._connect_event: + self._connect_event.set() def _disconnected(self, link_uri): self._remove_callbacks() self._is_link_open = False + if self._disconnect_event: + self._disconnect_event.set() def _add_callbacks(self): self.cf.connected.add_callback(self._connected) diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index 22e233740..1b5181274 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -49,7 +49,16 @@ def setUp(self): self.cf_mock.open_link = AsyncCallbackCaller( cb=self.cf_mock.connected, - args=[self.uri]).trigger + args=[self.uri], + delay=0.2 + ).trigger + + self.close_link_mock = AsyncCallbackCaller( + cb=self.cf_mock.disconnected, + args=[self.uri], + delay=0.2 + ) + self.cf_mock.close_link = self.close_link_mock.trigger self.sut = SyncCrazyflie(self.uri, self.cf_mock) @@ -110,7 +119,7 @@ def test_close_link(self): self.sut.close_link() # Assert - self.cf_mock.close_link.assert_called_once_with() + self.assertEqual(1, self.close_link_mock.call_count) self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() @@ -144,7 +153,21 @@ def test_open_close_with_context_mangement(self): self.assertTrue(sut.is_link_open()) # Assert - self.cf_mock.close_link.assert_called_once_with() + self.assertEqual(1, self.close_link_mock.call_count) + self._assertAllCallbacksAreRemoved() + + def test_multiple_open_close_of_link(self): + # Fixture + + # Test + self.sut.open_link() + self.sut.close_link() + self.sut.open_link() + self.sut.close_link() + + # Assert + self.assertEqual(2, self.close_link_mock.call_count) + self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() def _assertAllCallbacksAreRemoved(self): diff --git a/test/support/asyncCallbackCaller.py b/test/support/asyncCallbackCaller.py index ecf896432..bca42ccaf 100644 --- a/test/support/asyncCallbackCaller.py +++ b/test/support/asyncCallbackCaller.py @@ -32,8 +32,10 @@ def __init__(self, cb=None, delay=0, args=(), kwargs={}): self.delay = delay self.args = args self.kwargs = kwargs + self.call_count = 0 def trigger(self, *args, **kwargs): + self.call_count += 1 Thread(target=self._make_call).start() def call_and_wait(self): From 0171d46f4490c9e278c6f3760d8753c0d170e8a9 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 5 Jun 2020 11:21:57 +0200 Subject: [PATCH 028/861] #154 Added power switch class for remote manipulation of power for the STM and NRF --- cflib/utils/power_switch.py | 84 +++++++++++++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100644 cflib/utils/power_switch.py diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py new file mode 100644 index 000000000..f0c2ae041 --- /dev/null +++ b/cflib/utils/power_switch.py @@ -0,0 +1,84 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2020 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 . +""" +This class is used to turn the power of the Crazyflie on and off via +a Crazyradio. +""" +import sys +import time + +import cflib.crtp +from cflib.drivers.crazyradio import Crazyradio + + +class PowerSwitch: + def __init__(self, uri): + self.uri = uri + uri_parts = cflib.crtp.RadioDriver.parse_uri(uri) + self.devid = uri_parts[0] + self.channel = uri_parts[1] + self.datarate = uri_parts[2] + self.address = uri_parts[3] + + def platform_power_down(self): + """ Power down the platform, both NRF and STM MCUs. + Same as turning off the platform with the power button.""" + self._send((0xf3, 0xfe, 0x01)) + + def stm_power_down(self): + """ Power down the STM MCU, the NRF will still be powered and handle + basic radio communication. The STM can be restarted again remotely. + Note: the power to expansion decks is also turned off. """ + self._send((0xf3, 0xfe, 0x02)) + + def stm_power_up(self): + """ Power up (boot) the STM MCU and decks.""" + self._send((0xf3, 0xfe, 0x03)) + + def stm_power_cycle(self): + """ Restart the STM MCU by powering it off and on. + Expansion decks will also be rebooted.""" + self.stm_power_down() + time.sleep(1) + self.stm_power_up() + + def _send(self, packet): + cr = Crazyradio(devid=self.devid) + cr.set_channel(self.channel) + cr.set_data_rate(self.datarate) + cr.set_address(self.address) + cr.set_arc(3) + + success = False + for i in range(50): + res = cr.send_packet(packet) + if res.ack: + success = True + break + + time.sleep(0.01) + + cr.close() + + if not success: + print('Failed to connect to Crazyflie at', self.uri) + sys.exit(-1) From d973929f66e4468d3a37c277e51848cec769a271 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 5 Jun 2020 11:47:52 +0200 Subject: [PATCH 029/861] #154 Updates to power switch --- cflib/utils/power_switch.py | 19 ++++++++------ sys_test/swarm_test_rig/rig_support.py | 35 ++++++-------------------- 2 files changed, 19 insertions(+), 35 deletions(-) diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index f0c2ae041..e7959dcb5 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -23,7 +23,6 @@ This class is used to turn the power of the Crazyflie on and off via a Crazyradio. """ -import sys import time import cflib.crtp @@ -31,6 +30,10 @@ class PowerSwitch: + BOOTLOADER_CMD_ALLOFF = 0x01 + BOOTLOADER_CMD_SYSOFF = 0x02 + BOOTLOADER_CMD_SYSON = 0x03 + def __init__(self, uri): self.uri = uri uri_parts = cflib.crtp.RadioDriver.parse_uri(uri) @@ -42,17 +45,17 @@ def __init__(self, uri): def platform_power_down(self): """ Power down the platform, both NRF and STM MCUs. Same as turning off the platform with the power button.""" - self._send((0xf3, 0xfe, 0x01)) + self._send(self.BOOTLOADER_CMD_ALLOFF) def stm_power_down(self): """ Power down the STM MCU, the NRF will still be powered and handle basic radio communication. The STM can be restarted again remotely. Note: the power to expansion decks is also turned off. """ - self._send((0xf3, 0xfe, 0x02)) + self._send(self.BOOTLOADER_CMD_SYSOFF) def stm_power_up(self): """ Power up (boot) the STM MCU and decks.""" - self._send((0xf3, 0xfe, 0x03)) + self._send(self.BOOTLOADER_CMD_SYSON) def stm_power_cycle(self): """ Restart the STM MCU by powering it off and on. @@ -61,7 +64,9 @@ def stm_power_cycle(self): time.sleep(1) self.stm_power_up() - def _send(self, packet): + def _send(self, cmd): + packet = (0xf3, 0xfe, cmd) + cr = Crazyradio(devid=self.devid) cr.set_channel(self.channel) cr.set_data_rate(self.datarate) @@ -80,5 +85,5 @@ def _send(self, packet): cr.close() if not success: - print('Failed to connect to Crazyflie at', self.uri) - sys.exit(-1) + raise Exception( + 'Failed to connect to Crazyflie at {}'.format(self.uri)) diff --git a/sys_test/swarm_test_rig/rig_support.py b/sys_test/swarm_test_rig/rig_support.py index 9fec7a73e..2755df777 100644 --- a/sys_test/swarm_test_rig/rig_support.py +++ b/sys_test/swarm_test_rig/rig_support.py @@ -23,8 +23,7 @@ # MA 02110-1301, USA. import time -from cflib.crtp import RadioDriver -from cflib.drivers.crazyradio import Crazyradio +from cflib.utils.power_switch import PowerSwitch class RigSupport: @@ -43,35 +42,15 @@ def __init__(self): ] def restart_devices(self, uris): - def send_packets(uris, value, description): - for uri in uris: - devid, channel, datarate, address = RadioDriver.parse_uri(uri) - radio.set_channel(channel) - radio.set_data_rate(datarate) - radio.set_address(address) - - received_packet = False - for i in range(10): - result = radio.send_packet((0xf3, 0xfe, value)) - if result.ack is True: - received_packet = True - # if i > 0: - # print('Lost packets', i, uri) - break - - if not received_packet: - raise Exception('Failed to turn device {}, for {}'. - format(description, uri)) - print('Restarting devices') - BOOTLOADER_CMD_SYSOFF = 0x02 - BOOTLOADER_CMD_SYSON = 0x03 + for uri in uris: + PowerSwitch(uri).stm_power_down() + + time.sleep(1) - radio = Crazyradio() - send_packets(uris, BOOTLOADER_CMD_SYSOFF, 'off') - send_packets(uris, BOOTLOADER_CMD_SYSON, 'on') + for uri in uris: + PowerSwitch(uri).stm_power_up() # Wait for devices to boot time.sleep(8) - radio.close() From 78cb4a84e14e2c93bff6a99db87989f2f0b7f7b3 Mon Sep 17 00:00:00 2001 From: Marcus Eliasson Date: Wed, 10 Jun 2020 13:55:04 +0200 Subject: [PATCH 030/861] Update version to 0.1.11 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index ee6f22f12..626a6494d 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.10', + version='0.1.11', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 97bdf98df3985c165474c6b6cb51ded1a7cc0857 Mon Sep 17 00:00:00 2001 From: Abiel Fernandez Date: Fri, 26 Jun 2020 03:31:10 -0500 Subject: [PATCH 031/861] Fix spelling error in multiranger_pointcloud.py --- examples/multiranger_pointcloud.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/multiranger_pointcloud.py b/examples/multiranger_pointcloud.py index ddd0354e3..1265752fc 100644 --- a/examples/multiranger_pointcloud.py +++ b/examples/multiranger_pointcloud.py @@ -38,7 +38,7 @@ There's additional setting for (see constants below): * Plotting the downwards sensor * Plotting the estimated Crazyflie postition - * Max threashold for sensors + * Max threshold for sensors * Speed factor that set's how fast the Crazyflie moves The demo is ended by either closing the graph window. From 3ce77d379820675c2774745de094b135c1c3a648 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 30 Jun 2020 10:00:12 +0200 Subject: [PATCH 032/861] #146 added velocity tuning (without velocity setpoint for now) --- examples/tuning/PID_controller_tuner.py | 124 ++++++++++++++++++------ 1 file changed, 93 insertions(+), 31 deletions(-) diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index d0e5ab46d..1a86817cb 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -42,9 +42,10 @@ from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger -URI = 'radio://0/80/2M/E7E7E7E7E7' -STANDARD_HEIGHT = 1.2 +URI = 'radio://0/30/2M/E7E7E7E702' +STANDARD_HEIGHT = 0.8 STEP_RESPONSE_TIME = 3.0 +STEP_SIZE = -0.2 #meters if len(sys.argv) > 1: URI = sys.argv[1] @@ -65,19 +66,23 @@ def __init__(self, master): self.line2 = FigureCanvasTkAgg(self.figplot, self.master) self.line2.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=1) - self.scale_Kp = tk.Scale(master, label='scale_Kp', from_=0, to=20, - length=600, tickinterval=2, resolution=0.1, + self.scale_Kp = tk.Scale(master, label='scale_Kp', from_=0, to=100, + length=1200, tickinterval=5, resolution=0.1, orient=tk.HORIZONTAL) - self.scale_Ki = tk.Scale(master, label='scale_Ki', from_=0, to=10, - length=600, tickinterval=1, resolution=0.1, + self.scale_Ki = tk.Scale(master, label='scale_Ki', from_=0, to=50, + length=1200, tickinterval=3, resolution=0.1, orient=tk.HORIZONTAL) - self.scale_Kd = tk.Scale(master, label='scale_Kd', from_=0, to=10, - length=600, tickinterval=1, resolution=0.1, + self.scale_Kd = tk.Scale(master, label='scale_Kd', from_=0, to=50, + length=1200, tickinterval=3, resolution=0.1, + orient=tk.HORIZONTAL) + self.scale_vMax = tk.Scale(master, label='vMax', from_=0, to=5, + length=1200, tickinterval=5, resolution=0.1, orient=tk.HORIZONTAL) self.scale_Kp.pack() self.scale_Ki.pack() self.scale_Kd.pack() + self.scale_vMax.pack() self.pos_array_prev = [] self.sp_array_prev = [] @@ -115,10 +120,20 @@ def __init__(self, pid_gui, scf): variable.set('z') self.dropdown = tk.OptionMenu( self.pid_gui.master, variable, 'x', 'y', 'z', - command=self.change_param_callback) + command=self.change_param_axis_callback) self.dropdown.pack() self.axis_choice = 'z' + self.label = tk.Label(self.pid_gui.master, text='Choose velocity or position!') + self.label.pack() + variable_pos = tk.StringVar(self.pid_gui.master) + variable_pos.set('pos') + self.dropdown = tk.OptionMenu( + self.pid_gui.master, variable_pos, 'pos','vel', + command=self.change_param_unit_callback) + self.dropdown.pack() + self.unit_choice = 'pos' + self.button_send = tk.Button( self.pid_gui.master, text='SEND PID GAINS', command=self.send_pid_gains).pack() @@ -135,14 +150,18 @@ def __init__(self, pid_gui, scf): group='posCtlPid', name='zKi', cb=self.param_updated_callback_Ki) self.cf.param.add_update_callback( group='posCtlPid', name='zKd', cb=self.param_updated_callback_Kd) + self.cf.param.add_update_callback( + group='posCtlPid', name='xyVelMax', cb=self.param_updated_callback_vMax) self.current_value_kp = 0 self.current_value_kd = 0 self.current_value_ki = 0 + self.current_value_vmax = 0 self.cf.param.request_param_update('posCtlPid.zKp') self.cf.param.request_param_update('posCtlPid.zKi') self.cf.param.request_param_update('posCtlPid.zKd') + self.cf.param.request_param_update('posCtlPid.xyVelMax') time.sleep(0.1) @@ -157,6 +176,8 @@ def update_scale_info(self): self.pid_gui.scale_Kp.set(self.current_value_kp) self.pid_gui.scale_Kd.set(self.current_value_kd) self.pid_gui.scale_Ki.set(self.current_value_ki) + self.pid_gui.scale_vMax.set(self.current_value_vmax) + # Buttons def send_pid_gains(self): @@ -165,12 +186,13 @@ def send_pid_gains(self): str(self.pid_gui.scale_Kp.get()) + ', Ki: ' + str(self.pid_gui.scale_Ki.get()) + ', Kd: '+str(self.pid_gui.scale_Ki.get())) - cf.param.set_value('posCtlPid.'+self.axis_choice + + cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + 'Kp', self.pid_gui.scale_Kp.get()) - cf.param.set_value('posCtlPid.'+self.axis_choice + + cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + 'Ki', self.pid_gui.scale_Ki.get()) - cf.param.set_value('posCtlPid.'+self.axis_choice + + cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + 'Kd', self.pid_gui.scale_Kd.get()) + cf.param.set_value('posCtlPid.xyVelMax', self.pid_gui.scale_vMax.get()) time.sleep(0.1) @@ -183,11 +205,11 @@ def do_step(self): log_config.add_variable('ctrltarget.' + self.axis_choice, 'float') if self.axis_choice == 'z': - self.commander.go_to(0, 0, 0.2, 0, 0.3, relative=True) + self.commander.go_to(0, 0, STEP_SIZE, 0, 0.6, relative=True) elif self.axis_choice == 'x': - self.commander.go_to(0.2, 0, 0, 0, 0.3, relative=True) + self.commander.go_to(STEP_SIZE, 0, 0, 0, 0.6, relative=True) elif self.axis_choice == 'y': - self.commander.go_to(0, 0.2, 0, 0, 0.3, relative=True) + self.commander.go_to(0, STEP_SIZE, 0, 0, 0.6, relative=True) else: print('WRONG CHOICE?!?!') self.stop_gui() @@ -211,11 +233,11 @@ def do_step(self): # print(sp_history) self.pid_gui.draw_plot(time_history, pos_history, sp_history) if self.axis_choice == 'z': - self.commander.go_to(0, 0, -0.2, 0, 0.3, relative=True) + self.commander.go_to(0, 0, -1*STEP_SIZE, 0, 1.0, relative=True) elif self.axis_choice == 'x': - self.commander.go_to(-0.2, 0, 0, 0, 0.3, relative=True) + self.commander.go_to(-1*STEP_SIZE, 0, 0, 0, 1.0, relative=True) elif self.axis_choice == 'y': - self.commander.go_to(0, -0.2, 0, 0, 0.3, relative=True) + self.commander.go_to(0, -1*STEP_SIZE, 0, 0, 1.0, relative=True) else: print('WRONG CHOICE?!?!') self.stop_gui() @@ -225,35 +247,72 @@ def stop_gui(self): self.land_and_stop() # parameter update - def change_param_callback(self, value): + def change_param_axis_callback(self, value_axis): # - print(value) + print(self.unit_choice + 'CtlPid.'+value_axis) + + groupname=self.unit_choice + 'CtlPid' self.cf.param.remove_update_callback( - 'posCtlPid', name=self.axis_choice + 'Kp') + group=groupname, name=self.axis_choice + 'Kp') self.cf.param.remove_update_callback( - 'posCtlPid', name=self.axis_choice + 'Ki') + group=groupname, name=self.axis_choice + 'Ki') self.cf.param.remove_update_callback( - 'posCtlPid', name=self.axis_choice + 'Kd') + group=groupname, name=self.axis_choice + 'Kd') time.sleep(0.1) self.cf.param.add_update_callback( - group='posCtlPid', name=value + + group=groupname, name=value_axis + 'Kp', cb=self.param_updated_callback_Kp) self.cf.param.add_update_callback( - group='posCtlPid', name=value + + group=groupname, name=value_axis + 'Ki', cb=self.param_updated_callback_Ki) self.cf.param.add_update_callback( - group='posCtlPid', name=value + + group=groupname, name=value_axis + 'Kd', cb=self.param_updated_callback_Kd) - self.cf.param.request_param_update('posCtlPid.'+value+'Kp') - self.cf.param.request_param_update('posCtlPid.'+value+'Ki') - self.cf.param.request_param_update('posCtlPid.'+value+'Kd') + self.cf.param.request_param_update(groupname+'.'+value_axis+'Kp') + self.cf.param.request_param_update(groupname+'.'+value_axis+'Ki') + self.cf.param.request_param_update(groupname+'.'+value_axis+'Kd') time.sleep(0.1) self.update_scale_info() self.pid_gui.clear_plot() - self.axis_choice = value + self.axis_choice = value_axis + + # parameter update + def change_param_unit_callback(self, value_unit): + # + print(value_unit + 'CtlPid.'+ self.axis_choice) + + groupname_old=self.unit_choice + 'CtlPid' + self.cf.param.remove_update_callback( + group=groupname_old, name=self.axis_choice + 'Kp') + self.cf.param.remove_update_callback( + group=groupname_old, name=self.axis_choice + 'Ki') + self.cf.param.remove_update_callback( + group=groupname_old, name=self.axis_choice + 'Kd') + + time.sleep(0.1) + groupname_new=value_unit + 'CtlPid' + self.cf.param.add_update_callback( + group=groupname_new, name=self.axis_choice + + 'Kp', cb=self.param_updated_callback_Kp) + self.cf.param.add_update_callback( + group=groupname_new, name=self.axis_choice + + 'Ki', cb=self.param_updated_callback_Ki) + self.cf.param.add_update_callback( + group=groupname_new, name=self.axis_choice + + 'Kd', cb=self.param_updated_callback_Kd) + + print(groupname_new+'.'+self.axis_choice+'Kp') + self.cf.param.request_param_update(groupname_new+'.'+self.axis_choice+'Kp') + self.cf.param.request_param_update(groupname_new+'.'+self.axis_choice+'Ki') + self.cf.param.request_param_update(groupname_new+'.'+self.axis_choice+'Kd') + time.sleep(0.1) + + self.update_scale_info() + + self.unit_choice = value_unit def param_updated_callback_Kp(self, name, value): self.current_value_kp = float(value) @@ -263,9 +322,12 @@ def param_updated_callback_Ki(self, name, value): def param_updated_callback_Kd(self, name, value): self.current_value_kd = float(value) + + def param_updated_callback_vMax(self, name, value): + self.current_value_vmax = float(value) def take_off(self, height): - self.commander.takeoff(height, 1.0) + self.commander.takeoff(height, 2.0) def land_and_stop(self): self.commander.land(0.0, 2.0) From 33ca2c09965f7c00b9366ca5af6be3ddb1d8b41d Mon Sep 17 00:00:00 2001 From: joshmeranda Date: Tue, 30 Jun 2020 23:01:27 -0400 Subject: [PATCH 033/861] Fix documentaion and some code typos --- README.md | 2 +- cflib/crazyflie/high_level_commander.py | 4 +- cflib/crazyflie/platformservice.py | 4 +- cflib/crazyflie/syncCrazyflie.py | 2 +- cflib/crazyflie/toc.py | 2 +- cflib/crtp/serialdriver.py | 2 +- cflib/crtp/usbdriver.py | 4 +- cflib/drivers/cfusb.py | 2 +- cflib/drivers/crazyradio.py | 4 +- cflib/positioning/motion_commander.py | 6 +- docs/development/matlab.md | 2 +- docs/development/testing.md | 2 +- docs/functonal-areas/crazyradio_lib.md | 2 +- examples/basiclog.py | 4 +- examples/cfbridge.py | 8 +-- examples/erase-ow.py | 4 +- .../lighthouse/lighthouse_openvr_multigrab.py | 14 ++-- examples/lighthouse/read-geometry-mem.py | 2 +- examples/multiranger_pointcloud.py | 4 +- examples/multiranger_push.py | 2 +- examples/positioning/matrix_light_printer.py | 2 +- examples/read-eeprom.py | 4 +- examples/read-ow.py | 4 +- examples/swarm/hl-commander-swarm.py | 2 +- examples/swarm/swarmSequenceCircle.py | 4 +- examples/write-eeprom.py | 4 +- examples/write-ow.py | 4 +- test/crazyflie/test_syncCrazyflie.py | 2 +- .../positioning/test_position_hl_commander.py | 72 +++++++++---------- 29 files changed, 87 insertions(+), 87 deletions(-) diff --git a/README.md b/README.md index 31f6e6789..9c4cfe516 100644 --- a/README.md +++ b/README.md @@ -53,7 +53,7 @@ For information and installation of the * Check to see if you pass style guidelines: `tb verify` Note: Docker and the toolbelt is an optional way of running tests and reduces the -work needed to maintain your python environmet. +work needed to maintain your python environment. ### Native python on Linux, OSX, Windows [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index e36b2b2bc..46298187e 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -75,7 +75,7 @@ def takeoff(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS, """ vertical takeoff from current x-y position to given height - :param absolute_height_m: absolut (m) + :param absolute_height_m: absolute (m) :param duration_s: time it should take until target height is reached (s) :param group_mask: mask for which CFs this should apply to @@ -100,7 +100,7 @@ def land(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS, """ vertical land from current x-y position to given height - :param absolute_height_m: absolut (m) + :param absolute_height_m: absolute (m) :param duration_s: time it should take until target height is reached (s) :param group_mask: mask for which CFs this should apply to diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index ff442cb33..3434ebc98 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -117,7 +117,7 @@ def _crt_service_callback(self, pk): self._cf.send_packet(pk) else: self._protocolVersion = -1 - logger.info('Procotol version: {}'.format( + logger.info('Protocol version: {}'.format( self.get_protocol_version())) self._callback() @@ -125,6 +125,6 @@ def _platform_callback(self, pk): if pk.channel == VERSION_COMMAND and \ pk.data[0] == VERSION_GET_PROTOCOL: self._protocolVersion = pk.data[1] - logger.info('Procotol version: {}'.format( + logger.info('Protocol version: {}'.format( self.get_protocol_version())) self._callback() diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index bb6a02857..fd5ff727c 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -24,7 +24,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. """ -The syncronous Crazyflie class is a wrapper around the "normal" Crazyflie +The synchronous Crazyflie class is a wrapper around the "normal" Crazyflie class. It handles the asynchronous nature of the Crazyflie API and turns it into blocking function. It is useful for simple scripts that performs tasks as a sequence of events. diff --git a/cflib/crazyflie/toc.py b/cflib/crazyflie/toc.py index 12f085ea0..6ec7b469c 100644 --- a/cflib/crazyflie/toc.py +++ b/cflib/crazyflie/toc.py @@ -25,7 +25,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. """ -A generic TableOfContents module that is used to fetch, store and minipulate +A generic TableOfContents module that is used to fetch, store and manipulate a TOC for logging or parameters. """ import logging diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index bb2f608de..47e40c023 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -217,7 +217,7 @@ def run(self): if r != expected: continue - # NOTE: end is (expected - 2) as the lenght of the data +2 for + # NOTE: end is (expected - 2) as the length of the data +2 for # the header bytes cksum = compute_cksum(memoryview(received)[:expected]) if cksum[0] != received_data_chk[-2] or \ diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index 475a70ba6..c0da359b1 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -71,7 +71,7 @@ def connect(self, uri, link_quality_callback, link_error_callback): The callback for linkQuality can be called at any moment from the driver to report back the link quality in percentage. The - callback from linkError will be called when a error occues with + callback from linkError will be called when a error occurs with an error message. """ @@ -215,7 +215,7 @@ class _UsbReceiveThread(threading.Thread): Radio link receiver thread used to read data from the Crazyradio USB driver. """ - # RETRYCOUNT_BEFORE_DISCONNECT = 10 + # RETRY_COUNT_BEFORE_DISCONNECT = 10 def __init__(self, cfusb, inQueue, link_quality_callback, link_error_callback): diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index 3fd6c72cb..f17b9eeca 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -143,7 +143,7 @@ def set_crtp_to_usb(self, crtp_to_usb): # Data transfers def send_packet(self, dataOut): """ Send a packet and receive the ack from the radio dongle - The ack contains information about the packet transmition + The ack contains information about the packet transmission and a data payload if the ack packet contained any """ try: if (pyusb1 is False): diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 9622f4977..c3d470689 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -278,10 +278,10 @@ def scan_channels(self, start, stop, packet): result = result + (i,) return result - # Data transferts + # Data transfers def send_packet(self, dataOut): """ Send a packet and receive the ack from the radio dongle - The ack contains information about the packet transmition + The ack contains information about the packet transmission and a data payload if the ack packet contained any """ ackIn = None data = None diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index f075bcc8a..dc3f0e365 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -80,7 +80,7 @@ def __init__(self, crazyflie, default_height=0.3): def take_off(self, height=None, velocity=VELOCITY): """ - Takes off, that is starts the motors, goes straigt up and hovers. + Takes off, that is starts the motors, goes straight up and hovers. Do not call this function if you use the with keyword. Take off is done automatically when the context is created. @@ -197,7 +197,7 @@ def turn_left(self, angle_degrees, rate=RATE): Turn to the left, staying on the spot :param angle_degrees: How far to turn (degrees) - :param rate: The trurning speed (degrees/second) + :param rate: The turning speed (degrees/second) :return: """ flight_time = angle_degrees / rate @@ -211,7 +211,7 @@ def turn_right(self, angle_degrees, rate=RATE): Turn to the right, staying on the spot :param angle_degrees: How far to turn (degrees) - :param rate: The trurning speed (degrees/second) + :param rate: The turning speed (degrees/second) :return: """ flight_time = angle_degrees / rate diff --git a/docs/development/matlab.md b/docs/development/matlab.md index 15a94cb54..bc8216c78 100644 --- a/docs/development/matlab.md +++ b/docs/development/matlab.md @@ -39,7 +39,7 @@ double dashes (no space) before 'build-base' and 'user'. ### More information (from Mathworks MATLAB documentation) -System reqirements: +System requirements: Installation: diff --git a/docs/development/testing.md b/docs/development/testing.md index cd05d12bd..674f1f0e4 100644 --- a/docs/development/testing.md +++ b/docs/development/testing.md @@ -13,7 +13,7 @@ For information and installation of the * Check to see if you pass style guidelines: `tb verify` Note: Docker and the toolbelt is an optional way of running tests and reduces the -work needed to maintain your python environmet. +work needed to maintain your python environment. ### Native python on Linux, OSX, Windows [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` diff --git a/docs/functonal-areas/crazyradio_lib.md b/docs/functonal-areas/crazyradio_lib.md index 25c1b747d..ac12472f6 100644 --- a/docs/functonal-areas/crazyradio_lib.md +++ b/docs/functonal-areas/crazyradio_lib.md @@ -12,7 +12,7 @@ Theory of operation ------------------- The Crazyradio is configured in PTX mode which means that it will start -all comunication. If a device in PRX mode is on the same address, +all communication. If a device in PRX mode is on the same address, channel and datarate, the device will send back an ack packet that may contains data. diff --git a/examples/basiclog.py b/examples/basiclog.py index e8c118c89..b761270df 100644 --- a/examples/basiclog.py +++ b/examples/basiclog.py @@ -101,12 +101,12 @@ def _stab_log_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): - """Callback froma the log API when data arrives""" + """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 speficied address)""" + at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False diff --git a/examples/cfbridge.py b/examples/cfbridge.py index 1d7677e6f..0ff43a13f 100755 --- a/examples/cfbridge.py +++ b/examples/cfbridge.py @@ -3,8 +3,8 @@ 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/Reciever) -modes. Thus, data is only recieved as a response to a sent packet. +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) @@ -91,12 +91,12 @@ def _stab_log_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): - """Callback froma the log API when data arrives""" + """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 speficied address)""" + at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False diff --git a/examples/erase-ow.py b/examples/erase-ow.py index 705ceabe9..d8a66d2d2 100644 --- a/examples/erase-ow.py +++ b/examples/erase-ow.py @@ -100,12 +100,12 @@ def _stab_log_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): - """Callback froma the log API when data arrives""" + """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 speficied address)""" + at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index 8d30f053d..ec408cabe 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -107,7 +107,7 @@ def start_position_printing(scf): log_conf.start() -def vector_substract(v0, v1): +def vector_subtract(v0, v1): return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] @@ -151,10 +151,10 @@ def run_sequence(scf0, scf1): print('Grab started') grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - dist0 = vector_norm(vector_substract(grab_controller_start, - setpoints[0])) - dist1 = vector_norm(vector_substract(grab_controller_start, - setpoints[1])) + 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 @@ -171,8 +171,8 @@ def run_sequence(scf0, scf1): if trigger: curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] setpoints[closest] = vector_add( - grab_setpoint_start, vector_substract(curr, - grab_controller_start)) + grab_setpoint_start, vector_subtract(curr, + grab_controller_start)) cf0.commander.send_position_setpoint(setpoints[0][0], setpoints[0][1], diff --git a/examples/lighthouse/read-geometry-mem.py b/examples/lighthouse/read-geometry-mem.py index d6cd5b12b..26cd9a212 100644 --- a/examples/lighthouse/read-geometry-mem.py +++ b/examples/lighthouse/read-geometry-mem.py @@ -48,7 +48,7 @@ def __init__(self, uri): if count != 1: raise Exception('Unexpected nr of memories found:', count) - print('Rquesting data') + print('Requesting data') mems[0].update(self._data_updated) while not self.got_data: diff --git a/examples/multiranger_pointcloud.py b/examples/multiranger_pointcloud.py index 1265752fc..d9054e10b 100644 --- a/examples/multiranger_pointcloud.py +++ b/examples/multiranger_pointcloud.py @@ -37,7 +37,7 @@ There's additional setting for (see constants below): * Plotting the downwards sensor - * Plotting the estimated Crazyflie postition + * Plotting the estimated Crazyflie position * Max threshold for sensors * Speed factor that set's how fast the Crazyflie moves @@ -82,7 +82,7 @@ PLOT_CF = False # Enable plotting of down sensor PLOT_SENSOR_DOWN = False -# Set the sensor threashold (in mm) +# Set the sensor threshold (in mm) SENSOR_TH = 2000 # Set the speed factor for moving and rotating SPEED_FACTOR = 0.3 diff --git a/examples/multiranger_push.py b/examples/multiranger_push.py index a63e01f86..d52f774ac 100755 --- a/examples/multiranger_push.py +++ b/examples/multiranger_push.py @@ -25,7 +25,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. """ -Example scipts that allows a user to "push" the Crazyflie 2.0 around +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 diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index cd62017e8..b4fd15bca 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -26,7 +26,7 @@ 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 sicky paper can be attached on +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 diff --git a/examples/read-eeprom.py b/examples/read-eeprom.py index eea58f901..3800ecef2 100644 --- a/examples/read-eeprom.py +++ b/examples/read-eeprom.py @@ -94,12 +94,12 @@ def _stab_log_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): - """Callback froma the log API when data arrives""" + """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 speficied address)""" + at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False diff --git a/examples/read-ow.py b/examples/read-ow.py index 235e9eae3..c38a733fd 100644 --- a/examples/read-ow.py +++ b/examples/read-ow.py @@ -101,12 +101,12 @@ def _stab_log_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): - """Callback froma the log API when data arrives""" + """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 speficied address)""" + at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index c1685342a..d13c50846 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -25,7 +25,7 @@ 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 Crazyfles can +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. diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index d0c637809..64509a85e 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -25,7 +25,7 @@ # MA 02110-1301, USA. """ A script to fly 5 Crazyflies in formation. One stays in the center and the -other four fly aound it in a circle. Mainly intended to be used with 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 @@ -53,7 +53,7 @@ URI4 = 'radio://0/110/2M/E7E7E7E703' # d: diameter of circle -# z: altituce +# z: altitude params0 = {'d': 1.0, 'z': 0.3} params1 = {'d': 1.0, 'z': 0.3} params2 = {'d': 0.0, 'z': 0.5} diff --git a/examples/write-eeprom.py b/examples/write-eeprom.py index 0dfe09bbc..1c6efe249 100644 --- a/examples/write-eeprom.py +++ b/examples/write-eeprom.py @@ -106,12 +106,12 @@ def _stab_log_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): - """Callback froma the log API when data arrives""" + """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 speficied address)""" + at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False diff --git a/examples/write-ow.py b/examples/write-ow.py index 17ab60eed..63658dd18 100644 --- a/examples/write-ow.py +++ b/examples/write-ow.py @@ -112,12 +112,12 @@ def _stab_log_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): - """Callback froma the log API when data arrives""" + """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 speficied address)""" + at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index 1b5181274..29f6d7644 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -145,7 +145,7 @@ def test_closed_if_connection_is_lost(self): self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() - def test_open_close_with_context_mangement(self): + def test_open_close_with_context_management(self): # Fixture # Test diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index a347824d9..7aed9681f 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -180,13 +180,13 @@ def test_that_it_goes_to_position( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.go_to(1.0, 2.0, 3.0, 4.0) # Assert - distance = self._distance(inital_pos, (1.0, 2.0, 3.0)) + distance = self._distance(initial_pos, (1.0, 2.0, 3.0)) duration = distance / 4.0 self.commander_mock.go_to.assert_called_with( 1.0, 2.0, 3.0, 0.0, duration) @@ -196,10 +196,10 @@ def test_that_it_does_not_send_goto_to_same_position( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test - self.sut.go_to(inital_pos[0], inital_pos[1], inital_pos[2]) + self.sut.go_to(initial_pos[0], initial_pos[1], initial_pos[2]) # Assert self.commander_mock.go_to.assert_not_called() @@ -208,7 +208,7 @@ def test_that_it_moves_distance( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.move_distance(1.0, 2.0, 3.0, 4.0) @@ -217,9 +217,9 @@ def test_that_it_moves_distance( distance = self._distance((0.0, 0.0, 0.0), (1.0, 2.0, 3.0)) duration = distance / 4.0 final_pos = ( - inital_pos[0] + 1.0, - inital_pos[1] + 2.0, - inital_pos[2] + 3.0) + initial_pos[0] + 1.0, + initial_pos[1] + 2.0, + initial_pos[2] + 3.0) self.commander_mock.go_to.assert_called_with( final_pos[0], final_pos[1], final_pos[2], 0.0, duration) sleep_mock.assert_called_with(duration) @@ -228,7 +228,7 @@ def test_that_it_goes_forward( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.forward(1.0, 2.0) @@ -236,9 +236,9 @@ def test_that_it_goes_forward( # Assert duration = 1.0 / 2.0 final_pos = ( - inital_pos[0] + 1.0, - inital_pos[1], - inital_pos[2]) + initial_pos[0] + 1.0, + initial_pos[1], + initial_pos[2]) self.commander_mock.go_to.assert_called_with( final_pos[0], final_pos[1], final_pos[2], 0.0, duration) sleep_mock.assert_called_with(duration) @@ -247,7 +247,7 @@ def test_that_it_goes_back( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.back(1.0, 2.0) @@ -255,9 +255,9 @@ def test_that_it_goes_back( # Assert duration = 1.0 / 2.0 final_pos = ( - inital_pos[0] - 1.0, - inital_pos[1], - inital_pos[2]) + initial_pos[0] - 1.0, + initial_pos[1], + initial_pos[2]) self.commander_mock.go_to.assert_called_with( final_pos[0], final_pos[1], final_pos[2], 0.0, duration) sleep_mock.assert_called_with(duration) @@ -266,7 +266,7 @@ def test_that_it_goes_left( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.left(1.0, 2.0) @@ -274,9 +274,9 @@ def test_that_it_goes_left( # Assert duration = 1.0 / 2.0 final_pos = ( - inital_pos[0], - inital_pos[1] + 1.0, - inital_pos[2]) + initial_pos[0], + initial_pos[1] + 1.0, + initial_pos[2]) self.commander_mock.go_to.assert_called_with( final_pos[0], final_pos[1], final_pos[2], 0.0, duration) sleep_mock.assert_called_with(duration) @@ -285,7 +285,7 @@ def test_that_it_goes_right( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.right(1.0, 2.0) @@ -293,9 +293,9 @@ def test_that_it_goes_right( # Assert duration = 1.0 / 2.0 final_pos = ( - inital_pos[0], - inital_pos[1] - 1, - inital_pos[2]) + initial_pos[0], + initial_pos[1] - 1, + initial_pos[2]) self.commander_mock.go_to.assert_called_with( final_pos[0], final_pos[1], final_pos[2], 0, duration) sleep_mock.assert_called_with(duration) @@ -304,7 +304,7 @@ def test_that_it_goes_up( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.up(1.0, 2.0) @@ -312,9 +312,9 @@ def test_that_it_goes_up( # Assert duration = 1.0 / 2.0 final_pos = ( - inital_pos[0], - inital_pos[1], - inital_pos[2] + 1) + initial_pos[0], + initial_pos[1], + initial_pos[2] + 1) self.commander_mock.go_to.assert_called_with( final_pos[0], final_pos[1], final_pos[2], 0, duration) sleep_mock.assert_called_with(duration) @@ -323,7 +323,7 @@ def test_that_it_goes_down( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() # Test self.sut.down(1.0, 2.0) @@ -331,9 +331,9 @@ def test_that_it_goes_down( # Assert duration = 1.0 / 2.0 final_pos = ( - inital_pos[0], - inital_pos[1], - inital_pos[2] - 1) + initial_pos[0], + initial_pos[1], + initial_pos[2] - 1) self.commander_mock.go_to.assert_called_with( final_pos[0], final_pos[1], final_pos[2], 0, duration) sleep_mock.assert_called_with(duration) @@ -342,14 +342,14 @@ def test_that_default_velocity_is_used( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() self.sut.set_default_velocity(7) # Test self.sut.go_to(1.0, 2.0, 3.0) # Assert - distance = self._distance(inital_pos, (1.0, 2.0, 3.0)) + distance = self._distance(initial_pos, (1.0, 2.0, 3.0)) duration = distance / 7.0 self.commander_mock.go_to.assert_called_with( 1.0, 2.0, 3.0, 0.0, duration) @@ -359,7 +359,7 @@ def test_that_default_height_is_used( self, sleep_mock): # Fixture self.sut.take_off() - inital_pos = self.sut.get_position() + initial_pos = self.sut.get_position() self.sut.set_default_velocity(7.0) self.sut.set_default_height(5.0) @@ -367,7 +367,7 @@ def test_that_default_height_is_used( self.sut.go_to(1.0, 2.0) # Assert - distance = self._distance(inital_pos, (1.0, 2.0, 5.0)) + distance = self._distance(initial_pos, (1.0, 2.0, 5.0)) duration = distance / 7.0 self.commander_mock.go_to.assert_called_with( 1.0, 2.0, 5.0, 0.0, duration) From 2a17caf1b94d85b3c63a9ee07716153612658555 Mon Sep 17 00:00:00 2001 From: Nikos Verschore Date: Wed, 15 Jul 2020 23:38:10 +0200 Subject: [PATCH 034/861] Add memory handler for the Led Timings driver #597 + example --- cflib/crazyflie/mem.py | 63 +++++++++++++++++++++++++++++++++ examples/basicLedTiming.py | 72 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 135 insertions(+) create mode 100644 examples/basicLedTiming.py diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 06e0ac55a..74eb0b4da 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -76,6 +76,7 @@ class MemoryElement(object): TYPE_LOCO2 = 0x13 TYPE_LH = 0x14 TYPE_MEMORY_TESTER = 0x15 + TYPE_DRIVER_LEDMEM = 0x17 def __init__(self, id, type, size, mem_handler): """Initialize the element with default values""" @@ -91,6 +92,8 @@ def type_to_string(t): return 'I2C' if t == MemoryElement.TYPE_1W: return '1-wire' + if t == MemoryElement.TYPE_DRIVER_LEDMEM: + return 'LED memory driver' if t == MemoryElement.TYPE_DRIVER_LED: return 'LED driver' if t == MemoryElement.TYPE_LOCO: @@ -193,6 +196,60 @@ def disconnect(self): self._update_finished_cb = None self._write_finished_cb = None +class LEDMemoryDriverMemory(MemoryElement): + """Memory interface for using the LED-ring mapped memory for setting RGB + values for all the LEDs in the ring""" + + def __init__(self, id, type, size, mem_handler): + """Initialize with 12 LEDs""" + super(LEDMemoryDriverMemory, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + self._update_finished_cb = None + self._write_finished_cb = None + + self.timings = [] + + def add(self, time, rgb, leds=0, fade=False, rotate=0): + self.timings.append({ + "time": time, + "rgb": rgb, + "leds": leds, + "fade": fade, + "rotate": rotate + }); + + def write_data(self, write_finished_cb): + if write_finished_cb is not None: + self._write_finished_cb = write_finished_cb + + data = [] + for timing in self.timings: #[self.start:self.start+6]: + # In order to fit all the LEDs in one radio packet RGB565 is used + # to compress the colors. The calculations below converts 3 bytes + # RGB into 2 bytes RGB565. Then shifts the value of each color to + # LSB, applies the intensity and shifts them back for correct + # alignment on 2 bytes. + R5 = ((int)((((int(timing["rgb"]["r"]) & 0xFF) * 249 + 1014) >> 11) & 0x1F)) + G6 = ((int)((((int(timing["rgb"]["g"]) & 0xFF) * 253 + 505) >> 10) & 0x3F)) + B5 = ((int)((((int(timing["rgb"]["b"]) & 0xFF) * 249 + 1014) >> 11) & 0x1F)) + led = (int(R5) << 11) | (int(G6) << 5) | (int(B5) << 0) + extra = ((timing["leds"]) & 0x0F ) | ((timing["fade"] << 4) & 0x10) | ((timing["rotate"] << 5) & 0xE0) + + if (timing["time"] & 0xFF) != 0 or (led >> 8) !=0 or (led & 0xFF) != 0 or extra != 0: + data += [timing["time"] & 0xFF, led >> 8, led & 0xFF, extra] + + data += [0,0,0,0] + self.mem_handler.write(self, 0x00, bytearray(data), flush_queue=True) + + def write_done(self, mem, addr): + if mem.id == self.id and self._write_finished_cb: + self._write_finished_cb(self, addr) + self._write_finished_cb = None + + def disconnect(self): + self._update_finished_cb = None + self._write_finished_cb = None + class I2CElement(MemoryElement): @@ -1284,6 +1341,12 @@ def _new_packet_cb(self, packet): logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_DRIVER_LEDMEM: + mem = LEDMemoryDriverMemory(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py new file mode 100644 index 000000000..69c074a37 --- /dev/null +++ b/examples/basicLedTiming.py @@ -0,0 +1,72 @@ +# -*- 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +""" +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 + +URI = '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 (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + + # Get LED memory and write to it + mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LEDMEM) + print(mem) + 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) + + # 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) From cdc62db05152588804f2844002be9882a0435d98 Mon Sep 17 00:00:00 2001 From: Nikos Verschore Date: Thu, 16 Jul 2020 10:44:43 +0200 Subject: [PATCH 035/861] fix tests --- cflib/crazyflie/mem.py | 45 +++++++++++++++++++++++--------------- examples/basicLedTiming.py | 18 +++++++-------- 2 files changed, 36 insertions(+), 27 deletions(-) diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 74eb0b4da..472ab9146 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -196,14 +196,17 @@ def disconnect(self): self._update_finished_cb = None self._write_finished_cb = None + class LEDMemoryDriverMemory(MemoryElement): """Memory interface for using the LED-ring mapped memory for setting RGB values for all the LEDs in the ring""" def __init__(self, id, type, size, mem_handler): """Initialize with 12 LEDs""" - super(LEDMemoryDriverMemory, self).__init__(id=id, type=type, size=size, - mem_handler=mem_handler) + super(LEDMemoryDriverMemory, self).__init__(id=id, + type=type, + size=size, + mem_handler=mem_handler) self._update_finished_cb = None self._write_finished_cb = None @@ -211,34 +214,39 @@ def __init__(self, id, type, size, mem_handler): def add(self, time, rgb, leds=0, fade=False, rotate=0): self.timings.append({ - "time": time, - "rgb": rgb, - "leds": leds, - "fade": fade, - "rotate": rotate - }); + 'time': time, + 'rgb': rgb, + 'leds': leds, + 'fade': fade, + 'rotate': rotate + }) def write_data(self, write_finished_cb): if write_finished_cb is not None: self._write_finished_cb = write_finished_cb data = [] - for timing in self.timings: #[self.start:self.start+6]: + for timing in self.timings: # In order to fit all the LEDs in one radio packet RGB565 is used # to compress the colors. The calculations below converts 3 bytes # RGB into 2 bytes RGB565. Then shifts the value of each color to # LSB, applies the intensity and shifts them back for correct # alignment on 2 bytes. - R5 = ((int)((((int(timing["rgb"]["r"]) & 0xFF) * 249 + 1014) >> 11) & 0x1F)) - G6 = ((int)((((int(timing["rgb"]["g"]) & 0xFF) * 253 + 505) >> 10) & 0x3F)) - B5 = ((int)((((int(timing["rgb"]["b"]) & 0xFF) * 249 + 1014) >> 11) & 0x1F)) + R5 = ((int)((((int(timing['rgb']['r']) & 0xFF) * 249 + 1014) >> 11) + & 0x1F)) + G6 = ((int)((((int(timing['rgb']['g']) & 0xFF) * 253 + 505) >> 10) + & 0x3F)) + B5 = ((int)((((int(timing['rgb']['b']) & 0xFF) * 249 + 1014) >> 11) + & 0x1F)) led = (int(R5) << 11) | (int(G6) << 5) | (int(B5) << 0) - extra = ((timing["leds"]) & 0x0F ) | ((timing["fade"] << 4) & 0x10) | ((timing["rotate"] << 5) & 0xE0) - - if (timing["time"] & 0xFF) != 0 or (led >> 8) !=0 or (led & 0xFF) != 0 or extra != 0: - data += [timing["time"] & 0xFF, led >> 8, led & 0xFF, extra] + extra = ((timing['leds']) & 0x0F) | ( + (timing['fade'] << 4) & 0x10) | ( + (timing['rotate'] << 5) & 0xE0) - data += [0,0,0,0] + if (timing['time'] & 0xFF) != 0 or led != 0 or extra != 0: + data += [timing['time'] & 0xFF, led >> 8, led & 0xFF, extra] + + data += [0, 0, 0, 0] self.mem_handler.write(self, 0x00, bytearray(data), flush_queue=True) def write_done(self, mem, addr): @@ -1343,7 +1351,8 @@ def _new_packet_cb(self, packet): self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_DRIVER_LEDMEM: mem = LEDMemoryDriverMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) + size=mem_size, + mem_handler=self) logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py index 69c074a37..4c3fa4986 100644 --- a/examples/basicLedTiming.py +++ b/examples/basicLedTiming.py @@ -54,15 +54,15 @@ # Get LED memory and write to it mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LEDMEM) print(mem) - 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].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) # Set virtual mem effect effect From 2032cad114ba2f42b2e4423119f9ffb9e4115959 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Jul 2020 13:27:56 +0200 Subject: [PATCH 036/861] removed the retry count (part of #157) --- cflib/crtp/usbdriver.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index c0da359b1..e67a7b622 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -215,8 +215,6 @@ class _UsbReceiveThread(threading.Thread): Radio link receiver thread used to read data from the Crazyradio USB driver. """ - # RETRY_COUNT_BEFORE_DISCONNECT = 10 - def __init__(self, cfusb, inQueue, link_quality_callback, link_error_callback): """ Create the object """ From f4ba859a6c42ec62c6cc93235a45704bc8942348 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Jul 2020 13:36:30 +0200 Subject: [PATCH 037/861] (#146) resolved flake8 issue --- examples/tuning/PID_controller_tuner.py | 39 ++++++++++++++----------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 1a86817cb..e8ce3eb80 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -45,7 +45,7 @@ URI = 'radio://0/30/2M/E7E7E7E702' STANDARD_HEIGHT = 0.8 STEP_RESPONSE_TIME = 3.0 -STEP_SIZE = -0.2 #meters +STEP_SIZE = -0.2 # meters if len(sys.argv) > 1: URI = sys.argv[1] @@ -76,8 +76,8 @@ def __init__(self, master): length=1200, tickinterval=3, resolution=0.1, orient=tk.HORIZONTAL) self.scale_vMax = tk.Scale(master, label='vMax', from_=0, to=5, - length=1200, tickinterval=5, resolution=0.1, - orient=tk.HORIZONTAL) + length=1200, tickinterval=5, resolution=0.1, + orient=tk.HORIZONTAL) self.scale_Kp.pack() self.scale_Ki.pack() @@ -124,12 +124,13 @@ def __init__(self, pid_gui, scf): self.dropdown.pack() self.axis_choice = 'z' - self.label = tk.Label(self.pid_gui.master, text='Choose velocity or position!') + self.label = tk.Label(self.pid_gui.master, + text='Choose velocity or position!') self.label.pack() variable_pos = tk.StringVar(self.pid_gui.master) variable_pos.set('pos') self.dropdown = tk.OptionMenu( - self.pid_gui.master, variable_pos, 'pos','vel', + self.pid_gui.master, variable_pos, 'pos', 'vel', command=self.change_param_unit_callback) self.dropdown.pack() self.unit_choice = 'pos' @@ -151,7 +152,8 @@ def __init__(self, pid_gui, scf): self.cf.param.add_update_callback( group='posCtlPid', name='zKd', cb=self.param_updated_callback_Kd) self.cf.param.add_update_callback( - group='posCtlPid', name='xyVelMax', cb=self.param_updated_callback_vMax) + group='posCtlPid', name='xyVelMax', + cb=self.param_updated_callback_vMax) self.current_value_kp = 0 self.current_value_kd = 0 @@ -178,8 +180,8 @@ def update_scale_info(self): self.pid_gui.scale_Ki.set(self.current_value_ki) self.pid_gui.scale_vMax.set(self.current_value_vmax) - # Buttons + def send_pid_gains(self): print('Sending to the ' + self.axis_choice + 'position PID controller: Kp: ' + @@ -250,8 +252,8 @@ def stop_gui(self): def change_param_axis_callback(self, value_axis): # print(self.unit_choice + 'CtlPid.'+value_axis) - - groupname=self.unit_choice + 'CtlPid' + + groupname = self.unit_choice + 'CtlPid' self.cf.param.remove_update_callback( group=groupname, name=self.axis_choice + 'Kp') self.cf.param.remove_update_callback( @@ -282,9 +284,9 @@ def change_param_axis_callback(self, value_axis): # parameter update def change_param_unit_callback(self, value_unit): # - print(value_unit + 'CtlPid.'+ self.axis_choice) - - groupname_old=self.unit_choice + 'CtlPid' + print(value_unit + 'CtlPid.' + self.axis_choice) + + groupname_old = self.unit_choice + 'CtlPid' self.cf.param.remove_update_callback( group=groupname_old, name=self.axis_choice + 'Kp') self.cf.param.remove_update_callback( @@ -293,7 +295,7 @@ def change_param_unit_callback(self, value_unit): group=groupname_old, name=self.axis_choice + 'Kd') time.sleep(0.1) - groupname_new=value_unit + 'CtlPid' + groupname_new = value_unit + 'CtlPid' self.cf.param.add_update_callback( group=groupname_new, name=self.axis_choice + 'Kp', cb=self.param_updated_callback_Kp) @@ -305,9 +307,12 @@ def change_param_unit_callback(self, value_unit): 'Kd', cb=self.param_updated_callback_Kd) print(groupname_new+'.'+self.axis_choice+'Kp') - self.cf.param.request_param_update(groupname_new+'.'+self.axis_choice+'Kp') - self.cf.param.request_param_update(groupname_new+'.'+self.axis_choice+'Ki') - self.cf.param.request_param_update(groupname_new+'.'+self.axis_choice+'Kd') + self.cf.param.request_param_update( + groupname_new+'.'+self.axis_choice+'Kp') + self.cf.param.request_param_update( + groupname_new+'.'+self.axis_choice+'Ki') + self.cf.param.request_param_update( + groupname_new+'.'+self.axis_choice+'Kd') time.sleep(0.1) self.update_scale_info() @@ -322,7 +327,7 @@ def param_updated_callback_Ki(self, name, value): def param_updated_callback_Kd(self, name, value): self.current_value_kd = float(value) - + def param_updated_callback_vMax(self, name, value): self.current_value_vmax = float(value) From b7b537725a21a1b51e3ac6c40ee6f8e3234b66c0 Mon Sep 17 00:00:00 2001 From: Nikos Verschore Date: Thu, 16 Jul 2020 15:09:25 +0200 Subject: [PATCH 038/861] Change naming to ledtiming --- cflib/crazyflie/mem.py | 22 +++++++++++----------- examples/basicLedTiming.py | 3 +-- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 472ab9146..a35a6eec3 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -76,7 +76,7 @@ class MemoryElement(object): TYPE_LOCO2 = 0x13 TYPE_LH = 0x14 TYPE_MEMORY_TESTER = 0x15 - TYPE_DRIVER_LEDMEM = 0x17 + TYPE_DRIVER_LEDTIMING = 0x17 def __init__(self, id, type, size, mem_handler): """Initialize the element with default values""" @@ -92,7 +92,7 @@ def type_to_string(t): return 'I2C' if t == MemoryElement.TYPE_1W: return '1-wire' - if t == MemoryElement.TYPE_DRIVER_LEDMEM: + if t == MemoryElement.TYPE_DRIVER_LEDTIMING: return 'LED memory driver' if t == MemoryElement.TYPE_DRIVER_LED: return 'LED driver' @@ -197,16 +197,16 @@ def disconnect(self): self._write_finished_cb = None -class LEDMemoryDriverMemory(MemoryElement): +class LEDTimingsDriverMemory(MemoryElement): """Memory interface for using the LED-ring mapped memory for setting RGB values for all the LEDs in the ring""" def __init__(self, id, type, size, mem_handler): """Initialize with 12 LEDs""" - super(LEDMemoryDriverMemory, self).__init__(id=id, - type=type, - size=size, - mem_handler=mem_handler) + super(LEDTimingsDriverMemory, self).__init__(id=id, + type=type, + size=size, + mem_handler=mem_handler) self._update_finished_cb = None self._write_finished_cb = None @@ -1349,10 +1349,10 @@ def _new_packet_cb(self, packet): logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_DRIVER_LEDMEM: - mem = LEDMemoryDriverMemory(id=mem_id, type=mem_type, - size=mem_size, - mem_handler=self) + elif mem_type == MemoryElement.TYPE_DRIVER_LEDTIMING: + mem = LEDTimingsDriverMemory(id=mem_id, type=mem_type, + size=mem_size, + mem_handler=self) logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py index 4c3fa4986..620600e11 100644 --- a/examples/basicLedTiming.py +++ b/examples/basicLedTiming.py @@ -52,8 +52,7 @@ cf = scf.cf # Get LED memory and write to it - mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LEDMEM) - print(mem) + mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LEDTIMING) 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) From ccc309c66d9a89b644c775626eef4dce94fb6dbb Mon Sep 17 00:00:00 2001 From: Nikos Verschore Date: Thu, 16 Jul 2020 15:12:25 +0200 Subject: [PATCH 039/861] Update documentation --- cflib/crazyflie/mem.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index a35a6eec3..4aa065df8 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -199,10 +199,9 @@ def disconnect(self): class LEDTimingsDriverMemory(MemoryElement): """Memory interface for using the LED-ring mapped memory for setting RGB - values for all the LEDs in the ring""" + values over time. To upload and run a show sequence of the LEDs in the ring""" def __init__(self, id, type, size, mem_handler): - """Initialize with 12 LEDs""" super(LEDTimingsDriverMemory, self).__init__(id=id, type=type, size=size, From b16a2fc2735554ffd1db4ac91caf11534649b1b3 Mon Sep 17 00:00:00 2001 From: Nikos Verschore Date: Thu, 16 Jul 2020 15:51:45 +0200 Subject: [PATCH 040/861] Fix tests --- cflib/crazyflie/mem.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 4aa065df8..6ab6fa397 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -199,7 +199,8 @@ def disconnect(self): class LEDTimingsDriverMemory(MemoryElement): """Memory interface for using the LED-ring mapped memory for setting RGB - values over time. To upload and run a show sequence of the LEDs in the ring""" + values over time. To upload and run a show sequence of + the LEDs in the ring""" def __init__(self, id, type, size, mem_handler): super(LEDTimingsDriverMemory, self).__init__(id=id, From 58603fcc902b8dab04dc5cf2ea3dfa5018ce93ef Mon Sep 17 00:00:00 2001 From: Nikos Verschore Date: Thu, 16 Jul 2020 15:59:24 +0200 Subject: [PATCH 041/861] Fix tests --- cflib/crazyflie/mem.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 6ab6fa397..518b725cb 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -199,7 +199,7 @@ def disconnect(self): class LEDTimingsDriverMemory(MemoryElement): """Memory interface for using the LED-ring mapped memory for setting RGB - values over time. To upload and run a show sequence of + values over time. To upload and run a show sequence of the LEDs in the ring""" def __init__(self, id, type, size, mem_handler): From af037062914a110bf9af698aa463f1d7886f02cf Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Jul 2020 13:46:10 +0200 Subject: [PATCH 042/861] (#159) first draft of a step-by-step log-param tutorial --- docs/step-by-step/connect_log_param.md | 457 +++++++++++++++++++++ examples/step-by-step/connect_log_param.py | 112 +++++ 2 files changed, 569 insertions(+) create mode 100644 docs/step-by-step/connect_log_param.md create mode 100644 examples/step-by-step/connect_log_param.py diff --git a/docs/step-by-step/connect_log_param.md b/docs/step-by-step/connect_log_param.md new file mode 100644 index 000000000..d02f9f1e6 --- /dev/null +++ b/docs/step-by-step/connect_log_param.md @@ -0,0 +1,457 @@ +--- +title: Connecting, logging and parameters +page_id: sbs_connect_log_param +--- + +On this step by step guide we will show you how to connect to your crazyflie through the crazyflie python library by a python script. This is the starting point for developing for the crazyflie for off-board enabled flight. + +# Prerequisites + +We will assume that you already know this before you start with the tutorial: + +* Some basic experience with python +* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/). +* Able to connect the crazyflie to the CFClient and look at the log tabs and parameters (here is a [userguide](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/)). + + +## Install the cflib + +Make sure that you have [python3](https://www.python.org), which should contain pip3. In a terminal please write the following: + +`pip3 install cflib` + +This should have been installed if you installed the cfclient already (on a linux system), but it is always good to double check this :) + +# Step 1. Connecting with the crazyflie + +## Begin the python script + +Open up a python script anywhere that is convenient for you. We use Visual Studio code ourselves but you can use the Python editor IDLE3 if you want. + +* For python editor: select file->new +* For VS code: select file-> new file + +You can call it `connect_log_param.py` (that is what we are using in this tutorial) + +Then you would need to start with the following standard python libraries. + +``` +import logging +import time +``` + +then you need to import the libraries for cflib: + +``` +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +``` + +* The cflib.crtp module is for scanning for crazyflies instances. +* The Crazyflie class is used to easily connect/send/receive data +from a Crazyflie. +* The synCrazyflie class is a wrapper around the "normal" Crazyflie +class. It handles the asynchronous nature of the Crazyflie API and turns it +into blocking function. + +## URI of the Crazyflie + +After these imports, start the script with: + +``` +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M/E7E7E7E7E7' +``` + +This is the radio uri of the crazyflie, and currently displaying the default. It should be probably fine but if you do not know what the uri of your crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) + +## Main + +Write the following in the script: +``` +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + + simple_connect() +``` + +The `syncCrazyflie` will create a synchronous Crazyflie instance with the specified link_uri. As you can see we are currently calling an non-existing function, so you will need to make that function first before you run the script. + +## Function for connecting with the crazyflie + +Start a function above the main function (but below the URI) which you call simple connect: + +``` +def simple_connect(): + + print("Yeah, I'm connected! :D") + time.sleep(3) + print("Now I will disconnect :'(") +``` + + + +## Run the script + +Now run the script in your terminal: + + +`python3 connect_log_param.py` + +You are supposed to see the following in the terminal: +``` +Connecting to radio://0/80/2M/E7E7E7E7E7 +Connected to radio://0/80/2M/E7E7E7E7E7 +Yeah, I'm connected! :D +Now I will disconnect :'( +``` + + +The script connected with your crazyflie, synced and disconnected after a few seconds. You can see that the M3 LED is flashing yellow, which means that the crazyflie is connected to the script, but as soon as it leaves the `simple_connect()` function, the LED turns of. The Crazyflie is no longer connected + +Not super exciting stuff yet but it is a great start! It is also a good test if everything is correctly configured on your system. + + +If you are getting an error, retrace your steps or check if your code matches the entire code underneath here. Also make sure your crazyflie is on and your crazyradio PA connected to you computer, and that the crazyflie is not connected to anything else (like the cfclient). If everything is peachy, please continue to the next part! + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M/E7E7E7E7E7' + +def simple_connect(): + + print("Yeah, I'm connected! :D") + time.sleep(3) + print("Now I will disconnect :'(") + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + + simple_connect() +``` + +# Step 2a. Logging (synchronous) + + + +Alright, now taking a step up. We will now add to the script means to read out logging variables! + + + +## More imports + +Now we need to add several imports on the top of the script connect_log_param.py + + ``` + ... +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger + ``` + * LogConfig class is a representation of one log configuration that enables logging + from the Crazyflie + * The SyncLogger class provides synchronous access to log data from the Crazyflie. + +Also add the following underneath URI + +``` +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) +``` + +## Add logging config + +Now we are going to define the logging configuration. So add `lg_stab` in the `__main__` function : + ``` + ... + cflib.crtp.init_drivers(enable_debug_driver=False) + + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + ... + + ``` + +Here you will add the logs variables you would like to read out. If you are unsure how your variable is called, this can be checked by connecting to crazyflie to the cfclient and look at the log TOC tab. If the variables don't match, you get a `KeyError` (more on that later.) + + +## Make the logging function + +Use the same connect_log_param.py script, and add the following function above `simple_connect()` and underneath URI: + ``` +def simple_log(scf, logconf): + + ``` +Notice that now you will need to include the SyncCrazyflie instance (`scf`) and the logging configuration. + +Now the logging instances will be inserted by adding the following after you configured the lg_stab: + ``` + with SyncLogger(scf, lg_stab) as logger: + + for log_entry in logger: + + timestamp = log_entry[0] + data = log_entry[1] + logconf_name = log_entry[2] + + print('[%d][%s]: %s' % (timestamp, logconf_name, data)) + + break + + ``` + +## Test the script: + +First change the `simple_connect()` in _main_ in `simple_log(scf, lg_stab)`. Now run the script (`python3 connect_log_param.py`) like before. + +If everything is fine it should continuously print the logging variables, like this: + +`[1486704][]: {'stabilizer.roll': -0.054723262786865234, 'stabilizer.pitch': 0.006269464734941721, 'stabilizer.yaw': -0.008503230288624763}` + + +If you want to continuously receive the messages in the for loop, remove the `break`. You can stop the script with _ctrl+c_ + +If you are getting errors, check if your script corresponds with the full code: + ``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger + +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M/E7E7E7E7E7' + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + +def simple_log(scf, logconf): + + with SyncLogger(scf, lg_stab) as logger: + + for log_entry in logger: + + timestamp = log_entry[0] + data = log_entry[1] + logconf_name = log_entry[2] + + print('[%d][%s]: %s' % (timestamp, logconf_name, data)) + + break +... + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + + # simple_connect() + + simple_log(scf, lg_stab) + + ``` + +# Step 2b. Logging (Asynchronous) + +The logging we have showed you before was in a synchronous manner, so it reads out the logging in the function directly in the loop. Eventhough the SyncLogger does not take much time in general, for application purposes it might be preferred to receive the logging variables separately from this function, in a callback independently of the main loop-rate. + +Here we will explain how this asynchronous logging can be set up in the script. + +## Start a new function + +Above `simple_log(..)`, begin a new function: + +``` +def simple_log_async(scf, logconf): + cf = scf.cf + cf.log.add_config(logconf) +``` + +Here you add the logging configuration to to the logging framework of the crazyflie. It will check if the log configuration is part of the TOC, which is a list of all the logging variables defined in the crazyflie. You can test this out by changing one of the `lg_stab` variables to a completely bogus name like `'not.real'`. In this case you would receive the following message: + +`KeyError: 'Variable not.real not in TOC'` + +## Add a callback function + +First we will make the callback function like follows: +``` +def log_stab_callback(timestamp, data, logconf): + print('[%d][%s]: %s' % (timestamp, logconf.name, data)) +``` + +This callback will be called once the log variables have received it and prints the contents. The callback function added to the logging framework by adding it to the log config in `simple_log_async(..)`: + +``` + logconf.data_received_cb.add_callback(log_stab_callback) +``` + +Then the log configuration would need to be started manually, and then stopped after a few seconds: + +``` + logconf.start() + time.sleep(5) + logconf.stop() +``` + +# Run the script + +Make sure to replace the `simple_log(...)` to `simple_log_async(...)` in the `__main__` function. Run the script with `python3 connect_log_param.py` in a terminal and you should see several messages of the following: + +`[18101][Stabilizer]: {'stabilizer.roll': -174.58396911621094, 'stabilizer.pitch': 42.82120132446289, 'stabilizer.yaw': 166.29837036132812}` + +If something went wrong, check if your script corresponds to the this: + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger + +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M/E7E7E7E7E7' + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + +def log_stab_callback(timestamp, data, logconf): + print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + +def simple_log_async(scf, logconf): + cf = scf.cf + cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_stab_callback) + logconf.start() + time.sleep(5) + logconf.stop() + +(...) + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + + simple_log_async(scf, lg_stab) +``` + +# Step 3. Parameters + +Next to logging variables, it is possible to read and set parameters settings. That can be done within the cfclient, but here we will look at how we can change the state estimator method in the python script. + +First add the group parameter name just above `with SyncCrazyflie(...` in `__main__`. + +``` + group = "stabilizer" + name = "estimator" +``` + +## Start the function + +Start the following function above `def log_stab_callback(...)`: + +``` +def simple_param_async(scf, groupstr, namestr): + cf = scf.cf + full_name = groupstr+ "." +namestr +``` + +## Add the callback for parameters + +In a similar way as in the previous section for the Async logging, we are going to make a callback function for the parameters. Add the callback function above `def simple_param_async`: + +``` +def param_stab_est_callback(name, value): + print('The crazyflie has parameter ' + name + ' set at number: ' + value) +``` + +Then add the following to the `def simple_param_async(...)` function: +``` + cf.param.add_update_callback(group=groupstr, name=namestr, + cb=param_stab_est_callback) +``` + +If you would like to test out the script now already, replace `simple_log_async(...)` with `simple_param_async(group,name)` and runt the script. You can see that it will print out the variable name and value: +`The crazyflie has parameter stabilizer.estimator set at number: 1` + + +## Set a parameter + +Now we will set a variable in a parameter. Add the following to the `simple_param_async(...)` function: + +``` + time.sleep(1) + cf.param.set_value(full_name,2) +``` + +If you would run the script now you will also get this message: +`The crazyflie has parameter stabilizer.estimator set at number: 2` + +This means that the crazyflie has changed the parameter value to 2, which is another methods it uses for state estimation. This can also be done to change the color on the ledring, or to initiate the highlevel commander. + +What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) parameters, which can be checked by the parameter TOC in the CFclient. You can check this by changing the parameter name to group `'CPU' ` and name `flash'`. Then you will get the following error: + +`AttributeError: cpu.flash is read-only!` + +## Finishing and running the script + +It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the crazyflie, the parameter will still be set. Only after physcially restarting the crazyflie the parameter will reset to its default setting as defined in the firmware. + +So finish the `simple_param_async(...)` function by adding the next few lines: +``` + time.sleep(1) + cf.param.set_value(full_name,1) + time.sleep(1) +``` +Make sure the right function is in `__main__`. Run the script with `python3 connect_log_param.py` in a terminal and you should see the following: + +``` +Connecting to radio://0/80/2M/E7E7E7E7E7 +Connected to radio://0/80/2M/E7E7E7E7E7 +The crazyflie has parameter stabilizer.estimator set at number: 1 +The crazyflie has parameter stabilizer.estimator set at number: 2 +The crazyflie has parameter stabilizer.estimator set at number: 1 +``` + +# Finished and What's next? + +You're done and now you know how to connect to the crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the crazyflie fly by giving it setpoints which is one step closer to making your own application! \ No newline at end of file diff --git a/examples/step-by-step/connect_log_param.py b/examples/step-by-step/connect_log_param.py new file mode 100644 index 000000000..c91964452 --- /dev/null +++ b/examples/step-by-step/connect_log_param.py @@ -0,0 +1,112 @@ +# -*- 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. + +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger + +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M/E7E7E7E7E7' + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + +def param_stab_est_callback(name, value): + print('The crazyflie has parameter ' + name + ' set at number: ' + value) + +def simple_param_async(scf, groupstr, namestr): + cf = scf.cf + full_name = groupstr+ "." +namestr + + cf.param.add_update_callback(group=groupstr, name=namestr, + cb=param_stab_est_callback) + time.sleep(1) + cf.param.set_value(full_name,2) + time.sleep(1) + cf.param.set_value(full_name,1) + time.sleep(1) + +def log_stab_callback(timestamp, data, logconf): + print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + +def simple_log_async(scf, logconf): + cf = scf.cf + cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_stab_callback) + logconf.start() + time.sleep(5) + logconf.stop() + +def simple_log(scf, logconf): + + with SyncLogger(scf, lg_stab) as logger: + + for log_entry in logger: + + timestamp = log_entry[0] + data = log_entry[1] + logconf_name = log_entry[2] + + print('[%d][%s]: %s' % (timestamp, logconf_name, data)) + + break + + +def simple_connect(): + + print("Yeah, I'm connected! :D") + time.sleep(3) + print("Now I will disconnect :'(") + + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + lg_stab = LogConfig(name="Stabilizer", period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') + + group = "stabilizer" + name = "estimator" + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + + # simple_connect() + + # simple_log(scf, lg_stab) + + # simple_log_async(scf, lg_stab) + + simple_param_async(scf, group, name) + \ No newline at end of file From c404c6d47d57e2d99269cd140b25db45632749ad Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Jul 2020 13:51:32 +0200 Subject: [PATCH 043/861] (#159) forgot to style check the new script --- docs/step-by-step/connect_log_param.md | 2 +- examples/step-by-step/connect_log_param.py | 26 ++++++++++++---------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/docs/step-by-step/connect_log_param.md b/docs/step-by-step/connect_log_param.md index d02f9f1e6..bf48de01c 100644 --- a/docs/step-by-step/connect_log_param.md +++ b/docs/step-by-step/connect_log_param.md @@ -454,4 +454,4 @@ The crazyflie has parameter stabilizer.estimator set at number: 1 # Finished and What's next? -You're done and now you know how to connect to the crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the crazyflie fly by giving it setpoints which is one step closer to making your own application! \ No newline at end of file +You're done and now you know how to connect to the crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the crazyflie fly by giving it setpoints which is one step closer to making your own application! diff --git a/examples/step-by-step/connect_log_param.py b/examples/step-by-step/connect_log_param.py index c91964452..ed888c93c 100644 --- a/examples/step-by-step/connect_log_param.py +++ b/examples/step-by-step/connect_log_param.py @@ -23,15 +23,13 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. - import logging import time import cflib.crtp from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie - from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger # URI to the Crazyflie to connect to @@ -40,24 +38,28 @@ # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) + def param_stab_est_callback(name, value): print('The crazyflie has parameter ' + name + ' set at number: ' + value) + def simple_param_async(scf, groupstr, namestr): cf = scf.cf - full_name = groupstr+ "." +namestr + full_name = groupstr + '.' + namestr cf.param.add_update_callback(group=groupstr, name=namestr, - cb=param_stab_est_callback) + cb=param_stab_est_callback) time.sleep(1) - cf.param.set_value(full_name,2) + cf.param.set_value(full_name, 2) time.sleep(1) - cf.param.set_value(full_name,1) + cf.param.set_value(full_name, 1) time.sleep(1) + def log_stab_callback(timestamp, data, logconf): print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + def simple_log_async(scf, logconf): cf = scf.cf cf.log.add_config(logconf) @@ -65,7 +67,8 @@ def simple_log_async(scf, logconf): logconf.start() time.sleep(5) logconf.stop() - + + def simple_log(scf, logconf): with SyncLogger(scf, lg_stab) as logger: @@ -92,13 +95,13 @@ def simple_connect(): # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) - lg_stab = LogConfig(name="Stabilizer", period_in_ms=10) + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) lg_stab.add_variable('stabilizer.roll', 'float') lg_stab.add_variable('stabilizer.pitch', 'float') lg_stab.add_variable('stabilizer.yaw', 'float') - group = "stabilizer" - name = "estimator" + group = 'stabilizer' + name = 'estimator' with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -109,4 +112,3 @@ def simple_connect(): # simple_log_async(scf, lg_stab) simple_param_async(scf, group, name) - \ No newline at end of file From 3099b40d430200d41a61333db369562429079a40 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 29 Jul 2020 13:42:04 +0200 Subject: [PATCH 044/861] (#159) move sbs guide to userguide folder --- .../connect_log_param.md => user-guides/sbs_connect_log_param.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename docs/{step-by-step/connect_log_param.md => user-guides/sbs_connect_log_param.md} (100%) diff --git a/docs/step-by-step/connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md similarity index 100% rename from docs/step-by-step/connect_log_param.md rename to docs/user-guides/sbs_connect_log_param.md From 3bf5b47d85c614f4ce01a38517e0a4f83509863c Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 29 Jul 2020 14:33:53 +0200 Subject: [PATCH 045/861] (#159) add new tutorial about the motion commander and add links --- docs/_data/menu.yml | 2 + docs/user-guides/sbs_connect_log_param.md | 105 +++- docs/user-guides/sbs_motion_commander.md | 516 ++++++++++++++++++ ..._log_param.py => sbs_connect_log_param.py} | 0 examples/step-by-step/sbs_motion_commander.py | 128 +++++ 5 files changed, 734 insertions(+), 17 deletions(-) create mode 100644 docs/user-guides/sbs_motion_commander.md rename examples/step-by-step/{connect_log_param.py => sbs_connect_log_param.py} (100%) create mode 100644 examples/step-by-step/sbs_motion_commander.py diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index 8cc9f204e..fff7eef72 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -6,6 +6,8 @@ - title: User guides subs: - {page_id: python_api} + - {page_id: sbs_connect_log_param} + - {page_id: sbs_motion_commander} - title: Functional areas subs: - {page_id: crazyradio_lib} diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index bf48de01c..b1d53fac8 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -1,9 +1,10 @@ --- -title: Connecting, logging and parameters +title: "Step-by-Step: Connecting, logging and parameters" page_id: sbs_connect_log_param +redirect: /step-by-step/connect_log_param/ --- -On this step by step guide we will show you how to connect to your crazyflie through the crazyflie python library by a python script. This is the starting point for developing for the crazyflie for off-board enabled flight. +On this step by step guide we will show you how to connect to your Crazyflie through the Crazyflie python library by a python script. This is the starting point for developing for the Crazyflie for off-board enabled flight. # Prerequisites @@ -48,7 +49,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie ``` -* The cflib.crtp module is for scanning for crazyflies instances. +* The cflib.crtp module is for scanning for Crazyflies instances. * The Crazyflie class is used to easily connect/send/receive data from a Crazyflie. * The synCrazyflie class is a wrapper around the "normal" Crazyflie @@ -64,7 +65,7 @@ After these imports, start the script with: uri = 'radio://0/80/2M/E7E7E7E7E7' ``` -This is the radio uri of the crazyflie, and currently displaying the default. It should be probably fine but if you do not know what the uri of your crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) +This is the radio uri of the crazyflie, and currently displaying the default. It should be probably fine but if you do not know what the uri of your Crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) ## Main @@ -111,12 +112,12 @@ Now I will disconnect :'( ``` -The script connected with your crazyflie, synced and disconnected after a few seconds. You can see that the M3 LED is flashing yellow, which means that the crazyflie is connected to the script, but as soon as it leaves the `simple_connect()` function, the LED turns of. The Crazyflie is no longer connected +The script connected with your Crazyflie, synced and disconnected after a few seconds. You can see that the M4 LED is flashing yellow, which means that the Crazyflie is connected to the script, but as soon as it leaves the `simple_connect()` function, the LED turns of. The Crazyflie is no longer connected Not super exciting stuff yet but it is a great start! It is also a good test if everything is correctly configured on your system. -If you are getting an error, retrace your steps or check if your code matches the entire code underneath here. Also make sure your crazyflie is on and your crazyradio PA connected to you computer, and that the crazyflie is not connected to anything else (like the cfclient). If everything is peachy, please continue to the next part! +If you are getting an error, retrace your steps or check if your code matches the entire code underneath here. Also make sure your Crazyflie is on and your crazyradio PA connected to you computer, and that the Crazyflie is not connected to anything else (like the cfclient). If everything is peachy, please continue to the next part! ``` import logging @@ -191,7 +192,7 @@ Now we are going to define the logging configuration. So add `lg_stab` in the `_ ``` -Here you will add the logs variables you would like to read out. If you are unsure how your variable is called, this can be checked by connecting to crazyflie to the cfclient and look at the log TOC tab. If the variables don't match, you get a `KeyError` (more on that later.) +Here you will add the logs variables you would like to read out. If you are unsure how your variable is called, this can be checked by connecting to Crazyflie to the cfclient and look at the log TOC tab. If the variables don't match, you get a `KeyError` (more on that later.) ## Make the logging function @@ -288,7 +289,7 @@ Here we will explain how this asynchronous logging can be set up in the script. ## Start a new function -Above `simple_log(..)`, begin a new function: +Above `def simple_log(..)`, begin a new function: ``` def simple_log_async(scf, logconf): @@ -296,7 +297,7 @@ def simple_log_async(scf, logconf): cf.log.add_config(logconf) ``` -Here you add the logging configuration to to the logging framework of the crazyflie. It will check if the log configuration is part of the TOC, which is a list of all the logging variables defined in the crazyflie. You can test this out by changing one of the `lg_stab` variables to a completely bogus name like `'not.real'`. In this case you would receive the following message: +Here you add the logging configuration to to the logging framework of the Crazyflie. It will check if the log configuration is part of the TOC, which is a list of all the logging variables defined in the Crazyflie. You can test this out by changing one of the `lg_stab` variables to a completely bogus name like `'not.real'`. In this case you would receive the following message: `KeyError: 'Variable not.real not in TOC'` @@ -408,9 +409,12 @@ Then add the following to the `def simple_param_async(...)` function: ``` cf.param.add_update_callback(group=groupstr, name=namestr, cb=param_stab_est_callback) + time.sleep(1) + ``` +The sleep function is to give the script a bit more time to wait for the Crazyflies response and not lose the connection immediatly. -If you would like to test out the script now already, replace `simple_log_async(...)` with `simple_param_async(group,name)` and runt the script. You can see that it will print out the variable name and value: +If you would like to test out the script now already, replace `simple_log_async(...)` with `simple_param_async(scf, group, name)` and run the script. You can see that it will print out the variable name and value: `The crazyflie has parameter stabilizer.estimator set at number: 1` @@ -419,14 +423,13 @@ If you would like to test out the script now already, replace `simple_log_async( Now we will set a variable in a parameter. Add the following to the `simple_param_async(...)` function: ``` - time.sleep(1) cf.param.set_value(full_name,2) ``` If you would run the script now you will also get this message: `The crazyflie has parameter stabilizer.estimator set at number: 2` -This means that the crazyflie has changed the parameter value to 2, which is another methods it uses for state estimation. This can also be done to change the color on the ledring, or to initiate the highlevel commander. +This means that the Crazyflie has changed the parameter value to 2, which is another methods it uses for state estimation. This can also be done to change the color on the ledring, or to initiate the highlevel commander. What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) parameters, which can be checked by the parameter TOC in the CFclient. You can check this by changing the parameter name to group `'CPU' ` and name `flash'`. Then you will get the following error: @@ -434,15 +437,77 @@ What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) para ## Finishing and running the script -It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the crazyflie, the parameter will still be set. Only after physcially restarting the crazyflie the parameter will reset to its default setting as defined in the firmware. +It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physcially restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. So finish the `simple_param_async(...)` function by adding the next few lines: ``` - time.sleep(1) cf.param.set_value(full_name,1) time.sleep(1) ``` -Make sure the right function is in `__main__`. Run the script with `python3 connect_log_param.py` in a terminal and you should see the following: +Make sure the right function is in `__main__`. Check if your script corresponds with the code: + +``` +import logging +import time + +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 + +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M/E7E7E7E7E7' + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + + +def param_stab_est_callback(name, value): + print('The crazyflie has parameter ' + name + ' set at number: ' + value) + + +def simple_param_async(scf, groupstr, namestr): + cf = scf.cf + full_name = groupstr + '.' + namestr + + cf.param.add_update_callback(group=groupstr, name=namestr, + cb=param_stab_est_callback) + time.sleep(1) + cf.param.set_value(full_name, 2) + time.sleep(1) + cf.param.set_value(full_name, 1) + time.sleep(1) + + +def log_stab_callback(timestamp, data, logconf): + ... +def simple_log_async(scf, logconf): + ... +def simple_log(scf, logconf): + ... +def simple_connect(): + ... + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') + + group = 'stabilizer' + name = 'estimator' + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + simple_param_async(scf, group, name) +``` + + + +Run the script with `python3 connect_log_param.py` in a terminal and you should see the following: ``` Connecting to radio://0/80/2M/E7E7E7E7E7 @@ -452,6 +517,12 @@ The crazyflie has parameter stabilizer.estimator set at number: 2 The crazyflie has parameter stabilizer.estimator set at number: 1 ``` -# Finished and What's next? +You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. + + +# What's next? + + + Now you know how to connect to the Crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the Crazyflie fly by giving it setpoints which is one step closer to making your own application! -You're done and now you know how to connect to the crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the crazyflie fly by giving it setpoints which is one step closer to making your own application! + Go to the [next tutorial](/user-guides/sbs_motion_commander/) about the motion commander. diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md new file mode 100644 index 000000000..d2b573f30 --- /dev/null +++ b/docs/user-guides/sbs_motion_commander.md @@ -0,0 +1,516 @@ +--- +title: "Step-by-Step: Motion Commander" +page_id: sbs_motion_commander +--- + +Here we will go through step-by-step how to make your crazyflie move based on a motion script. For the first part of this tutorial, you just need the crazyflie and the flowdeck. For the second part, it would be handy to have the multiranger present. + +# Prerequisites + +We will assume that you already know this before you start with the tutorial: + +* Some basic experience with python +* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) and [the connecting, logging and parameters tutorial](/user-guides/sbs_connect_log_param/). + + +# Get the script started + +Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up an new python script called `motion_flying.py`. First you will start by adding the following import to the script: + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + +URI = 'radio://0/80/2M/E7E7E7E7E7' + +logging.basicConfig(level=logging.ERROR) + +if __name__ == '__main__': + + cflib.crtp.init_drivers(enable_debug_driver=False) + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: +``` + +This probably all looks pretty familiar, except for one thing line, namely: + +`from cflib.positioning.motion_commander import MotionCommander` + +This imports the motion commander, which is pretty much a wrapper around the position setpoint frame work of the crazyflie. You probably have unknowingly experienced this a when trying out the assist modes in this [tutorial with the flowdeck in the cfclient](https://www.bitcraze.io/documentation/tutorials/getting-started-with-flow-deck/) + +# Step 1: Security before flying + +Since this tutorial won't be a table top tutorial like last time, but an actual flying one, we need to put some securities in place. The flowdeck or any other positioning deck that you are using, should be correctly attached to the crazyflie. If it is not, it will try to fly anyway without a good position estimate and for sure is going to crash. + +We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...init_drivers(...)` in `__main__` +``` + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) +``` + +Above `__main__`, start a parameter callback function: +``` +def param_deck_flow(name, value): + if value == 1: + is_deck_attached = True + print("Deck is attached!") + else: + is_deck_attached = False + print("Deck is NOT attached!") +``` + +The `is_deck_attached` is a global variable which should be defined under `URI` + +``` +... +URI = 'radio://0/80/2M/E7E7E7E7E7' +is_deck_attached = False +``` + +Try to run the script now, and try to see if it is able to detect that the flowdeck (or any other positioning deck), is correctly attached. Also try to remove it to see if it can detect it missing as well. + +This is the full script as we are: +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + + +URI = 'radio://0/80/2M/E7E7E7E7E7' + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) + +def param_deck_flow(name, value): + global is_deck_attached + + print(value) + + if value: + is_deck_attached = True + print("Deck is attached!") + else: + is_deck_attached = False + print("Deck is NOT attached!") + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: +``` + +# Step 2: Take off function + +So now we are going to start up the SyncCrazyflie and start a function in the `__main__` function: + +``` + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + if is_deck_attached: + take_off_simple(scf) +``` +See that we are now using `is_deck_attached`? If this is false, the function will not be called and the crazyflie will not take off. + +Now make the function `take_off_simple(..)` above `__main__`, which will contain the motion commander instance. + +``` +def take_off_simple(scf): + with MotionCommander(scf) as mc: + time.sleep(3) +``` + +If you run the python script, you will see the crazyflie connect and immediately take off. After flying for 3 seconds it will land again. + +The reason for the crazyflie to immediately take off, is that the motion commander if intialized with a take off function that will already start sending position setpoints to the crazyflie. Once the script goes out of the instance, the motion commander instance will close with a land function. + +## Changing the height + +Currently the motion commander had 0.3 meters height as default but this can ofcourse be changed. + +Change the following line in `take_off_simple(...)`: +``` + with MotionCommander(scf) as mc: + mc.up(0.3) + time.sleep(3) +``` + +Run the script again. The crazyflie will first take off to 0.3 meters and then goes up for another 0.3 meters. + +The same can be achieved by adjusting the default_height of the motion_commander, which is what we will do for now on in this tutorial. Remove the `mc.up(0.3)` and replace the motion commander line with +``` + with MotionCommander(scf, default_height = DEFAULT_HEIGHT) as mc: +``` + +Add the variable underneath `URI`: +``` +DEFAULT_HEIGHT = 0.6 +``` + + +Double check if your script is the same as beneath and run it again to check + + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 + +is_deck_attached = False +logging.basicConfig(level=logging.ERROR) + +def take_off_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(3) + mc.stop() + +def param_deck_flow(name, value): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + if is_deck_attached: + take_off_simple(scf) +``` + +# Step 3 Go Forward, Turn and Go back + +So now we know how to take off, so the second step is to move in a direction! Start a new function above `def take_off_simple(scf)`: + +``` +def move_linear_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.back(0.5) + time.sleep(1) +``` + +If you replace `take_off_simple(scf)` in `__main__` with `move_linear_simple(scf)`, try to run the script. + +You will see the crazyflie take off, fly 0.5 m forward, fly backwards and land again. + +Now we are going to add a turn into it. Replace the content under motion commander in `move_linear_simple(..)` with the following: + +``` + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.turn_left(180) + time.sleep(1) + mc.forward(0.5) + time.sleep(1) +``` + +Try to run the script again. Now you can see the crazyflie take off, go forward, turn 180 degrees and go forward again to its initial position. The `mc.back()` needed to be replaced with the forward since the motion commander sends the velocity setpoints in the __body fixed coordinated__ system. This means that the commands forward will go forward to whereever the current heading (the front) of the crazyflie points to. + +Double check if your code code is still correct: + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +is_deck_attached = False +DEFAULT_HEIGHT = 0.5 +logging.basicConfig(level=logging.ERROR) + +def move_linear_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.turn_left(180) + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + +def take_off_simple(scf): + ... +def param_deck_flow(name, value): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + if is_deck_attached: + move_linear_simple(scf) +``` + +# Step 4: Logging while flying + +When the motion commander commands have been executed, the script stops and the crazyflie lands... however that is a bit boring. Maybe you would like for it to keep flying and responding certain elements in the mean time! + +Let's integrate some logging to this as well. Add the following log config right into `__main__` under `SyncCrazyflie` + + +``` + lg_stab = LogConfig(name='Position', period_in_ms=10) + lg_stab.add_variable('stateEstimate.x', 'float') + lg_stab.add_variable('stateEstimate.y', 'float') + cf = scf.cf + cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + + move_linear_simple(scf) + + logconf.stop() +``` + +Don't forget to add `from cflib.crazyflie.log import LogConfig` at the imports (we don't need the sync logger since we are going to use the callback). Make the function `log_pos_callback` above param_deck_flow: + + + +``` +def log_pos_callback(timestamp, data, logconf): + print(data) +``` + +Make global variable which is a list called `position_estimate` and fill this in in the logging callback function with the x and y position. The `data` is a dict. + +Just double check that everything has been implemented correctly and then run the script. You will see the same behavior as with the previous step but then with the position estimated printed at the same time. + +*You can replace the print function in the callback with a plotter if you would like to try that out, like with the python lib matplotlib :)* +``` +... +from cflib.crazyflie.log import LogConfig + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) +position_estimate = [0,0] + + +def move_linear_simple(scf): + ... + +def take_off_simple(scf): + ... + +def log_pos_callback(timestamp, data, logconf): + print(data) + global postion_estimate + + position_estimate[0] = data["stateEstimate.x"] + position_estimate[1] = data["stateEstimate.y"] + +def param_deck_flow(name, value): +... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') + scf.cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + + move_linear_simple(scf) + + logconf.stop() + +``` + +# Step 5: Combine logging and motion commander + +There is a reason why we put the position_estimate to catch the positions from the log, since we would like to now do something with it! + +## Back and forth with limits +Lets start with a new function above `move_in_box_limit(scf)`: + +``` +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + while (1): + + time.sleep(0.1) + +``` + +If you would run this (don't forget to replace `move_linear_simple()` in `__main`), you see the crazyflie take off but it will stay in the air. A keyboard interrupt (ctrl+c) will stop the script and make the crazyflie land again. + +Now we will add some behavior in the while loop: + +``` +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + start_forward() + + while (1): + if position_estimate[0] > BOX_LIMIT: + mc.start_back() + elif position_estimate[0] < -BOX_LIMIT + mc.start_forward() + time.sleep(0.1) + +``` + +Add `BOX_LIMIT = 0.5` underneath the definition of the `DEFAULT_HEIGHT = 0.5`. + +Run the script and you will see that the crazyflie will start moving back and forth until you hit ctrl+c. It changes its command based on the logging input, which is the state estimate x and y position. Once it indicates that it reached the border of the 'virtual' limit, it will change it's direction. + +You probably also noticed that we are using `mc.start_back()` and `mc.start_forward()` instead of the `mc.forward(0.5)` and `mc.back(0.5)` used in the previous steps. The main difference is that the *mc.forward* and *mc.back* are **blocking** functions that won't continue the code until the distance has been reached. The *mc.start_...()* will start the crazyflie in a direction and will not stop until the `mc.stop()` is given, which is given automatically when the motion commander instance is exited. That is why this is nice functions to use in reactive scenarios like these. + +## Bouncing in a bounding box + +Let's take it up a notch! Replace the content in the while loop with the following: +``` + body_x_cmd = 0.2; + body_y_cmd = 0.1; + max_vel = 0.2; + + while (1): + if position_estimate[0] > BOX_LIMIT: + body_x_cmd=-max_vel + elif position_estimate[0] < -BOX_LIMIT: + body_x_cmd=max_vel + if position_estimate[1] > BOX_LIMIT: + body_y_cmd=-max_vel + elif position_estimate[1] < -BOX_LIMIT: + body_y_cmd=max_vel + + mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) + + time.sleep(0.1) +``` + +This will now start a linear motion into a certain direction, and makes the Crazyflie bounce around in a virtual box of which the size is indicated by 'BOX_LIMIT'. So before you fly make sure that you pick a box_limit small enough so that it able to fit in your flying area. + +**Note**: if you are using the flowdeck, it might be that the orientation of this box will seem to change. This is due to that the flowdeck is not able to provide an absolute heading estimate, which will be only based on gyroscope measurements. This will drift over time, which is accelerated if you incorperate many turns in your application. There are also reports that happens quickly when the crazyflie is still on the ground. This should not happen with MoCap or the lighthouse deck. + + +Check out if your code still matches and run the script! +``` +import logging +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.crazyflie.log import LogConfig + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) + +position_estimate = [0,0] + +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + body_x_cmd = 0.2; + body_y_cmd = 0.1; + max_vel = 0.2; + + while (1): + if position_estimate[0] > BOX_LIMIT: + body_x_cmd=-max_vel + elif position_estimate[0] < -BOX_LIMIT: + body_x_cmd=max_vel + if position_estimate[1] > BOX_LIMIT: + body_y_cmd=-max_vel + elif position_estimate[1] < -BOX_LIMIT: + body_y_cmd=max_vel + + mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) + + time.sleep(0.1) + + + +def move_linear_simple(scf): + ... +def take_off_simple(scf): + ... +def log_pos_callback(timestamp, data, logconf): + ... +def param_deck_flow(name, value): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + time.sleep(1) + + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') + scf.cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + move_box_limit(scf) + logconf.stop() + +``` + +You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. + + +# What is next ? + +Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. \ No newline at end of file diff --git a/examples/step-by-step/connect_log_param.py b/examples/step-by-step/sbs_connect_log_param.py similarity index 100% rename from examples/step-by-step/connect_log_param.py rename to examples/step-by-step/sbs_connect_log_param.py diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py new file mode 100644 index 000000000..d4f52456c --- /dev/null +++ b/examples/step-by-step/sbs_motion_commander.py @@ -0,0 +1,128 @@ +# -*- 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. + +import logging +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.crazyflie.log import LogConfig + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) + +position_estimate = [0,0] + +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + body_x_cmd = 0.2; + body_y_cmd = 0.1; + max_vel = 0.2; + + while (1): + '''if position_estimate[0] > BOX_LIMIT: + mc.start_back() + elif position_estimate[0] < -BOX_LIMIT + mc.start_forward() + ''' + + if position_estimate[0] > BOX_LIMIT: + body_x_cmd=-max_vel + elif position_estimate[0] < -BOX_LIMIT: + body_x_cmd=max_vel + if position_estimate[1] > BOX_LIMIT: + body_y_cmd=-max_vel + elif position_estimate[1] < -BOX_LIMIT: + body_y_cmd=max_vel + + mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) + + time.sleep(0.1) + + + +def move_linear_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.turn_left(180) + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + +def take_off_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(3) + mc.stop() + +def log_pos_callback(timestamp, data, logconf): + print(data) + global postion_estimate + position_estimate[0] = data["stateEstimate.x"] + position_estimate[1] = data["stateEstimate.y"] + +def param_deck_flow(name, value): + global is_deck_attached + print(value) + if value: + is_deck_attached = True + print("Deck is attached!") + else: + is_deck_attached = False + print("Deck is NOT attached!") + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + time.sleep(1) + + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') + scf.cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + + #take_off_simple(scf) + #move_linear_simple(scf) + move_box_limit(scf) + logconf.stop() \ No newline at end of file From a212e92cf572b81ae98540d270a8433390d778db Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 29 Jul 2020 14:41:18 +0200 Subject: [PATCH 046/861] (#159) fix style --- docs/user-guides/sbs_motion_commander.md | 2 +- examples/step-by-step/sbs_motion_commander.py | 46 ++++++++++--------- 2 files changed, 25 insertions(+), 23 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index d2b573f30..2f49abac5 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -513,4 +513,4 @@ You're done! The full code of this tutorial can be found in the example/step-by- # What is next ? -Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. \ No newline at end of file +Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index d4f52456c..4acad4784 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -23,15 +23,14 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. - import logging import time import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander -from cflib.crazyflie.log import LogConfig URI = 'radio://0/80/2M/E7E7E7E7E7' @@ -42,13 +41,14 @@ logging.basicConfig(level=logging.ERROR) -position_estimate = [0,0] +position_estimate = [0, 0] + def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: - body_x_cmd = 0.2; - body_y_cmd = 0.1; - max_vel = 0.2; + body_x_cmd = 0.2 + body_y_cmd = 0.1 + max_vel = 0.2 while (1): '''if position_estimate[0] > BOX_LIMIT: @@ -58,18 +58,17 @@ def move_box_limit(scf): ''' if position_estimate[0] > BOX_LIMIT: - body_x_cmd=-max_vel + body_x_cmd = -max_vel elif position_estimate[0] < -BOX_LIMIT: - body_x_cmd=max_vel + body_x_cmd = max_vel if position_estimate[1] > BOX_LIMIT: - body_y_cmd=-max_vel + body_y_cmd = -max_vel elif position_estimate[1] < -BOX_LIMIT: - body_y_cmd=max_vel + body_y_cmd = max_vel mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) - - time.sleep(0.1) + time.sleep(0.1) def move_linear_simple(scf): @@ -82,35 +81,38 @@ def move_linear_simple(scf): mc.forward(0.5) time.sleep(1) + def take_off_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) mc.stop() + def log_pos_callback(timestamp, data, logconf): print(data) global postion_estimate - position_estimate[0] = data["stateEstimate.x"] - position_estimate[1] = data["stateEstimate.y"] + position_estimate[0] = data['stateEstimate.x'] + position_estimate[1] = data['stateEstimate.y'] + def param_deck_flow(name, value): global is_deck_attached print(value) if value: is_deck_attached = True - print("Deck is attached!") + print('Deck is attached!') else: is_deck_attached = False - print("Deck is NOT attached!") + print('Deck is NOT attached!') + if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - scf.cf.param.add_update_callback(group="deck", name="bcFlow2", - cb=param_deck_flow) + scf.cf.param.add_update_callback(group='deck', name='bcFlow2', + cb=param_deck_flow) time.sleep(1) logconf = LogConfig(name='Position', period_in_ms=10) @@ -122,7 +124,7 @@ def param_deck_flow(name, value): if is_deck_attached: logconf.start() - #take_off_simple(scf) - #move_linear_simple(scf) + # take_off_simple(scf) + # move_linear_simple(scf) move_box_limit(scf) - logconf.stop() \ No newline at end of file + logconf.stop() From 4374a8a3c183bc595bb3655a3f099c0e286105c4 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 31 Jul 2020 12:53:02 +0200 Subject: [PATCH 047/861] (#159) fixed mistakes of old code reminents --- docs/user-guides/sbs_motion_commander.md | 233 +++++++++++++----- examples/step-by-step/sbs_motion_commander.py | 4 +- 2 files changed, 177 insertions(+), 60 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 2f49abac5..ce3f0453b 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -46,21 +46,27 @@ This imports the motion commander, which is pretty much a wrapper around the pos Since this tutorial won't be a table top tutorial like last time, but an actual flying one, we need to put some securities in place. The flowdeck or any other positioning deck that you are using, should be correctly attached to the crazyflie. If it is not, it will try to fly anyway without a good position estimate and for sure is going to crash. -We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...init_drivers(...)` in `__main__` +We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...SyncCrazyflie(...)` in `__main__` ``` - cf.param.add_update_callback(group="deck", name="bcFlow2", + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group="deck", name="bcFlow2", cb=param_deck_flow) + time.sleep(1) + ``` Above `__main__`, start a parameter callback function: ``` def param_deck_flow(name, value): - if value == 1: + global is_deck_attached + print(value) + if value: is_deck_attached = True - print("Deck is attached!") + print('Deck is attached!') else: is_deck_attached = False - print("Deck is NOT attached!") + print('Deck is NOT attached!') ``` The `is_deck_attached` is a global variable which should be defined under `URI` @@ -83,7 +89,6 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander - URI = 'radio://0/80/2M/E7E7E7E7E7' is_deck_attached = False @@ -92,23 +97,24 @@ logging.basicConfig(level=logging.ERROR) def param_deck_flow(name, value): global is_deck_attached - print(value) - if value: is_deck_attached = True - print("Deck is attached!") + print('Deck is attached!') else: is_deck_attached = False - print("Deck is NOT attached!") + print('Deck is NOT attached!') + if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) - cf.param.add_update_callback(group="deck", name="bcFlow2", - cb=param_deck_flow) - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group='deck', name='bcFlow2', + cb=param_deck_flow) + time.sleep(1) + ``` # Step 2: Take off function @@ -155,7 +161,7 @@ The same can be achieved by adjusting the default_height of the motion_commander Add the variable underneath `URI`: ``` -DEFAULT_HEIGHT = 0.6 +DEFAULT_HEIGHT = 0.5 ``` @@ -176,6 +182,7 @@ URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 is_deck_attached = False + logging.basicConfig(level=logging.ERROR) def take_off_simple(scf): @@ -189,12 +196,15 @@ def param_deck_flow(name, value): if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) - cf.param.add_update_callback(group="deck", name="bcFlow2", - cb=param_deck_flow) - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group='deck', name='bcFlow2', + cb=param_deck_flow) + time.sleep(1) + if is_deck_attached: take_off_simple(scf) + ``` # Step 3 Go Forward, Turn and Go back @@ -242,10 +252,13 @@ from cflib.positioning.motion_commander import MotionCommander URI = 'radio://0/80/2M/E7E7E7E7E7' -is_deck_attached = False DEFAULT_HEIGHT = 0.5 + +is_deck_attached = False + logging.basicConfig(level=logging.ERROR) + def move_linear_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(1) @@ -256,18 +269,23 @@ def move_linear_simple(scf): mc.forward(0.5) time.sleep(1) + def take_off_simple(scf): ... + def param_deck_flow(name, value): - ... + ... + if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) - cf.param.add_update_callback(group="deck", name="bcFlow2", - cb=param_deck_flow) - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group='deck', name='bcFlow2', + cb=param_deck_flow) + time.sleep(1) + if is_deck_attached: move_linear_simple(scf) ``` @@ -295,7 +313,7 @@ Let's integrate some logging to this as well. Add the following log config right logconf.stop() ``` -Don't forget to add `from cflib.crazyflie.log import LogConfig` at the imports (we don't need the sync logger since we are going to use the callback). Make the function `log_pos_callback` above param_deck_flow: +Don't forget to add `from cflib.crazyflie.log import LogConfig` at the imports (we don't need the sync logger since we are going to use the callback). Make the function `log_pos_callback` above param_deck_flow: @@ -304,50 +322,63 @@ def log_pos_callback(timestamp, data, logconf): print(data) ``` -Make global variable which is a list called `position_estimate` and fill this in in the logging callback function with the x and y position. The `data` is a dict. +NOW: Make global variable which is a list called `position_estimate` and fill this in in the logging callback function with the x and y position. The `data` is a dict structure. Just double check that everything has been implemented correctly and then run the script. You will see the same behavior as with the previous step but then with the position estimated printed at the same time. *You can replace the print function in the callback with a plotter if you would like to try that out, like with the python lib matplotlib :)* ``` -... +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 -BOX_LIMIT = 0.5 is_deck_attached = False logging.basicConfig(level=logging.ERROR) -position_estimate = [0,0] +position_estimate = [0, 0] def move_linear_simple(scf): - ... + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.turn_left(180) + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + def take_off_simple(scf): ... def log_pos_callback(timestamp, data, logconf): print(data) - global postion_estimate + global position_estimate + position_estimate[0] = data['stateEstimate.x'] + position_estimate[1] = data['stateEstimate.y'] - position_estimate[0] = data["stateEstimate.x"] - position_estimate[1] = data["stateEstimate.y"] def param_deck_flow(name, value): -... + ... if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) - cf.param.add_update_callback(group="deck", name="bcFlow2", - cb=param_deck_flow) - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + scf.cf.param.add_update_callback(group='deck', name='bcFlow2', + cb=param_deck_flow) + time.sleep(1) logconf = LogConfig(name='Position', period_in_ms=10) logconf.add_variable('stateEstimate.x', 'float') @@ -359,9 +390,9 @@ if __name__ == '__main__': logconf.start() move_linear_simple(scf) - logconf.stop() + ``` # Step 5: Combine logging and motion commander @@ -374,13 +405,14 @@ Lets start with a new function above `move_in_box_limit(scf)`: ``` def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + while (1): time.sleep(0.1) ``` -If you would run this (don't forget to replace `move_linear_simple()` in `__main`), you see the crazyflie take off but it will stay in the air. A keyboard interrupt (ctrl+c) will stop the script and make the crazyflie land again. +If you would run this (don't forget to replace `move_linear_simple()` in `__main__`), you see the crazyflie take off but it will stay in the air. A keyboard interrupt (ctrl+c) will stop the script and make the crazyflie land again. Now we will add some behavior in the while loop: @@ -392,7 +424,7 @@ def move_box_limit(scf): while (1): if position_estimate[0] > BOX_LIMIT: mc.start_back() - elif position_estimate[0] < -BOX_LIMIT + elif position_estimate[0] < -BOX_LIMIT: mc.start_forward() time.sleep(0.1) @@ -404,6 +436,73 @@ Run the script and you will see that the crazyflie will start moving back and fo You probably also noticed that we are using `mc.start_back()` and `mc.start_forward()` instead of the `mc.forward(0.5)` and `mc.back(0.5)` used in the previous steps. The main difference is that the *mc.forward* and *mc.back* are **blocking** functions that won't continue the code until the distance has been reached. The *mc.start_...()* will start the crazyflie in a direction and will not stop until the `mc.stop()` is given, which is given automatically when the motion commander instance is exited. That is why this is nice functions to use in reactive scenarios like these. +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) + +position_estimate = [0, 0] + + +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + body_x_cmd = 0.2 + body_y_cmd = 0.1 + max_vel = 0.2 + + while (1): + if position_estimate[0] > BOX_LIMIT: + mc.start_back() + elif position_estimate[0] < -BOX_LIMIT: + mc.start_forward() + +def move_linear_simple(scf): + ... + +def take_off_simple(scf): + ... + +def log_pos_callback(timestamp, data, logconf): + ... + +def param_deck_flow(name, value): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group='deck', name='bcFlow2', + cb=param_deck_flow) + time.sleep(1) + + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') + scf.cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + move_box_limit(scf) + logconf.stop() +``` + ## Bouncing in a bounding box Let's take it up a notch! Replace the content in the while loop with the following: @@ -429,19 +528,19 @@ Let's take it up a notch! Replace the content in the while loop with the followi This will now start a linear motion into a certain direction, and makes the Crazyflie bounce around in a virtual box of which the size is indicated by 'BOX_LIMIT'. So before you fly make sure that you pick a box_limit small enough so that it able to fit in your flying area. -**Note**: if you are using the flowdeck, it might be that the orientation of this box will seem to change. This is due to that the flowdeck is not able to provide an absolute heading estimate, which will be only based on gyroscope measurements. This will drift over time, which is accelerated if you incorperate many turns in your application. There are also reports that happens quickly when the crazyflie is still on the ground. This should not happen with MoCap or the lighthouse deck. +**Note**: if you are using the flowdeck, it might be that the orientation of this box will seem to change. This is due to that the flowdeck is not able to provide an absolute heading estimate, which will be only based on gyroscope measurements. This will drift over time, which is accelerated if you incorporate many turns in your application. There are also reports that happens quickly when the crazyflie is still on the ground. This should not happen with MoCap or the lighthouse deck. -Check out if your code still matches and run the script! +Check out if your code still matches the full code and run the script! ``` import logging import time import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander -from cflib.crazyflie.log import LogConfig URI = 'radio://0/80/2M/E7E7E7E7E7' @@ -452,47 +551,66 @@ is_deck_attached = False logging.basicConfig(level=logging.ERROR) -position_estimate = [0,0] +position_estimate = [0, 0] + def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: - body_x_cmd = 0.2; - body_y_cmd = 0.1; - max_vel = 0.2; + body_x_cmd = 0.2 + body_y_cmd = 0.1 + max_vel = 0.2 while (1): + #if position_estimate[0] > BOX_LIMIT: + # mc.start_back() + #elif position_estimate[0] < -BOX_LIMIT: + # mc.start_forward() + if position_estimate[0] > BOX_LIMIT: - body_x_cmd=-max_vel + body_x_cmd = -max_vel elif position_estimate[0] < -BOX_LIMIT: - body_x_cmd=max_vel + body_x_cmd = max_vel if position_estimate[1] > BOX_LIMIT: - body_y_cmd=-max_vel + body_y_cmd = -max_vel elif position_estimate[1] < -BOX_LIMIT: - body_y_cmd=max_vel + body_y_cmd = max_vel mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) - - time.sleep(0.1) + time.sleep(0.1) def move_linear_simple(scf): ... + def take_off_simple(scf): ... + def log_pos_callback(timestamp, data, logconf): - ... + print(data) + global position_estimate + position_estimate[0] = data['stateEstimate.x'] + position_estimate[1] = data['stateEstimate.y'] + + def param_deck_flow(name, value): - ... + global is_deck_attached + print(value) + if value: + is_deck_attached = True + print('Deck is attached!') + else: + is_deck_attached = False + print('Deck is NOT attached!') + if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - scf.cf.param.add_update_callback(group="deck", name="bcFlow2", - cb=param_deck_flow) + scf.cf.param.add_update_callback(group='deck', name='bcFlow2', + cb=param_deck_flow) time.sleep(1) logconf = LogConfig(name='Position', period_in_ms=10) @@ -504,8 +622,7 @@ if __name__ == '__main__': if is_deck_attached: logconf.start() move_box_limit(scf) - logconf.stop() - + logconf.stop() ``` You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index 4acad4784..552455455 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -53,7 +53,7 @@ def move_box_limit(scf): while (1): '''if position_estimate[0] > BOX_LIMIT: mc.start_back() - elif position_estimate[0] < -BOX_LIMIT + elif position_estimate[0] < -BOX_LIMIT: mc.start_forward() ''' @@ -90,7 +90,7 @@ def take_off_simple(scf): def log_pos_callback(timestamp, data, logconf): print(data) - global postion_estimate + global position_estimate position_estimate[0] = data['stateEstimate.x'] position_estimate[1] = data['stateEstimate.y'] From 69d2a88e2eb9cb5d78cfb92e4fd6518bf897611a Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 3 Aug 2020 14:19:35 +0200 Subject: [PATCH 048/861] #163 Converted urls to github compliant format --- docs/user-guides/sbs_connect_log_param.md | 14 +++++----- docs/user-guides/sbs_motion_commander.md | 34 +++++++++++------------ 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index b1d53fac8..341b9df6d 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -27,7 +27,7 @@ This should have been installed if you installed the cfclient already (on a linu ## Begin the python script -Open up a python script anywhere that is convenient for you. We use Visual Studio code ourselves but you can use the Python editor IDLE3 if you want. +Open up a python script anywhere that is convenient for you. We use Visual Studio code ourselves but you can use the Python editor IDLE3 if you want. * For python editor: select file->new * For VS code: select file-> new file @@ -248,9 +248,9 @@ uri = 'radio://0/80/2M/E7E7E7E7E7' # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) - + def simple_log(scf, logconf): - + with SyncLogger(scf, lg_stab) as logger: for log_entry in logger: @@ -278,7 +278,7 @@ if __name__ == '__main__': # simple_connect() simple_log(scf, lg_stab) - + ``` # Step 2b. Logging (Asynchronous) @@ -429,7 +429,7 @@ Now we will set a variable in a parameter. Add the following to the `simple_para If you would run the script now you will also get this message: `The crazyflie has parameter stabilizer.estimator set at number: 2` -This means that the Crazyflie has changed the parameter value to 2, which is another methods it uses for state estimation. This can also be done to change the color on the ledring, or to initiate the highlevel commander. +This means that the Crazyflie has changed the parameter value to 2, which is another methods it uses for state estimation. This can also be done to change the color on the ledring, or to initiate the highlevel commander. What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) parameters, which can be checked by the parameter TOC in the CFclient. You can check this by changing the parameter name to group `'CPU' ` and name `flash'`. Then you will get the following error: @@ -437,7 +437,7 @@ What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) para ## Finishing and running the script -It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physcially restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. +It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physcially restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. So finish the `simple_param_async(...)` function by adding the next few lines: ``` @@ -525,4 +525,4 @@ You're done! The full code of this tutorial can be found in the example/step-by- Now you know how to connect to the Crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the Crazyflie fly by giving it setpoints which is one step closer to making your own application! - Go to the [next tutorial](/user-guides/sbs_motion_commander/) about the motion commander. + Go to the [next tutorial](/docs/user-guides/sbs_motion_commander.md) about the motion commander. diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index ce3f0453b..66058942d 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -10,7 +10,7 @@ Here we will go through step-by-step how to make your crazyflie move based on a We will assume that you already know this before you start with the tutorial: * Some basic experience with python -* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) and [the connecting, logging and parameters tutorial](/user-guides/sbs_connect_log_param/). +* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) and [the connecting, logging and parameters tutorial](/docs/user-guides/sbs_connect_log_param.md). # Get the script started @@ -36,7 +36,7 @@ if __name__ == '__main__': with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: ``` -This probably all looks pretty familiar, except for one thing line, namely: +This probably all looks pretty familiar, except for one thing line, namely: `from cflib.positioning.motion_commander import MotionCommander` @@ -77,7 +77,7 @@ URI = 'radio://0/80/2M/E7E7E7E7E7' is_deck_attached = False ``` -Try to run the script now, and try to see if it is able to detect that the flowdeck (or any other positioning deck), is correctly attached. Also try to remove it to see if it can detect it missing as well. +Try to run the script now, and try to see if it is able to detect that the flowdeck (or any other positioning deck), is correctly attached. Also try to remove it to see if it can detect it missing as well. This is the full script as we are: ``` @@ -123,11 +123,11 @@ So now we are going to start up the SyncCrazyflie and start a function in the `_ ``` with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - + if is_deck_attached: take_off_simple(scf) ``` -See that we are now using `is_deck_attached`? If this is false, the function will not be called and the crazyflie will not take off. +See that we are now using `is_deck_attached`? If this is false, the function will not be called and the crazyflie will not take off. Now make the function `take_off_simple(..)` above `__main__`, which will contain the motion commander instance. @@ -137,9 +137,9 @@ def take_off_simple(scf): time.sleep(3) ``` -If you run the python script, you will see the crazyflie connect and immediately take off. After flying for 3 seconds it will land again. +If you run the python script, you will see the crazyflie connect and immediately take off. After flying for 3 seconds it will land again. -The reason for the crazyflie to immediately take off, is that the motion commander if intialized with a take off function that will already start sending position setpoints to the crazyflie. Once the script goes out of the instance, the motion commander instance will close with a land function. +The reason for the crazyflie to immediately take off, is that the motion commander if intialized with a take off function that will already start sending position setpoints to the crazyflie. Once the script goes out of the instance, the motion commander instance will close with a land function. ## Changing the height @@ -152,9 +152,9 @@ Change the following line in `take_off_simple(...)`: time.sleep(3) ``` -Run the script again. The crazyflie will first take off to 0.3 meters and then goes up for another 0.3 meters. +Run the script again. The crazyflie will first take off to 0.3 meters and then goes up for another 0.3 meters. -The same can be achieved by adjusting the default_height of the motion_commander, which is what we will do for now on in this tutorial. Remove the `mc.up(0.3)` and replace the motion commander line with +The same can be achieved by adjusting the default_height of the motion_commander, which is what we will do for now on in this tutorial. Remove the `mc.up(0.3)` and replace the motion commander line with ``` with MotionCommander(scf, default_height = DEFAULT_HEIGHT) as mc: ``` @@ -221,7 +221,7 @@ def move_linear_simple(scf): time.sleep(1) ``` -If you replace `take_off_simple(scf)` in `__main__` with `move_linear_simple(scf)`, try to run the script. +If you replace `take_off_simple(scf)` in `__main__` with `move_linear_simple(scf)`, try to run the script. You will see the crazyflie take off, fly 0.5 m forward, fly backwards and land again. @@ -310,7 +310,7 @@ Let's integrate some logging to this as well. Add the following log config right move_linear_simple(scf) - logconf.stop() + logconf.stop() ``` Don't forget to add `from cflib.crazyflie.log import LogConfig` at the imports (we don't need the sync logger since we are going to use the callback). Make the function `log_pos_callback` above param_deck_flow: @@ -322,7 +322,7 @@ def log_pos_callback(timestamp, data, logconf): print(data) ``` -NOW: Make global variable which is a list called `position_estimate` and fill this in in the logging callback function with the x and y position. The `data` is a dict structure. +NOW: Make global variable which is a list called `position_estimate` and fill this in in the logging callback function with the x and y position. The `data` is a dict structure. Just double check that everything has been implemented correctly and then run the script. You will see the same behavior as with the previous step but then with the position estimated printed at the same time. @@ -412,7 +412,7 @@ def move_box_limit(scf): ``` -If you would run this (don't forget to replace `move_linear_simple()` in `__main__`), you see the crazyflie take off but it will stay in the air. A keyboard interrupt (ctrl+c) will stop the script and make the crazyflie land again. +If you would run this (don't forget to replace `move_linear_simple()` in `__main__`), you see the crazyflie take off but it will stay in the air. A keyboard interrupt (ctrl+c) will stop the script and make the crazyflie land again. Now we will add some behavior in the while loop: @@ -430,7 +430,7 @@ def move_box_limit(scf): ``` -Add `BOX_LIMIT = 0.5` underneath the definition of the `DEFAULT_HEIGHT = 0.5`. +Add `BOX_LIMIT = 0.5` underneath the definition of the `DEFAULT_HEIGHT = 0.5`. Run the script and you will see that the crazyflie will start moving back and forth until you hit ctrl+c. It changes its command based on the logging input, which is the state estimate x and y position. Once it indicates that it reached the border of the 'virtual' limit, it will change it's direction. @@ -522,7 +522,7 @@ Let's take it up a notch! Replace the content in the while loop with the followi body_y_cmd=max_vel mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) - + time.sleep(0.1) ``` @@ -565,7 +565,7 @@ def move_box_limit(scf): # mc.start_back() #elif position_estimate[0] < -BOX_LIMIT: # mc.start_forward() - + if position_estimate[0] > BOX_LIMIT: body_x_cmd = -max_vel elif position_estimate[0] < -BOX_LIMIT: @@ -630,4 +630,4 @@ You're done! The full code of this tutorial can be found in the example/step-by- # What is next ? -Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. +Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. From 05e1020c489f99a25c113af1cbf18d9a5a7aff5d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 3 Aug 2020 16:49:54 +0200 Subject: [PATCH 049/861] #163 Corrected links to the firmware repo --- docs/user-guides/python_api.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 85beed309..a45eccbb6 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -11,8 +11,8 @@ the API that it implements. If you are interested in more details look in the PyDoc in the code or: - Communication protocol for - [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/ctrp_log/) or - [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/ctrp_parameters/) + [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_log/) or + [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_parameters/) Structure of the library ======================== From 9984c167ca48ab5f60c8eef87d8d1b671d5ef6f8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 5 Aug 2020 12:43:08 +0200 Subject: [PATCH 050/861] #145 Updated documentation for serial driver --- docs/installation/install_from_source.md | 10 +++-- docs/user-guides/python_api.md | 54 +++++++++++------------- 2 files changed, 31 insertions(+), 33 deletions(-) diff --git a/docs/installation/install_from_source.md b/docs/installation/install_from_source.md index 8c3cacf05..88c7ca297 100644 --- a/docs/installation/install_from_source.md +++ b/docs/installation/install_from_source.md @@ -1,11 +1,11 @@ --- title: Installing from source -page_id: install_from_source +page_id: install_from_source --- ### Developing for the cfclient * [Fork the cflib](https://help.github.com/articles/fork-a-repo/) * [Clone the cflib](https://help.github.com/articles/cloning-a-repository/), `git clone git@github.com:YOUR-USERNAME/crazyflie-lib-python.git` -* [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` +* [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` * [Uninstall the cflib if you don't want it any more](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib` @@ -17,7 +17,7 @@ Note: If you are developing for the [cfclient][cfclient] you must use python3. O The following should be executed in the root of the crazyflie-lib-python file tree. #### Virtualenv -This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) +This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) with package dependencies. If you don't want to use virualenv and don't mind installing cflib dependencies system-wide you can skip this section. @@ -35,3 +35,7 @@ create an environment, activate it and install dependencies. Install dependencies required by the lib: `pip install -r requirements.txt` To verify the installation, connect the crazyflie and run an example: `python examples/basiclog` + +#### Install serial driver dependencies + +To use the serial driver, pyserial must be installed: `pip install pyserial` diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index a45eccbb6..1be2ad58a 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -14,8 +14,7 @@ If you are interested in more details look in the PyDoc in the code or: [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_log/) or [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_parameters/) -Structure of the library -======================== +## Structure of the library The library is asynchronous and based on callbacks for events. Functions like `open_link` will return immediately, and the callback `connected` @@ -23,8 +22,7 @@ will be called when the link is opened. The library doesn\'t contain any threads or locks that will keep the application running, it\'s up to the application that is using the library to do this. -Uniform Resource Identifier (URI) ---------------------------------- +### Uniform Resource Identifier (URI) All communication links are identified using an URI build up of the following: InterfaceType://InterfaceId/InterfaceChannel/InterfaceSpeed @@ -37,8 +35,7 @@ examples: speed 250 Kbit/s: radio://0/10/250K %% - Debug interface, id 0, channel 1: debug://0/1 -Variables and logging ---------------------- +### Variables and logging The library supports setting up logging configurations that are used for logging variables from the firmware. Each log configuration contains a @@ -67,8 +64,7 @@ There\'s a few limitations that needs to be taken into account: dependent on what types they are - The minimum period of a for a log configuration is multiples of 10ms -Parameters ----------- +### Parameters The library supports reading and writing parameters at run-time to the firmware. This is intended to be used for data that is not continuously @@ -90,8 +86,7 @@ parameters should be used in the following way: - For each write all the callbacks registered for this parameter will be called back -Variable and parameter names ----------------------------- +### Variable and parameter names All names of parameters and log variables use the same structure: `group.name` @@ -108,11 +103,9 @@ There\'s a limit of 28 chars in total and here are some examples: - imu\_tests.MPU6050 - pid\_attitide.pitch\_kd -Utilities -========= +## Utilities -Callbacks ---------- +### Callbacks All callbacks are handled using the `Caller` class that contains the following methods: @@ -128,8 +121,7 @@ following methods: """ Call the callbacks registered with the arguments args """ ``` -Debug driver ------------- +### Debug driver The library contains a special link driver, named `DebugDriver`. This driver will emulate a Crazyflie and is used for testing of the UI and @@ -147,8 +139,7 @@ random TOC CRC to always trigger TOC downloading or failing during connect. The driver also has support for sending back data with random delays to trigger random re-sending by the library. -Initiating the link drivers -=========================== +## Initiating the link drivers Before the library can be used the link drivers have to he initialized. This will search for available drivers and instantiate them. @@ -158,8 +149,16 @@ This will search for available drivers and instantiate them. """ Search for and initialize link drivers. If enable_debug_driver is True then the DebugDriver will also be used.""" ``` -Connection- and link-callbacks -============================== +### Serial driver + +The serial driver is disabled by default and has to be enabled to +be used. Enable it in the call to `init_drivers()` + +``` {.python} + init_drivers(enable_serial_driver=True) +``` + +## Connection- and link-callbacks Operations on the link and connection will return directly and will call the following callbacks when events occur: @@ -193,8 +192,7 @@ To register for callbacks the following is used: crazyflie.connected.add_callback(crazyflie_connected) ``` -Finding a Crazyflie and connecting -================================== +## Finding a Crazyflie and connecting The first thing to do is to find a Crazyflie quadcopter that we can connect to. This is done by queuing the library that will scan all the @@ -222,8 +220,7 @@ Then you can use the following to close the link again: crazyflie.close_link() ``` -Sending control commands -======================== +## Sending control commands The control commands are not implemented as parameters, instead they have a special API. @@ -256,8 +253,7 @@ every 2 seconds. Ideally you should be sending one tick every 10 ms, for 100 commands a second. This has a nice added benefit of allowing for very precise control. -Parameters -========== +## Parameters The parameter framework is used to read and set parameters. This functionality should be used when: @@ -307,8 +303,7 @@ Here\'s an example of how to use the calls. print "%s has value %d" % (name, value) ``` -Logging -======= +## Logging The logging framework is used to enable the \"automatic\" sending of variable values at specified intervals to the client. This functionality @@ -408,7 +403,6 @@ The logging cannot be started until your are connected to a Crazyflie: print "Error when logging %s" % logconf.name ``` -Examples -======== +## Examples The examples are now placed in the repository in the examples folder. From a3c3e4fad6e53d312d47b450896d49dfb5efa02f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 5 Aug 2020 13:27:37 +0200 Subject: [PATCH 051/861] #26 Removed python2 support --- README.md | 13 ++++++------- cflib/crazyflie/mem.py | 6 +----- cflib/crazyflie/param.py | 17 +++++------------ cflib/crazyflie/syncLogger.py | 7 +------ cflib/crtp/crtpstack.py | 8 ++------ cflib/crtp/debugdriver.py | 6 +----- cflib/crtp/radiodriver.py | 7 +------ cflib/crtp/serialdriver.py | 7 +------ cflib/crtp/udpdriver.py | 6 +----- cflib/crtp/usbdriver.py | 6 +----- cflib/positioning/motion_commander.py | 8 ++------ docs/development/testing.md | 9 ++++----- examples/multiranger_pointcloud.py | 2 +- module.json | 2 +- test/crazyflie/test_swarm.py | 7 +------ test/crazyflie/test_syncCrazyflie.py | 7 +------ test/crazyflie/test_syncLogger.py | 10 ++-------- test/positioning/test_motion_commander.py | 9 +++------ test/positioning/test_position_hl_commander.py | 9 +++------ test/utils/test_multiranger.py | 9 +++------ tools/build/test | 3 --- 21 files changed, 41 insertions(+), 117 deletions(-) diff --git a/README.md b/README.md index 9c4cfe516..e1781db37 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ For more info see our [documentation](https://www.bitcraze.io/documentation/repo ### Developing for the cfclient * [Fork the cflib](https://help.github.com/articles/fork-a-repo/) * [Clone the cflib](https://help.github.com/articles/cloning-a-repository/), `git clone git@github.com:YOUR-USERNAME/crazyflie-lib-python.git` -* [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` +* [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` * [Uninstall the cflib if you don't want it any more](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib` @@ -24,7 +24,7 @@ Note: If you are developing for the [cfclient][cfclient] you must use python3. O The following should be executed in the root of the crazyflie-lib-python file tree. #### Virtualenv -This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) +This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) with package dependencies. If you don't want to use virualenv and don't mind installing cflib dependencies system-wide you can skip this section. @@ -46,24 +46,23 @@ To verify the installation, connect the crazyflie and run an example: `python ex ## Testing ### With docker and the toolbelt -For information and installation of the +For information and installation of the [toolbelt.](https://wiki.bitcraze.io/projects:dockerbuilderimage:index) - + * Check to see if you pass tests: `tb test` * Check to see if you pass style guidelines: `tb verify` -Note: Docker and the toolbelt is an optional way of running tests and reduces the +Note: Docker and the toolbelt is an optional way of running tests and reduces the work needed to maintain your python environment. ### Native python on Linux, OSX, Windows [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` * If test fails after installing tox with `pip install tox`, installing with `sudo apt-get install tox`result a successful test run -* Test package in python2.7 `TOXENV=py27 tox` * Test package in python3.4 `TOXENV=py34 tox` * Test package in python3.6 `TOXENV=py36 tox` -Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python2.7, 3.3, 3.4 and would require all python versions to be installed on the machine.) +Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python 3.3, 3.4 and would require all python versions to be installed on the machine.) ## Platform notes diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 518b725cb..7523d650a 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -31,7 +31,6 @@ import errno import logging import struct -import sys from array import array from binascii import crc32 from functools import reduce @@ -57,10 +56,7 @@ # The max size of a CRTP packet payload MAX_LOG_DATA_PACKET_SIZE = 30 -if sys.version_info < (3,): - EEPROM_TOKEN = '0xBC' -else: - EEPROM_TOKEN = b'0xBC' +EEPROM_TOKEN = b'0xBC' logger = logging.getLogger(__name__) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index b8b42d2da..238735fbb 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -33,7 +33,7 @@ """ import logging import struct -import sys +from queue import Queue from threading import Lock from threading import Thread @@ -42,10 +42,6 @@ from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort from cflib.utils.callbacks import Caller -if sys.version_info < (3,): - from Queue import Queue -else: - from queue import Queue __author__ = 'Bitcraze AB' @@ -89,13 +85,10 @@ def __init__(self, ident=0, data=None): self.ident = ident if (data): strs = struct.unpack('s' * len(data[1:]), data[1:]) - if sys.version_info < (3,): - strs = ('{}' * len(strs)).format(*strs).split('\0') - else: - s = '' - for ch in strs: - s += ch.decode('ISO-8859-1') - strs = s.split('\x00') + s = '' + for ch in strs: + s += ch.decode('ISO-8859-1') + strs = s.split('\x00') self.group = strs[0] self.name = strs[1] diff --git a/cflib/crazyflie/syncLogger.py b/cflib/crazyflie/syncLogger.py index 5fc95888d..483e169a6 100644 --- a/cflib/crazyflie/syncLogger.py +++ b/cflib/crazyflie/syncLogger.py @@ -29,15 +29,10 @@ It acts as an iterator and returns the next value on each iteration. If no value is available it blocks until log data is received again. """ -import sys +from queue import Queue from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -if sys.version_info < (3,): - from Queue import Queue -else: - from queue import Queue - class SyncLogger: DISCONNECT_EVENT = 'DISCONNECT_EVENT' diff --git a/cflib/crtp/crtpstack.py b/cflib/crtp/crtpstack.py index 74c5d98dc..a792ff30e 100644 --- a/cflib/crtp/crtpstack.py +++ b/cflib/crtp/crtpstack.py @@ -28,7 +28,6 @@ CRTP packet and ports. """ import logging -import sys __author__ = 'Bitcraze AB' __all__ = ['CRTPPort', 'CRTPPacket'] @@ -121,13 +120,10 @@ def _set_data(self, data): if type(data) == bytearray: self._data = data elif type(data) == str: - if sys.version_info < (3,): - self._data = bytearray(data) - else: - self._data = bytearray(data.encode('ISO-8859-1')) + self._data = bytearray(data.encode('ISO-8859-1')) elif type(data) == list or type(data) == tuple: self._data = bytearray(data) - elif sys.version_info >= (3,) and type(data) == bytes: + elif type(data) == bytes: self._data = bytearray(data) else: raise Exception('Data must be bytearray, string, list or tuple,' diff --git a/cflib/crtp/debugdriver.py b/cflib/crtp/debugdriver.py index 65abc8cef..2ba70f808 100644 --- a/cflib/crtp/debugdriver.py +++ b/cflib/crtp/debugdriver.py @@ -36,11 +36,11 @@ """ import errno import logging +import queue import random import re import string import struct -import sys import time from datetime import datetime from threading import Thread @@ -51,10 +51,6 @@ from .exceptions import WrongUriType from cflib.crazyflie.log import LogTocElement from cflib.crazyflie.param import ParamTocElement -if sys.version_info < (3,): - import Queue as queue -else: - import queue __author__ = 'Bitcraze AB' __all__ = ['DebugDriver'] diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index a0a86a8d4..735837e96 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -34,9 +34,9 @@ import binascii import collections import logging +import queue import re import struct -import sys import threading import cflib.drivers.crazyradio as crazyradio @@ -45,11 +45,6 @@ from cflib.crtp.crtpdriver import CRTPDriver from cflib.drivers.crazyradio import Crazyradio -if sys.version_info < (3,): - import Queue as queue -else: - import queue - __author__ = 'Bitcraze AB' __all__ = ['RadioDriver'] diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index 47e40c023..ba78a059f 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -29,19 +29,14 @@ run high-speed CRTP with the Crazyflie. The UART can be run at 2Mbit. """ import logging +import queue import re -import sys import threading from .crtpstack import CRTPPacket from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver -if sys.version_info < (3,): - import Queue as queue -else: - import queue - found_serial = True try: import serial diff --git a/cflib/crtp/udpdriver.py b/cflib/crtp/udpdriver.py index dfba56d57..bd2a1c352 100644 --- a/cflib/crtp/udpdriver.py +++ b/cflib/crtp/udpdriver.py @@ -26,18 +26,14 @@ # MA 02110-1301, USA. """ CRTP UDP Driver. Work either with the UDP server or with an UDP device See udpserver.py for the protocol""" +import queue import re import struct -import sys from socket import socket from .crtpdriver import CRTPDriver from .crtpstack import CRTPPacket from .exceptions import WrongUriType -if sys.version_info < (3,): - import Queue as queue -else: - import queue __author__ = 'Bitcraze AB' __all__ = ['UdpDriver'] diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index e67a7b622..2e29b2b1e 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -30,18 +30,14 @@ This driver is used to communicate with the Crazyflie using the USB connection. """ import logging +import queue import re -import sys import threading from .crtpstack import CRTPPacket from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver from cflib.drivers.cfusb import CfUsb -if sys.version_info < (3,): - import Queue as queue -else: - import queue __author__ = 'Bitcraze AB' __all__ = ['UsbDriver'] diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index dc3f0e365..8053ae18a 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -42,17 +42,13 @@ created/closed. """ import math -import sys import time +from queue import Empty +from queue import Queue from threading import Thread from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -if sys.version_info < (3,): - from Queue import Queue, Empty -else: - from queue import Queue, Empty - class MotionCommander: """The motion commander""" diff --git a/docs/development/testing.md b/docs/development/testing.md index 674f1f0e4..f94860452 100644 --- a/docs/development/testing.md +++ b/docs/development/testing.md @@ -6,21 +6,20 @@ page_id: testing ## Testing ### With docker and the toolbelt -For information and installation of the +For information and installation of the [toolbelt.](https://wiki.bitcraze.io/projects:dockerbuilderimage:index) - + * Check to see if you pass tests: `tb test` * Check to see if you pass style guidelines: `tb verify` -Note: Docker and the toolbelt is an optional way of running tests and reduces the +Note: Docker and the toolbelt is an optional way of running tests and reduces the work needed to maintain your python environment. ### Native python on Linux, OSX, Windows [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` * If test fails after installing tox with `pip install tox`, installing with `sudo apt-get install tox`result a successful test run -* Test package in python2.7 `TOXENV=py27 tox` * Test package in python3.4 `TOXENV=py34 tox` * Test package in python3.6 `TOXENV=py36 tox` -Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python2.7, 3.3, 3.4 and would require all python versions to be installed on the machine.) +Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python 3.3, 3.4 and would require all python versions to be installed on the machine.) diff --git a/examples/multiranger_pointcloud.py b/examples/multiranger_pointcloud.py index d9054e10b..a753a5118 100644 --- a/examples/multiranger_pointcloud.py +++ b/examples/multiranger_pointcloud.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # -*- coding: utf-8 -*- # # || ____ _ __ diff --git a/module.json b/module.json index 3d8af4559..7ba35679b 100644 --- a/module.json +++ b/module.json @@ -1,4 +1,4 @@ { "version": "1.0", - "environmentReq": ["python2", "python3"] + "environmentReq": ["python3"] } diff --git a/test/crazyflie/test_swarm.py b/test/crazyflie/test_swarm.py index ef2cdd6c7..ca125f1fa 100644 --- a/test/crazyflie/test_swarm.py +++ b/test/crazyflie/test_swarm.py @@ -21,17 +21,12 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. -import sys import unittest +from unittest.mock import MagicMock from cflib.crazyflie.swarm import Swarm from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -if sys.version_info < (3, 3): - from mock import MagicMock -else: - from unittest.mock import MagicMock - class TestSwarm(unittest.TestCase): URI1 = 'uri1' diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index 29f6d7644..977963b96 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -23,19 +23,14 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. -import sys import unittest from test.support.asyncCallbackCaller import AsyncCallbackCaller +from unittest.mock import MagicMock from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils.callbacks import Caller -if sys.version_info < (3, 3): - from mock import MagicMock -else: - from unittest.mock import MagicMock - class SyncCrazyflieTest(unittest.TestCase): diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index 64d2baeea..3417fe397 100644 --- a/test/crazyflie/test_syncLogger.py +++ b/test/crazyflie/test_syncLogger.py @@ -23,9 +23,10 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. -import sys import unittest from test.support.asyncCallbackCaller import AsyncCallbackCaller +from unittest.mock import call +from unittest.mock import MagicMock from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import Log @@ -34,13 +35,6 @@ from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils.callbacks import Caller -if sys.version_info < (3, 3): - from mock import MagicMock - from mock import call -else: - from unittest.mock import MagicMock - from unittest.mock import call - class SyncLoggerTest(unittest.TestCase): diff --git a/test/positioning/test_motion_commander.py b/test/positioning/test_motion_commander.py index 7fd3d7ce1..044b29ae6 100644 --- a/test/positioning/test_motion_commander.py +++ b/test/positioning/test_motion_commander.py @@ -22,8 +22,10 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import math -import sys import unittest +from unittest.mock import call +from unittest.mock import MagicMock +from unittest.mock import patch from cflib.crazyflie import Commander from cflib.crazyflie import Crazyflie @@ -31,11 +33,6 @@ from cflib.positioning.motion_commander import _SetPointThread from cflib.positioning.motion_commander import MotionCommander -if sys.version_info < (3, 3): - from mock import MagicMock, patch, call -else: - from unittest.mock import MagicMock, patch, call - @patch('time.sleep') @patch('cflib.positioning.motion_commander._SetPointThread', diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index 7aed9681f..6b7c28ee0 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -22,19 +22,16 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import math -import sys import unittest +from unittest.mock import call +from unittest.mock import MagicMock +from unittest.mock import patch from cflib.crazyflie import Crazyflie from cflib.crazyflie import HighLevelCommander from cflib.crazyflie import Param from cflib.positioning.position_hl_commander import PositionHlCommander -if sys.version_info < (3, 3): - from mock import MagicMock, patch, call -else: - from unittest.mock import MagicMock, patch, call - @patch('time.sleep') class TestPositionHlCommander(unittest.TestCase): diff --git a/test/utils/test_multiranger.py b/test/utils/test_multiranger.py index f4a514b47..f79b45015 100644 --- a/test/utils/test_multiranger.py +++ b/test/utils/test_multiranger.py @@ -21,8 +21,10 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. -import sys import unittest +from unittest.mock import call +from unittest.mock import MagicMock +from unittest.mock import patch from cflib.crazyflie import Crazyflie from cflib.crazyflie import Log @@ -30,11 +32,6 @@ from cflib.utils.callbacks import Caller from cflib.utils.multiranger import Multiranger -if sys.version_info < (3, 3): - from mock import MagicMock, call, patch -else: - from unittest.mock import MagicMock, call, patch - class MultirangerTest(unittest.TestCase): FRONT = 'range.front' diff --git a/tools/build/test b/tools/build/test index d2ed44468..d7b2ed4b6 100755 --- a/tools/build/test +++ b/tools/build/test @@ -7,9 +7,6 @@ try: script_dir = os.path.dirname(os.path.realpath(__file__)) root = _path.normpath(_path.join(script_dir, '../../test')) - print('**** Running tests in python2 ****') - subprocess.check_call(['python2', '-m', 'unittest', 'discover', root]) - print('') print('**** Running tests in python3 ****') subprocess.check_call(['python3', '-m', 'unittest', 'discover', root]) From 5b50b79f80b75fe305e0bc60f236a23b330d5308 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 5 Aug 2020 20:31:14 +0200 Subject: [PATCH 052/861] Fixed flake 8 problems with new builder image --- .pre-commit-config.yaml | 4 ++-- cflib/bootloader/__init__.py | 4 ++-- cflib/crtp/radiodriver.py | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 59c77fa95..e0c3343e9 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,5 +1,5 @@ - repo: git://github.com/pre-commit/pre-commit-hooks - sha: 7192665e31cea6ace58a71e086c7248d7e5610c2 + rev: 7192665e31cea6ace58a71e086c7248d7e5610c2 hooks: - id: autopep8-wrapper - id: check-added-large-files @@ -16,6 +16,6 @@ - id: flake8 - id: requirements-txt-fixer - repo: git://github.com/asottile/reorder_python_imports - sha: ab609b9b982729dfc287b4e75963c0c4de254a31 + rev: ab609b9b982729dfc287b4e75963c0c4de254a31 hooks: - id: reorder-python-imports diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index cde26e9cb..cd1332278 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -334,7 +334,7 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): ctr): if self.progress_cb: self.progress_cb( - 'Error during flash operation (code %d)'.format( + 'Error during flash operation (code {})'.format( self._cload.error_code), int(progress)) else: @@ -361,7 +361,7 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): (ctr - 1)), ctr): if self.progress_cb: self.progress_cb( - 'Error during flash operation (code %d)'.format( + 'Error during flash operation (code {})'.format( self._cload.error_code), int(progress)) else: diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 735837e96..16a2c97a2 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -293,11 +293,11 @@ def _scan_radio_channels(self, cradio, start=0, stop=125): def scan_selected(self, links): to_scan = () - for l in links: + for link in links: one_to_scan = {} uri_data = re.search('^radio://([0-9]+)((/([0-9]+))' '(/(250K|1M|2M))?)?$', - l) + link) one_to_scan['channel'] = int(uri_data.group(4)) From 751ff352eb5befe149bafbd7ae261933be159357 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 6 Aug 2020 09:56:05 +0200 Subject: [PATCH 053/861] #26 Explicitly use python3 for building dist --- tools/build/bdist | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/build/bdist b/tools/build/bdist index 0aaf41489..c0bc8dc31 100755 --- a/tools/build/bdist +++ b/tools/build/bdist @@ -9,7 +9,7 @@ try: script_dir = os.path.dirname(os.path.realpath(__file__)) root = _path.normpath(_path.join(script_dir, '../..')) - subprocess.check_call(['python', 'setup.py', 'bdist_wheel'], cwd=root) + subprocess.check_call(['python3', 'setup.py', 'bdist_wheel'], cwd=root) print('Wheel built') except subprocess.CalledProcessError as e: From 8dad5491d9bbfba7062fbf01d1af1e078917e9f3 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 1 Sep 2020 08:41:09 +0200 Subject: [PATCH 054/861] #164 Refactoring --- cflib/crazyflie/log.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 7f5e34ad9..f5b3d63ea 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -217,14 +217,13 @@ def _get_started(self): added = property(_get_added, _set_added) started = property(_get_started, _set_started) - def create(self): - """Save the log configuration in the Crazyflie""" - pk = CRTPPacket() - pk.set_header(5, CHAN_SETTINGS) + def _cmd_create_block(self): if self.useV2: - pk.data = (CMD_CREATE_BLOCK_V2, self.id) + return CMD_CREATE_BLOCK_V2 else: - pk.data = (CMD_CREATE_BLOCK, self.id) + return CMD_CREATE_BLOCK + + def _setup_log_elements(self, pk): for var in self.variables: if (var.is_toc_variable() is False): # Memory location logger.debug('Logging to raw memory %d, 0x%04X', @@ -244,12 +243,16 @@ def create(self): pk.data.append((ident >> 8) & 0x0ff) else: pk.data.append(self.cf.log.toc.get_element_id(var.name)) + + def create(self): + """Save the log configuration in the Crazyflie""" + command = self._cmd_create_block() + pk = CRTPPacket() + pk.set_header(5, CHAN_SETTINGS) + pk.data = (command, self.id) + self._setup_log_elements(pk) logger.debug('Adding log block id {}'.format(self.id)) - if self.useV2: - self.cf.send_packet(pk, expected_reply=( - CMD_CREATE_BLOCK_V2, self.id)) - else: - self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id)) + self.cf.send_packet(pk, expected_reply=(command, self.id)) def start(self): """Start the logging for this entry""" From b4a78fc2e306402b75d2053113b375b91b7c0b68 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 1 Sep 2020 10:49:01 +0200 Subject: [PATCH 055/861] #164 Support log blocks with many variables by using the append command when needed --- cflib/crazyflie/log.py | 54 ++++++++++++++++++++++++++++------------- cflib/crtp/crtpstack.py | 12 +++++++++ 2 files changed, 49 insertions(+), 17 deletions(-) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index f5b3d63ea..47aeaacb3 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -99,8 +99,6 @@ GET_TOC_INF = 'GET_TOC_INFO' GET_TOC_ELEMENT = 'GET_TOC_ELEMENT' -# The max size of a CRTP packet payload -MAX_LOG_DATA_PACKET_SIZE = 30 logger = logging.getLogger(__name__) @@ -223,8 +221,16 @@ def _cmd_create_block(self): else: return CMD_CREATE_BLOCK - def _setup_log_elements(self, pk): - for var in self.variables: + def _cmd_append_block(self): + if self.useV2: + return CMD_APPEND_BLOCK_V2 + else: + return CMD_APPEND_BLOCK + + def _setup_log_elements(self, pk, next_to_add): + i = next_to_add + for i in range(next_to_add, len(self.variables)): + var = self.variables[i] if (var.is_toc_variable() is False): # Memory location logger.debug('Logging to raw memory %d, 0x%04X', var.get_storage_and_fetch_byte(), var.address) @@ -232,27 +238,41 @@ def _setup_log_elements(self, pk): var.get_storage_and_fetch_byte())) pk.data.append(struct.pack('> 8) & 0x0ff) + size_to_add = 2 + if pk.available_data_size() >= size_to_add: + pk.data.append(element_id & 0x0ff) + pk.data.append((element_id >> 8) & 0x0ff) + else: + # Packet is full + return False, i else: - pk.data.append(self.cf.log.toc.get_element_id(var.name)) + pk.data.append(element_id) + + return True, i def create(self): """Save the log configuration in the Crazyflie""" command = self._cmd_create_block() - pk = CRTPPacket() - pk.set_header(5, CHAN_SETTINGS) - pk.data = (command, self.id) - self._setup_log_elements(pk) - logger.debug('Adding log block id {}'.format(self.id)) - self.cf.send_packet(pk, expected_reply=(command, self.id)) + next_to_add = 0 + is_done = False + while not is_done: + pk = CRTPPacket() + pk.set_header(5, CHAN_SETTINGS) + pk.data = (command, self.id) + is_done, next_to_add = self._setup_log_elements(pk, next_to_add) + + logger.debug('Adding/appending log block id {}'.format(self.id)) + self.cf.send_packet(pk, expected_reply=(command, self.id)) + + # Use append if we have to add more variables + command = self._cmd_append_block() def start(self): """Start the logging for this entry""" @@ -455,7 +475,7 @@ def add_config(self, logconf): logconf.valid = False raise KeyError('Variable {} not in TOC'.format(var.name)) - if (size <= MAX_LOG_DATA_PACKET_SIZE and + if (size <= CRTPPacket.MAX_DATA_SIZE and (logconf.period > 0 and logconf.period < 0xFF)): logconf.valid = True logconf.cf = self.cf diff --git a/cflib/crtp/crtpstack.py b/cflib/crtp/crtpstack.py index a792ff30e..c640bed2c 100644 --- a/cflib/crtp/crtpstack.py +++ b/cflib/crtp/crtpstack.py @@ -58,6 +58,9 @@ class CRTPPacket(object): A packet that can be sent via the CRTP. """ + # The max size of a CRTP packet payload + MAX_DATA_SIZE = 30 + def __init__(self, header=0, data=None): """ Create an empty packet with default values. @@ -141,6 +144,15 @@ def __str__(self): """Get a string representation of the packet""" return '{}:{} {}'.format(self._port, self.channel, self.datat) + def get_data_size(self): + return len(self._data) + + def available_data_size(self): + return self.MAX_DATA_SIZE - self.get_data_size() + + def is_data_size_valid(self): + return self.available_data_size() >= 0 + data = property(_get_data, _set_data) datal = property(_get_data_l, _set_data) datat = property(_get_data_t, _set_data) From e657ca42207349f879a0958258b25c353d154f20 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 1 Sep 2020 10:51:52 +0200 Subject: [PATCH 056/861] #164 Added check that size is OK when sending packet --- cflib/crazyflie/__init__.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index b504735b4..7a0f7d22e 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -311,6 +311,10 @@ def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): be sent back, otherwise false """ + + if not pk.is_data_size_valid(): + raise Exception("Data part of packet is too large") + self._send_lock.acquire() if self.link is not None: if len(expected_reply) > 0 and not resend and \ From 0dd11ca5189b0f1c22cb7753d46bf6fc2a4a75af Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 1 Sep 2020 10:52:49 +0200 Subject: [PATCH 057/861] #164 Removed unused constant --- cflib/crazyflie/mem.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 7523d650a..9761a73ac 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -53,9 +53,6 @@ CMD_INFO_NBR = 1 CMD_INFO_DETAILS = 2 -# The max size of a CRTP packet payload -MAX_LOG_DATA_PACKET_SIZE = 30 - EEPROM_TOKEN = b'0xBC' logger = logging.getLogger(__name__) From 77a73a55dfbbf5f64d510501aefa796734c2cd57 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 1 Sep 2020 10:56:35 +0200 Subject: [PATCH 058/861] #164 flake8 corrections --- cflib/crazyflie/__init__.py | 2 +- cflib/crazyflie/log.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 7a0f7d22e..caea384e0 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -313,7 +313,7 @@ def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): """ if not pk.is_data_size_valid(): - raise Exception("Data part of packet is too large") + raise Exception('Data part of packet is too large') self._send_lock.acquire() if self.link is not None: diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 47aeaacb3..6f40ea58b 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -100,7 +100,6 @@ GET_TOC_ELEMENT = 'GET_TOC_ELEMENT' - logger = logging.getLogger(__name__) From f96ba79e5c525e1383361e228c274e5bfd1145f2 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 1 Sep 2020 13:38:34 +0200 Subject: [PATCH 059/861] #143 Added docs for synchronous API --- docs/user-guides/python_api.md | 124 ++++++++++++++++++++++++++++++++- 1 file changed, 123 insertions(+), 1 deletion(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 1be2ad58a..140327006 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -22,6 +22,10 @@ will be called when the link is opened. The library doesn\'t contain any threads or locks that will keep the application running, it\'s up to the application that is using the library to do this. +There are a few synchronous wrappers for selected classes that creates +a synchronous API by wrapping the asynchronous classes, see the +[Synchronous API section](#synchronous-api) + ### Uniform Resource Identifier (URI) All communication links are identified using an URI build up of the @@ -403,6 +407,124 @@ The logging cannot be started until your are connected to a Crazyflie: print "Error when logging %s" % logconf.name ``` +The values of log varibles are transferred from the Crazyflie using CRTP +packets, where all varibles belonging to one logging configuration are +transfered in the same packet. A CRTP packet has a maximum data size of +30 bytes, which sets an upper limit to the number of variables that +can be used in one logging configuration. If the desired log variables +do not fit in one logging configuration, a second cofiguration may +be added. + +``` {.python} + crazyflie.log.add_config([logconf1, logconfig2]) +``` + +## Synchronous API + +The synchronous classes are wrappers around the asynchronouse API, where the asynchronous +calls/callbacks are replaced with blocking calls. The synchronous API does not +provide the full flexibility of the asynchronous API, but is useful when writing +small scripts for logging for instance. + +The synchronous API uses the python Context manager concept, that is the ```with``` keyword. +A resource is allocated when entering a ```with``` section and automatically released when +exiting it, for instance a connection or take off/landing of a Crazyflie. + +### SyncCrazyflie + +The SyncCrazyflie class wrapps a Crazyflie instance and mainly simplifies connect/disconnect. + +Basic usage +``` {.python} + with SyncCrazyflie(uri) as scf: + # A Crazyflie instance is created and is now connected. If the connection failes, + # an exception is raised. + + # The underlying crazyflie object can be accessed through the cf member + scf.cf.param.set_value('kalman.resetEstimation', '1') + + # Do useful stuff + # When leaving the "with" section, the connection is automatically closed +``` + +If some special properties are required for the underlying Crazyflie object, +a Crazyflie instance can be passed in to the SyncCrazyflie instance. + +``` {.python} + my_cf = Crazyflie(rw_cache='./cache') + with SyncCrazyflie(uri, cf=my_cf) as scf: + # The my_cf is now connected + # Do useful stuff + # When leaving the "with" section, the connection is automatically closed +``` + +### SyncLogger + +The SyncLogger class wraps setting up, as well as starting/stopping logging. It works both for Crazyflie +and SyncCrazyflie instances. To get the log values, iterate the instance. + +``` {.python} + # Connect to a Crazyflie + with SyncCrazyflie(uri) as scf: + # Create a log configuration + log_conf = LogConfig(name='myConf', period_in_ms=200) + log_conf.add_variable('stateEstimateZ.vx', 'int16_t') + + # Start logging + with SyncLogger(scf, log_conf) as logger: + # Iterate the logger to get the values + count = 0 + for log_entry in logger: + print(log_entry) + # Do useful stuff + count += 1 + if (count > 10): + # The logging will continue until you exit the loop + break + # When leaving this "with" section, the logging is automatically stopped + # When leaving this "with" section, the connection is automatically closed +``` + +### MotionCommander + +The MotionCommander is intended to simplify basic autonomous flight. The Crazyflie takes off +when entering the "with" section, and lands when exiting. It has functions for basic +movements that are blocking until the motion is finished. + +The MotionCommander is using velocity set points and does not have a global coordinate +system, all positions are relative. It is mainly intended to be used with a Flow deck. + +``` {.python} + with SyncCrazyflie(URI) as scf: + # We take off when the commander is created + with MotionCommander(scf) as mc: + # Move one meter forward + mc.forward(1) + # Move one meter back + mc.back(1) + # The Crazyflie lands when leaving this "with" section + # When leaving this "with" section, the connection is automatically closed +``` + +### PositionHlCommander + +The PositionHlCommander is intended to simplify basic autonomous flight. The Crazyflie takes off +when entering the "with" section, and lands when exiting. It has functions for basic +movements that are blocking until the motion is finished. + +The PositionHlCommander uses the high level commander in the Crazyflie and is +based on a global coordinate system and absolute positoinins. It is inteneded +to be used with a positioning system such as LPS, the lighthouse or a mocap system. + +``` {.python} + with SyncCrazyflie(URI) as scf: + with PositionHlCommander(scf) as pc: + # Go to the coordinate (0, 0, 1) + pc.go_to(0.0, 0.0, 1.0) + # The Crazyflie lands when leaving this "with" section + # When leaving this "with" section, the connection is automatically closed +``` + ## Examples -The examples are now placed in the repository in the examples folder. +The see the example folder of the repository. From 04462951d4d7f0d5bdd7629489954f2ca6573deb Mon Sep 17 00:00:00 2001 From: Marlene Boehmer Date: Tue, 1 Sep 2020 16:38:58 +0200 Subject: [PATCH 060/861] documentation for uart communication --- docs/user-guides/uart_communication.md | 83 ++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 docs/user-guides/uart_communication.md diff --git a/docs/user-guides/uart_communication.md b/docs/user-guides/uart_communication.md new file mode 100644 index 000000000..16adceaaa --- /dev/null +++ b/docs/user-guides/uart_communication.md @@ -0,0 +1,83 @@ +--- +title: UART communication +page_id: uart_communication +--- + +This page describes how to control your Crazyflie via UART, e.g. with a direct connection to a Raspberry Pi or with your computer through an FTDI cable. + +# Physical Connection + +To control the Crazyflie via UART first establish a physical connection between the Crazyflie and the controlling device. On the Crazyflie use the pins for UART2 which are on the right expansion connector TX2 (pin 1) and RX2 (pin 2). + +If you are connecting to a Raspberry Pi look for the UART pins there connect them as follows + +- Crazyflie TX2 -- Raspberry Pi RX +- Crazyflie RX2 -- Raspberry Pi TX + +# Crazyflie Firmware + +Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the firmware has to be compiled with the `UART2_LINK=1` flag (e.g. `make UART2_LINK=1`) and flashed to the Crazyflie. + +# Controlling Device + +On the controlling device the Python library `crazyflie-lib-python` can be used to send commands to the Crazyflie via UART if some additional dependencies are installed and UART is properly set up to be used. + +## UART setup Raspberry Pi + +A prerequisite for controlling the Crazyflie via UART is that UART is properly set up. The following steps are tested with a Raspberry Pi Zero and a Raspberry Pi 3B+. Depending on what Raspberry Pi version you use some steps might be skipped or be a bit different. Additional information can be found in the [Raspberry Pi UART documentation](https://www.raspberrypi.org/documentation/configuration/uart.md). + +- Enable the mini UART (`/dev/ttyS0`) in `/boot/config.txt`: `enable_uart=1` + +- Restore `/dev/ttyAMA0` as primary UART in `/boot/config.txt`: `dtoverlay=miniuart-bt` + + This step is not necessary, but might be desirable as the mini UART is less capable than the PL011 UART. If you skip this step use the mini UART (`/dev/ttyS0`) to communicate with the Crazyflie. + + (You can confirm that ttyAMA0 is the primary UART by using `ls -l /dev/` and looking for `serial0 -> ttyAMA0`.) + +- Disable login shell with `sudo raspi-config`: Interfacing Options->Serial->disable Login Shell/enable serial hardware port + +- Allow the user to use the UART device: `sudo usermod -a -G dialout $USER` + + (You can confirm that the user is listed in the dialout group with `groups` and that the device belongs to the group with `ls -l /dev/ttyAMA0` or `ls -l /dev/ttyS0`.) + +## Additional Dependencies + +The Python library needs `pyserial` as an additional dependency for the UART communication. To install `pyserial` use `pip3 install pyserial`. (You can confirm that pyserial indeed finds your UART port by `python3 -m serial.tools.list_ports -v`.) + +# Usage + +Once everything is set up you should be able to control the Crazyflie via UART. + +Add the parameter `enable_serial_driver=True` to `cflib.crtp.init_drivers()` and connect to the Crazyflie using a serial URI. +The serial URI has the form `serial://` (e.g. `serial://ttyAMA0`, `serial://ttyUSB5`) or if the OS of the controlling device does not provide the name `serial://` (e.g. `serial:///dev/ttyAMA0`). + +The following script might give an idea on how a first test of the setup might look like. + +```{.python} +#!/usr/bin/env python3 + +import logging +import time + +import cflib.crtp +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + +# choose the serial URI that matches the setup serial device +URI = 'serial://ttyAMA0' + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + +if __name__ == '__main__': + # Initialize the low-level drivers including the serial driver + cflib.crtp.init_drivers(enable_serial_driver=True) + + with SyncCrazyflie(URI) as scf: + # We take off when the commander is created + with MotionCommander(scf) as mc: + print('Taking off!') + time.sleep(0.1) + # We land when the MotionCommander goes out of scope + print('Landing!') +``` From cb06b596f4291a692b0f9a6ee97f8abc66cf7390 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 2 Sep 2020 08:34:29 +0200 Subject: [PATCH 061/861] #165 Added uart user guide to the menu --- docs/_data/menu.yml | 1 + docs/user-guides/uart_communication.md | 12 ++++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index fff7eef72..a17ece329 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -6,6 +6,7 @@ - title: User guides subs: - {page_id: python_api} + - {page_id: uart_communication} - {page_id: sbs_connect_log_param} - {page_id: sbs_motion_commander} - title: Functional areas diff --git a/docs/user-guides/uart_communication.md b/docs/user-guides/uart_communication.md index 16adceaaa..eb2a956e4 100644 --- a/docs/user-guides/uart_communication.md +++ b/docs/user-guides/uart_communication.md @@ -5,7 +5,7 @@ page_id: uart_communication This page describes how to control your Crazyflie via UART, e.g. with a direct connection to a Raspberry Pi or with your computer through an FTDI cable. -# Physical Connection +## Physical Connection To control the Crazyflie via UART first establish a physical connection between the Crazyflie and the controlling device. On the Crazyflie use the pins for UART2 which are on the right expansion connector TX2 (pin 1) and RX2 (pin 2). @@ -14,15 +14,15 @@ If you are connecting to a Raspberry Pi look for the UART pins there connect the - Crazyflie TX2 -- Raspberry Pi RX - Crazyflie RX2 -- Raspberry Pi TX -# Crazyflie Firmware +## Crazyflie Firmware Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the firmware has to be compiled with the `UART2_LINK=1` flag (e.g. `make UART2_LINK=1`) and flashed to the Crazyflie. -# Controlling Device +## Controlling Device On the controlling device the Python library `crazyflie-lib-python` can be used to send commands to the Crazyflie via UART if some additional dependencies are installed and UART is properly set up to be used. -## UART setup Raspberry Pi +### UART setup Raspberry Pi A prerequisite for controlling the Crazyflie via UART is that UART is properly set up. The following steps are tested with a Raspberry Pi Zero and a Raspberry Pi 3B+. Depending on what Raspberry Pi version you use some steps might be skipped or be a bit different. Additional information can be found in the [Raspberry Pi UART documentation](https://www.raspberrypi.org/documentation/configuration/uart.md). @@ -40,11 +40,11 @@ A prerequisite for controlling the Crazyflie via UART is that UART is properly s (You can confirm that the user is listed in the dialout group with `groups` and that the device belongs to the group with `ls -l /dev/ttyAMA0` or `ls -l /dev/ttyS0`.) -## Additional Dependencies +### Additional Dependencies The Python library needs `pyserial` as an additional dependency for the UART communication. To install `pyserial` use `pip3 install pyserial`. (You can confirm that pyserial indeed finds your UART port by `python3 -m serial.tools.list_ports -v`.) -# Usage +## Usage Once everything is set up you should be able to control the Crazyflie via UART. From 4cce04c26551fb5028e25659b6bec69d11cea91c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Sep 2020 12:43:55 +0200 Subject: [PATCH 062/861] #167 Added callback to Bootloader class for terminating flashing process --- cflib/bootloader/__init__.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index cd1332278..94ebeb8d6 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -73,6 +73,7 @@ def __init__(self, clink=None): self.in_bootloader_cb = None # Target self.dev_info_cb = None + self.terminate_flashing_cb = None # self.dev_info_cb.add_callback(self._dev_info) # self.in_bootloader_cb.add_callback(self._bootloader_info) @@ -294,6 +295,9 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): # For each page ctr = 0 # Buffer counter for i in range(0, int((len(image) - 1) / t_data.page_size) + 1): + if self.terminate_flashing_cb and self.terminate_flashing_cb(): + raise Exception("Flashing terminated") + # Load the buffer if ((i + 1) * t_data.page_size) > len(image): self._cload.upload_buffer( From 161583f01d940694f1aa47ced0fd49c9b915598b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Sep 2020 12:50:24 +0200 Subject: [PATCH 063/861] #167 Fixed flake8 issue --- cflib/bootloader/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 94ebeb8d6..6aa123c9d 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -296,7 +296,7 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): ctr = 0 # Buffer counter for i in range(0, int((len(image) - 1) / t_data.page_size) + 1): if self.terminate_flashing_cb and self.terminate_flashing_cb(): - raise Exception("Flashing terminated") + raise Exception('Flashing terminated') # Load the buffer if ((i + 1) * t_data.page_size) > len(image): From 0c70f88e407129cbe91281867cde9e165ab20e3c Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 8 Sep 2020 15:07:14 +0200 Subject: [PATCH 064/861] Update setup.py version to 0.1.12.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 626a6494d..43cda1bb5 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.11', + version='0.1.12.1', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 1a5de7f97a4ca89e6211e043cc555b5d7ca93ce7 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 30 Sep 2020 17:35:00 +0200 Subject: [PATCH 065/861] #118: Implement share radio sharing between link This commit implements a queue based radio sharing instead of the previously implemented mutex-based sharing of the radio. This should guarantee that the radio is shared fairly between connections using the same radio. Mutex could not offer this guarantee since Mutex implementation in python uses the underlying OS implementation that does not nececarly guarantee that the thread waiting are unlocked in order. --- cflib/crtp/radiodriver.py | 349 +++++++++++++++++++++++--------------- 1 file changed, 216 insertions(+), 133 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 16a2c97a2..1bedde166 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -33,11 +33,15 @@ import array import binascii import collections +from enum import Enum import logging import queue +from queue import Queue import re import struct import threading +from threading import Semaphore, Thread +from typing import Any, Dict, Iterable, List, Tuple, Union import cflib.drivers.crazyradio as crazyradio from .crtpstack import CRTPPacket @@ -58,68 +62,151 @@ DEFAULT_ADDR = 0xE7E7E7E7E7 -class _SharedRadio(): - """ Manages access to one shared radio - """ +class _RadioCommands(Enum): + STOP = 0 + SEND_PACKET = 1 + SET_ARC = 2 + SCAN_SELECTED = 3 + SCAN_CHANNELS = 4 - def __init__(self, devid): - self.radio = Crazyradio(devid=devid) - self.lock = threading.Lock() - self.usage_counter = 0 - - -class _RadioManager(): - """ Radio manager helper class - Get a Crazyradio with: - radio_manager = _RadioManager(devid) - Then use your Crazyradio: - with radio_manager as cradio: - # cradio is the Crazyradio driver object, it is locked - Finally close it when finished. - cradio.close() - """ - # Configuration lock. Protects opening and closing Crazyradios - _config_lock = threading.Lock() - _radios = [] # Hardware Crazyradio objects +class _SharedRadioInstance(): + def __init__(self, instance_id: int, + cmd_queue: "Queue[Tuple[int, _RadioCommands, Any]]", + rsp_queue: Queue, + version: float): + self._instance_id = instance_id + self._cmd_queue = cmd_queue + self._rsp_queue = rsp_queue + + self._channel = 2 + self._address = [0xe7]*5 + self._datarate = crazyradio.Crazyradio.DR_2MPS + + self._opened = True - def __init__(self, devid, channel=0, datarate=0, address=DEFAULT_ADDR_A): - self._devid = devid + self.version = version + + def set_channel(self, channel: int): self._channel = channel - self._datarate = datarate - self._address = address - with _RadioManager._config_lock: - if len(_RadioManager._radios) <= self._devid or \ - _RadioManager._radios[self._devid] is None: - _RadioManager._radios += ((self._devid + 1) - - len(_RadioManager._radios)) * [None] - _RadioManager._radios[self._devid] = _SharedRadio(self._devid) + def set_address(self, address): + self._address = address - _RadioManager._radios[self._devid].usage_counter += 1 + def set_data_rate(self, dr): + self._datarate = dr + + def send_packet(self, data: List[int]) -> crazyradio._radio_ack: + assert(self._opened) + self._cmd_queue.put((self._instance_id, + _RadioCommands.SEND_PACKET, + (self._channel, + self._address, + self._datarate, + data))) + ack = self._rsp_queue.get() # type: crazyradio._radio_ack + return ack + + def set_arc(self, arc): + assert(self._opened) + self._cmd_queue.put((self._instance_id, + _RadioCommands.SET_ARC, + arc)) + + def scan_selected(self, selected, packet): + assert(self._opened) + self._cmd_queue.put((self._instance_id, + _RadioCommands.SCAN_SELECTED, + (self._datarate, self._address, + selected, packet))) + return self._rsp_queue.get() + + def scan_channels(self, start: int, stop: int, packet: Iterable[int]): + assert(self._opened) + self._cmd_queue.put((self._instance_id, + _RadioCommands.SCAN_CHANNELS, + (self._datarate, self._address, + start, stop, packet))) + return self._rsp_queue.get() def close(self): - with _RadioManager._config_lock: - _RadioManager._radios[self._devid].usage_counter -= 1 + assert(self._opened) + self._cmd_queue.put((self._instance_id, _RadioCommands.STOP, None)) + self._opened = False - if _RadioManager._radios[self._devid].usage_counter == 0: - try: - _RadioManager._radios[self._devid].radio.close() - except Exception: - pass - _RadioManager._radios[self._devid] = None - def __enter__(self): - _RadioManager._radios[self._devid].lock.acquire() +class _SharedRadio(Thread): + def __init__(self, devid: int): + Thread.__init__(self) + self._radio = Crazyradio(devid=devid) + self.version = self._radio.version + + self._cmd_queue = Queue() # type: Queue[Tuple[int, _RadioCommands, Any]] # noqa + self._rsp_queues = {} # type: Dict[int, Queue[Any]] + self._next_instance_id = 0 + + self._lock = Semaphore(1) + + self.start() + + def open_instance(self) -> _SharedRadioInstance: + rsp_queue = Queue() + with self._lock: + instance_id = self._next_instance_id + self._rsp_queues[instance_id] = rsp_queue + self._next_instance_id += 1 + return _SharedRadioInstance(instance_id, + self._cmd_queue, + rsp_queue, + self._radio.version) + + def run(self): + while True: + command = self._cmd_queue.get() + + if command[1] == _RadioCommands.STOP: + with self._lock: + pass + elif command[1] == _RadioCommands.SEND_PACKET: + channel, address, datarate, data = command[2] + self._radio.set_channel(channel) + self._radio.set_address(address) + self._radio.set_data_rate(datarate) + ack = self._radio.send_packet(data) + self._rsp_queues[command[0]].put(ack) + elif command[1] == _RadioCommands.SET_ARC: + self._radio.set_arc(command[2]) + elif command[1] == _RadioCommands.SCAN_SELECTED: + datarate, address, selected, data = command[2] + self._radio.set_data_rate(datarate) + self._radio.set_address(address) + resp = self._radio.scan_selected(selected, data) + self._rsp_queues[command[0]].put(resp) + elif command[1] == _RadioCommands.SCAN_CHANNELS: + datarate, address, start, stop, packet = command[2] + self._radio.set_data_rate(datarate) + self._radio.set_address(address) + resp = self._radio.scan_channels(start, stop, packet) + self._rsp_queues[command[0]].put(resp) + + +class _RadioManager: + _radios = [] # type: List[Union[_SharedRadio, None]] + _lock = Semaphore(1) - _RadioManager._radios[self._devid].radio.set_channel(self._channel) - _RadioManager._radios[self._devid].radio.set_data_rate(self._datarate) - _RadioManager._radios[self._devid].radio.set_address(self._address) + @staticmethod + def open(devid: int) -> _SharedRadioInstance: + with _RadioManager._lock: + if len(_RadioManager._radios) <= devid: + padding = [None] * (devid - len(_RadioManager._radios) + 1) + _RadioManager._radios.extend(padding) - return _RadioManager._radios[self._devid].radio + shared_radio = _RadioManager._radios[devid] + if not shared_radio: + shared_radio = _SharedRadio(devid) + _RadioManager._radios[devid] = shared_radio - def __exit__(self, type, value, traceback): - _RadioManager._radios[self._devid].lock.release() + return shared_radio.open_instance() class RadioDriver(CRTPDriver): @@ -128,7 +215,7 @@ class RadioDriver(CRTPDriver): def __init__(self): """ Create the link driver """ CRTPDriver.__init__(self) - self._radio_manager = None + self._radio = None self.uri = '' self.link_error_callback = None self.link_quality_callback = None @@ -151,19 +238,18 @@ def connect(self, uri, link_quality_callback, link_error_callback): devid, channel, datarate, address = self.parse_uri(uri) self.uri = uri - if self._radio_manager is None: - self._radio_manager = _RadioManager(devid, - channel, - datarate, - address) + if self._radio is None: + self._radio = _RadioManager.open(devid) + self._radio.set_channel(channel) + self._radio.set_data_rate(datarate) + self._radio.set_address(address) else: raise Exception('Link already open!') - with self._radio_manager as cradio: - if cradio.version >= 0.4: - cradio.set_arc(_nr_of_arc_retries) - else: - logger.warning('Radio version <0.4 will be obsoleted soon!') + if self._radio.version >= 0.4: + self._radio.set_arc(_nr_of_arc_retries) + else: + logger.warning('Radio version <0.4 will be obsoleted soon!') # Prepare the inter-thread communication queue self.in_queue = queue.Queue() @@ -171,7 +257,7 @@ def connect(self, uri, link_quality_callback, link_error_callback): self.out_queue = queue.Queue(1) # Launch the comm thread - self._thread = _RadioDriverThread(self._radio_manager, + self._thread = _RadioDriverThread(self._radio, self.in_queue, self.out_queue, link_quality_callback, @@ -263,7 +349,7 @@ def restart(self): if self._thread: return - self._thread = _RadioDriverThread(self._radio_manager, self.in_queue, + self._thread = _RadioDriverThread(self._radio, self.in_queue, self.out_queue, self.link_quality_callback, self.link_error_callback, @@ -276,9 +362,9 @@ def close(self): self._thread.stop() # Close the USB dongle - if self._radio_manager: - self._radio_manager.close() - self._radio_manager = None + if self._radio: + self._radio.close() + self._radio = None while not self.out_queue.empty(): self.out_queue.get() @@ -287,9 +373,9 @@ def close(self): self.link_error_callback = None self.link_quality_callback = None - def _scan_radio_channels(self, cradio, start=0, stop=125): + def _scan_radio_channels(self, radio: _SharedRadioInstance, start=0, stop=125): """ Scan for Crazyflies between the supplied channels. """ - return list(cradio.scan_channels(start, stop, (0xff,))) + return list(radio.scan_channels(start, stop, (0xff,))) def scan_selected(self, links): to_scan = () @@ -313,8 +399,7 @@ def scan_selected(self, links): to_scan += (one_to_scan,) - with self._radio_manager as cradio: - found = cradio.scan_selected(to_scan, (0xFF, 0xFF, 0xFF)) + found = self._radio.scan_selected(to_scan, (0xFF, 0xFF, 0xFF)) ret = () for f in found: @@ -333,61 +418,60 @@ def scan_selected(self, links): def scan_interface(self, address): """ Scan interface for Crazyflies """ - if self._radio_manager is None: + if self._radio is None: try: - self._radio_manager = _RadioManager(0) - except Exception: + self._radio = _RadioManager.open(0) + except Exception as e: + print(e) return [] - with self._radio_manager as cradio: - # FIXME: implements serial number in the Crazyradio driver! - serial = 'N/A' + # FIXME: implements serial number in the Crazyradio driver! + serial = 'N/A' - logger.info('v%s dongle with serial %s found', cradio.version, - serial) - found = [] + logger.info('v%s dongle with serial %s found', self._radio.version, + serial) + found = [] - if address is not None: - addr = '{:X}'.format(address) - new_addr = struct.unpack(' Date: Thu, 1 Oct 2020 09:12:02 +0200 Subject: [PATCH 066/861] #118: Fix Flake8 and other style details --- cflib/crtp/radiodriver.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 1bedde166..220acadfa 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -33,15 +33,21 @@ import array import binascii import collections -from enum import Enum import logging import queue -from queue import Queue import re import struct import threading -from threading import Semaphore, Thread -from typing import Any, Dict, Iterable, List, Tuple, Union +from enum import Enum +from queue import Queue +from threading import Semaphore +from threading import Thread +from typing import Any +from typing import Dict +from typing import Iterable +from typing import List +from typing import Tuple +from typing import Union import cflib.drivers.crazyradio as crazyradio from .crtpstack import CRTPPacket @@ -72,7 +78,7 @@ class _RadioCommands(Enum): class _SharedRadioInstance(): def __init__(self, instance_id: int, - cmd_queue: "Queue[Tuple[int, _RadioCommands, Any]]", + cmd_queue: 'Queue[Tuple[int, _RadioCommands, Any]]', rsp_queue: Queue, version: float): self._instance_id = instance_id @@ -373,7 +379,8 @@ def close(self): self.link_error_callback = None self.link_quality_callback = None - def _scan_radio_channels(self, radio: _SharedRadioInstance, start=0, stop=125): + def _scan_radio_channels(self, radio: _SharedRadioInstance, + start=0, stop=125): """ Scan for Crazyflies between the supplied channels. """ return list(radio.scan_channels(start, stop, (0xff,))) From 8d802b4179ecfec6084c7d03b59ee2b979df1229 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 1 Oct 2020 10:30:03 +0200 Subject: [PATCH 067/861] #170: Add Github action build configuration --- .github/workflows/build.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 .github/workflows/build.yml diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 000000000..4c08bf718 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,18 @@ +# Run check and build of the lib using the Bitcraze builder docker image + +name: lib + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + - name: Check and build + run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build From bc8d80ae95a7b9383fdb543fd2e06d7f1605ba90 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 1 Oct 2020 10:34:21 +0200 Subject: [PATCH 068/861] #170: Rename GH action build and add it to Readme --- .github/workflows/build.yml | 2 +- README.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 4c08bf718..df919ef74 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,6 +1,6 @@ # Run check and build of the lib using the Bitcraze builder docker image -name: lib +name: build on: push: diff --git a/README.md b/README.md index e1781db37..1349da58c 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# cflib: Crazyflie python library [![Build Status](https://api.travis-ci.org/bitcraze/crazyflie-lib-python.svg)](https://travis-ci.org/bitcraze/crazyflie-lib-python) +# cflib: Crazyflie python library [![Build Status](https://api.travis-ci.org/bitcraze/crazyflie-lib-python.svg)](https://travis-ci.org/bitcraze/crazyflie-lib-python) ![build](https://github.com/bitcraze/crazyflie-lib-python/workflows/build/badge.svg) cflib is an API written in Python that is used to communicate with the Crazyflie and Crazyflie 2.0 quadcopters. It is intended to be used by client software to From f4ffebcbc15e59b015f97f4ca14767f6a354854c Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 1 Oct 2020 10:38:59 +0200 Subject: [PATCH 069/861] #170: Add proper link to GH action badge --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 1349da58c..1d171cfac 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# cflib: Crazyflie python library [![Build Status](https://api.travis-ci.org/bitcraze/crazyflie-lib-python.svg)](https://travis-ci.org/bitcraze/crazyflie-lib-python) ![build](https://github.com/bitcraze/crazyflie-lib-python/workflows/build/badge.svg) +# cflib: Crazyflie python library [![Build Status](https://api.travis-ci.org/bitcraze/crazyflie-lib-python.svg)](https://travis-ci.org/bitcraze/crazyflie-lib-python) [![build](https://github.com/bitcraze/crazyflie-lib-python/workflows/build/badge.svg)](https://github.com/bitcraze/crazyflie-lib-python/actions) cflib is an API written in Python that is used to communicate with the Crazyflie and Crazyflie 2.0 quadcopters. It is intended to be used by client software to From 044d6aed9f835b5b551cf87e532d0a9a2e6f2aee Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 1 Oct 2020 10:42:36 +0200 Subject: [PATCH 070/861] #170: Check that GH action catches Flake8 errors --- cflib/crtp/radiodriver.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 220acadfa..28f8b66b4 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -379,8 +379,7 @@ def close(self): self.link_error_callback = None self.link_quality_callback = None - def _scan_radio_channels(self, radio: _SharedRadioInstance, - start=0, stop=125): + def _scan_radio_channels(self, radio: _SharedRadioInstance, start=0, stop=125): """ Scan for Crazyflies between the supplied channels. """ return list(radio.scan_channels(start, stop, (0xff,))) From 57ea1a545b660e28f786465464feb8917c9798eb Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 1 Oct 2020 10:42:57 +0200 Subject: [PATCH 071/861] Revert "#170: Check that GH action catches Flake8 errors" This reverts commit 044d6aed9f835b5b551cf87e532d0a9a2e6f2aee. --- cflib/crtp/radiodriver.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 28f8b66b4..220acadfa 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -379,7 +379,8 @@ def close(self): self.link_error_callback = None self.link_quality_callback = None - def _scan_radio_channels(self, radio: _SharedRadioInstance, start=0, stop=125): + def _scan_radio_channels(self, radio: _SharedRadioInstance, + start=0, stop=125): """ Scan for Crazyflies between the supplied channels. """ return list(radio.scan_channels(start, stop, (0xff,))) From 12f2a49427e6342459065410d0fef154abe04a37 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 6 Oct 2020 15:16:16 +0200 Subject: [PATCH 072/861] #118: Close radio when no not used anymore --- cflib/crtp/radiodriver.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 220acadfa..fc1683588 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -145,6 +145,7 @@ class _SharedRadio(Thread): def __init__(self, devid: int): Thread.__init__(self) self._radio = Crazyradio(devid=devid) + self._devid = devid self.version = self._radio.version self._cmd_queue = Queue() # type: Queue[Tuple[int, _RadioCommands, Any]] # noqa @@ -172,7 +173,11 @@ def run(self): if command[1] == _RadioCommands.STOP: with self._lock: - pass + del self._rsp_queues[command[0]] + if len(self._rsp_queues) == 0: + self._radio.close() + _RadioManager.remove(self._devid) + return elif command[1] == _RadioCommands.SEND_PACKET: channel, address, datarate, data = command[2] self._radio.set_channel(channel) @@ -214,6 +219,11 @@ def open(devid: int) -> _SharedRadioInstance: return shared_radio.open_instance() + @staticmethod + def remove(devid: int): + with _RadioManager._lock: + _RadioManager._radios[devid] = None + class RadioDriver(CRTPDriver): """ Crazyradio link driver """ From 9c9ce52e19d3c4f2a1bb49df9ab2639ff3929cf3 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 27 Oct 2020 17:12:34 +0100 Subject: [PATCH 073/861] #173 Clear callbacks in the _clear_state() method of the memory handling module --- cflib/crazyflie/mem.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 9761a73ac..49cf222f4 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -1113,13 +1113,6 @@ class Memory(): def __init__(self, crazyflie=None): """Instantiate class and connect callbacks""" - # Called when new memories have been added - self.mem_added_cb = Caller() - # Called when new data has been read - self.mem_read_cb = Caller() - - self.mem_write_cb = Caller() - self.cf = crazyflie self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) self.cf.disconnected.add_callback(self._disconnected) @@ -1129,6 +1122,12 @@ def __init__(self, crazyflie=None): def _clear_state(self): self.mems = [] + # Called when new memories have been added + self.mem_added_cb = Caller() + # Called when new data has been read + self.mem_read_cb = Caller() + self.mem_write_cb = Caller() + self._refresh_callback = None self._fetch_id = 0 self.nbr_of_mems = 0 From f78a5122b040e3b92197b0029a175294becaaa08 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 5 Nov 2020 13:36:08 +0100 Subject: [PATCH 074/861] #174 Refactoring om memory sub system. Broke it up in multiple files --- cflib/crazyflie/mem.py | 1426 ----------------- cflib/crazyflie/mem/__init__.py | 534 ++++++ cflib/crazyflie/mem/i2c_element.py | 131 ++ cflib/crazyflie/mem/led_driver_memory.py | 106 ++ .../mem/led_timings_driver_memory.py | 88 + cflib/crazyflie/mem/lighthouse_memory.py | 134 ++ cflib/crazyflie/mem/loco_memory.py | 110 ++ cflib/crazyflie/mem/loco_memory_2.py | 176 ++ cflib/crazyflie/mem/memory_element.py | 76 + cflib/crazyflie/mem/memory_tester.py | 102 ++ cflib/crazyflie/mem/ow_element.py | 169 ++ cflib/crazyflie/mem/trajectory_memory.py | 78 + 12 files changed, 1704 insertions(+), 1426 deletions(-) delete mode 100644 cflib/crazyflie/mem.py create mode 100644 cflib/crazyflie/mem/__init__.py create mode 100644 cflib/crazyflie/mem/i2c_element.py create mode 100644 cflib/crazyflie/mem/led_driver_memory.py create mode 100644 cflib/crazyflie/mem/led_timings_driver_memory.py create mode 100644 cflib/crazyflie/mem/lighthouse_memory.py create mode 100644 cflib/crazyflie/mem/loco_memory.py create mode 100644 cflib/crazyflie/mem/loco_memory_2.py create mode 100644 cflib/crazyflie/mem/memory_element.py create mode 100644 cflib/crazyflie/mem/memory_tester.py create mode 100644 cflib/crazyflie/mem/ow_element.py create mode 100644 cflib/crazyflie/mem/trajectory_memory.py diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py deleted file mode 100644 index 49cf222f4..000000000 --- a/cflib/crazyflie/mem.py +++ /dev/null @@ -1,1426 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2011-2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Enables flash access to the Crazyflie. - -""" -import errno -import logging -import struct -from array import array -from binascii import crc32 -from functools import reduce -from threading import Lock - -from cflib.crtp.crtpstack import CRTPPacket -from cflib.crtp.crtpstack import CRTPPort -from cflib.utils.callbacks import Caller - -__author__ = 'Bitcraze AB' -__all__ = ['Memory', 'MemoryElement'] - -# Channels used for the logging port -CHAN_INFO = 0 -CHAN_READ = 1 -CHAN_WRITE = 2 - -# Commands used when accessing the Settings port -CMD_INFO_VER = 0 -CMD_INFO_NBR = 1 -CMD_INFO_DETAILS = 2 - -EEPROM_TOKEN = b'0xBC' - -logger = logging.getLogger(__name__) - - -class MemoryElement(object): - """A memory """ - - TYPE_I2C = 0 - TYPE_1W = 1 - TYPE_DRIVER_LED = 0x10 - TYPE_LOCO = 0x11 - TYPE_TRAJ = 0x12 - TYPE_LOCO2 = 0x13 - TYPE_LH = 0x14 - TYPE_MEMORY_TESTER = 0x15 - TYPE_DRIVER_LEDTIMING = 0x17 - - def __init__(self, id, type, size, mem_handler): - """Initialize the element with default values""" - self.id = id - self.type = type - self.size = size - self.mem_handler = mem_handler - - @staticmethod - def type_to_string(t): - """Get string representation of memory type""" - if t == MemoryElement.TYPE_I2C: - return 'I2C' - if t == MemoryElement.TYPE_1W: - return '1-wire' - if t == MemoryElement.TYPE_DRIVER_LEDTIMING: - return 'LED memory driver' - if t == MemoryElement.TYPE_DRIVER_LED: - return 'LED driver' - if t == MemoryElement.TYPE_LOCO: - return 'Loco Positioning' - if t == MemoryElement.TYPE_TRAJ: - return 'Trajectory' - if t == MemoryElement.TYPE_LOCO2: - return 'Loco Positioning 2' - if t == MemoryElement.TYPE_LH: - return 'Lighthouse positioning' - if t == MemoryElement.TYPE_MEMORY_TESTER: - return 'Memory tester' - return 'Unknown' - - def new_data(self, mem, addr, data): - logger.debug('New data, but not OW mem') - - def __str__(self): - """Generate debug string for memory""" - return ('Memory: id={}, type={}, size={}'.format( - self.id, MemoryElement.type_to_string(self.type), self.size)) - - -class LED: - """Used to set color/intensity of one LED in the LED-ring""" - - def __init__(self): - """Initialize to off""" - self.r = 0 - self.g = 0 - self.b = 0 - self.intensity = 100 - - def set(self, r, g, b, intensity=None): - """Set the R/G/B and optionally intensity in one call""" - self.r = r - self.g = g - self.b = b - if intensity: - self.intensity = intensity - - -class LEDDriverMemory(MemoryElement): - """Memory interface for using the LED-ring mapped memory for setting RGB - values for all the LEDs in the ring""" - - def __init__(self, id, type, size, mem_handler): - """Initialize with 12 LEDs""" - super(LEDDriverMemory, self).__init__(id=id, type=type, size=size, - mem_handler=mem_handler) - self._update_finished_cb = None - self._write_finished_cb = None - - self.leds = [] - for i in range(12): - self.leds.append(LED()) - - def new_data(self, mem, addr, data): - """Callback for when new memory data has been fetched""" - if mem.id == self.id: - logger.debug( - "Got new data from the LED driver, but we don't care.") - - def write_data(self, write_finished_cb): - """Write the saved LED-ring data to the Crazyflie""" - self._write_finished_cb = write_finished_cb - data = bytearray() - for led in self.leds: - # In order to fit all the LEDs in one radio packet RGB565 is used - # to compress the colors. The calculations below converts 3 bytes - # RGB into 2 bytes RGB565. Then shifts the value of each color to - # LSB, applies the intensity and shifts them back for correct - # alignment on 2 bytes. - R5 = ((int)((((int(led.r) & 0xFF) * 249 + 1014) >> 11) & 0x1F) * - led.intensity / 100) - G6 = ((int)((((int(led.g) & 0xFF) * 253 + 505) >> 10) & 0x3F) * - led.intensity / 100) - B5 = ((int)((((int(led.b) & 0xFF) * 249 + 1014) >> 11) & 0x1F) * - led.intensity / 100) - tmp = (int(R5) << 11) | (int(G6) << 5) | (int(B5) << 0) - data += bytearray((tmp >> 8, tmp & 0xFF)) - self.mem_handler.write(self, 0x00, data, flush_queue=True) - - def update(self, update_finished_cb): - """Request an update of the memory content""" - if not self._update_finished_cb: - self._update_finished_cb = update_finished_cb - self.valid = False - logger.debug('Updating content of memory {}'.format(self.id)) - # Start reading the header - self.mem_handler.read(self, 0, 16) - - def write_done(self, mem, addr): - if self._write_finished_cb and mem.id == self.id: - logger.debug('Write to LED driver done') - self._write_finished_cb(self, addr) - self._write_finished_cb = None - - def disconnect(self): - self._update_finished_cb = None - self._write_finished_cb = None - - -class LEDTimingsDriverMemory(MemoryElement): - """Memory interface for using the LED-ring mapped memory for setting RGB - values over time. To upload and run a show sequence of - the LEDs in the ring""" - - def __init__(self, id, type, size, mem_handler): - super(LEDTimingsDriverMemory, self).__init__(id=id, - type=type, - size=size, - mem_handler=mem_handler) - self._update_finished_cb = None - self._write_finished_cb = None - - self.timings = [] - - def add(self, time, rgb, leds=0, fade=False, rotate=0): - self.timings.append({ - 'time': time, - 'rgb': rgb, - 'leds': leds, - 'fade': fade, - 'rotate': rotate - }) - - def write_data(self, write_finished_cb): - if write_finished_cb is not None: - self._write_finished_cb = write_finished_cb - - data = [] - for timing in self.timings: - # In order to fit all the LEDs in one radio packet RGB565 is used - # to compress the colors. The calculations below converts 3 bytes - # RGB into 2 bytes RGB565. Then shifts the value of each color to - # LSB, applies the intensity and shifts them back for correct - # alignment on 2 bytes. - R5 = ((int)((((int(timing['rgb']['r']) & 0xFF) * 249 + 1014) >> 11) - & 0x1F)) - G6 = ((int)((((int(timing['rgb']['g']) & 0xFF) * 253 + 505) >> 10) - & 0x3F)) - B5 = ((int)((((int(timing['rgb']['b']) & 0xFF) * 249 + 1014) >> 11) - & 0x1F)) - led = (int(R5) << 11) | (int(G6) << 5) | (int(B5) << 0) - extra = ((timing['leds']) & 0x0F) | ( - (timing['fade'] << 4) & 0x10) | ( - (timing['rotate'] << 5) & 0xE0) - - if (timing['time'] & 0xFF) != 0 or led != 0 or extra != 0: - data += [timing['time'] & 0xFF, led >> 8, led & 0xFF, extra] - - data += [0, 0, 0, 0] - self.mem_handler.write(self, 0x00, bytearray(data), flush_queue=True) - - def write_done(self, mem, addr): - if mem.id == self.id and self._write_finished_cb: - self._write_finished_cb(self, addr) - self._write_finished_cb = None - - def disconnect(self): - self._update_finished_cb = None - self._write_finished_cb = None - - -class I2CElement(MemoryElement): - - def __init__(self, id, type, size, mem_handler): - super(I2CElement, self).__init__(id=id, type=type, size=size, - mem_handler=mem_handler) - self._update_finished_cb = None - self._write_finished_cb = None - self.elements = {} - self.valid = False - - def new_data(self, mem, addr, data): - """Callback for when new memory data has been fetched""" - if mem.id == self.id: - if addr == 0: - done = False - # Check for header - if data[0:4] == EEPROM_TOKEN: - logger.debug('Got new data: {}'.format(data)) - [self.elements['version'], - self.elements['radio_channel'], - self.elements['radio_speed'], - self.elements['pitch_trim'], - self.elements['roll_trim']] = struct.unpack('> 32, - self.elements['radio_address'] & 0xFFFFFFFF) - image += struct.pack(' 0: - (eid, elen) = struct.unpack('BB', elem_data[:2]) - self.elements[self.element_mapping[eid]] = \ - elem_data[2:2 + elen].decode('ISO-8859-1') - elem_data = elem_data[2 + elen:] - return True - return False - - def write_done(self, mem, addr): - if self._write_finished_cb: - self._write_finished_cb(self, addr) - self._write_finished_cb = None - - def write_data(self, write_finished_cb): - # First generate the header part - header_data = struct.pack(' 0: - self._update_data_finished_cb = update_data_finished_cb - self.anchor_data = {} - - self.data_valid = False - self._nr_of_anchors_to_fetch = self.nr_of_anchors - - logger.debug('Updating anchor data of memory {}'.format(self.id)) - - # Start reading the first anchor - self._currently_fetching_index = 0 - self._request_page(self.anchor_ids[self._currently_fetching_index]) - - def disconnect(self): - self._update_ids_finished_cb = None - self._update_data_finished_cb = None - - def _handle_id_list_data(self, data): - self.nr_of_anchors = data[0] - for i in range(self.nr_of_anchors): - self.anchor_ids.append(data[1 + i]) - self.ids_valid = True - - if self._update_ids_finished_cb: - self._update_ids_finished_cb(self) - self._update_ids_finished_cb = None - - def _handle_active_id_list_data(self, data): - count = data[0] - for i in range(count): - self.active_anchor_ids.append(data[1 + i]) - self.active_ids_valid = True - - if self._update_active_ids_finished_cb: - self._update_active_ids_finished_cb(self) - self._update_active_ids_finished_cb = None - - def _handle_anchor_data(self, id, data): - anchor = AnchorData2() - anchor.set_from_mem_data(data) - self.anchor_data[id] = anchor - - self._currently_fetching_index += 1 - if self._currently_fetching_index < self.nr_of_anchors: - self._request_page(self.anchor_ids[self._currently_fetching_index]) - else: - self.data_valid = True - if self._update_data_finished_cb: - self._update_data_finished_cb(self) - self._update_data_finished_cb = None - - def _request_page(self, page): - addr = LocoMemory2.ADR_ANCHOR_BASE + \ - LocoMemory2.ANCHOR_PAGE_SIZE * page - self.mem_handler.read(self, addr, LocoMemory2.PAGE_LEN) - - -class Poly4D: - class Poly: - def __init__(self, values=[0.0] * 8): - self.values = values - - def __init__(self, duration, x=None, y=None, z=None, yaw=None): - self.duration = duration - self.x = x if x else self.Poly() - self.y = y if y else self.Poly() - self.z = z if z else self.Poly() - self.yaw = yaw if yaw else self.Poly() - - -class TrajectoryMemory(MemoryElement): - """ - Memory interface for trajectories used by the high level commander - """ - - def __init__(self, id, type, size, mem_handler): - """Initialize trajectory memory""" - super(TrajectoryMemory, self).__init__(id=id, type=type, size=size, - mem_handler=mem_handler) - self._write_finished_cb = None - - # A list of Poly4D objects to write to the Crazyflie - self.poly4Ds = [] - - def write_data(self, write_finished_cb): - """Write trajectory data to the Crazyflie""" - self._write_finished_cb = write_finished_cb - data = bytearray() - - for poly4D in self.poly4Ds: - data += struct.pack(' _ReadRequest.MAX_DATA_LENGTH: - new_len = _ReadRequest.MAX_DATA_LENGTH - - logger.debug('Requesting new chunk of {}bytes at 0x{:X}'.format( - new_len, self._current_addr)) - - # Request the data for the next address - pk = CRTPPacket() - pk.set_header(CRTPPort.MEM, CHAN_READ) - pk.data = struct.pack(' 0: - self._request_new_chunk() - return False - else: - return True - - -class _WriteRequest: - """ - Class used to handle memory reads that will split up the read in multiple - packets in necessary - """ - MAX_DATA_LENGTH = 25 - - def __init__(self, mem, addr, data, cf): - """Initialize the object with good defaults""" - self.mem = mem - self.addr = addr - self._bytes_left = len(data) - self._data = data - self.data = bytearray() - self.cf = cf - - self._current_addr = addr - - self._sent_packet = None - self._sent_reply = None - - self._addr_add = 0 - - def start(self): - """Start the fetching of the data""" - self._write_new_chunk() - - def resend(self): - logger.debug('Sending write again...') - self.cf.send_packet( - self._sent_packet, expected_reply=self._sent_reply, timeout=1) - - def _write_new_chunk(self): - """ - Called to request a new chunk of data to be read from the Crazyflie - """ - # Figure out the length of the next request - new_len = len(self._data) - if new_len > _WriteRequest.MAX_DATA_LENGTH: - new_len = _WriteRequest.MAX_DATA_LENGTH - - logger.debug('Writing new chunk of {}bytes at 0x{:X}'.format( - new_len, self._current_addr)) - - data = self._data[:new_len] - self._data = self._data[new_len:] - - pk = CRTPPacket() - pk.set_header(CRTPPort.MEM, CHAN_WRITE) - pk.data = struct.pack(' 0: - self._current_addr += self._addr_add - self._write_new_chunk() - return False - else: - logger.debug('This write request is done') - return True - - -class Memory(): - """Access memories on the Crazyflie""" - - # These codes can be decoded using os.stderror, but - # some of the text messages will look very strange - # in the UI, so they are redefined here - _err_codes = { - errno.ENOMEM: 'No more memory available', - errno.ENOEXEC: 'Command not found', - errno.ENOENT: 'No such block id', - errno.E2BIG: 'Block too large', - errno.EEXIST: 'Block already exists' - } - - def __init__(self, crazyflie=None): - """Instantiate class and connect callbacks""" - self.cf = crazyflie - self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) - self.cf.disconnected.add_callback(self._disconnected) - self._write_requests_lock = Lock() - - self._clear_state() - - def _clear_state(self): - self.mems = [] - # Called when new memories have been added - self.mem_added_cb = Caller() - # Called when new data has been read - self.mem_read_cb = Caller() - self.mem_write_cb = Caller() - - self._refresh_callback = None - self._fetch_id = 0 - self.nbr_of_mems = 0 - self._ow_mem_fetch_index = 0 - self._elem_data = () - self._read_requests = {} - self._write_requests = {} - self._ow_mems_left_to_update = [] - self._getting_count = False - - def _mem_update_done(self, mem): - """ - Callback from each individual memory (only 1-wire) when reading of - header/elements are done - """ - if mem.id in self._ow_mems_left_to_update: - self._ow_mems_left_to_update.remove(mem.id) - - logger.debug(mem) - - if len(self._ow_mems_left_to_update) == 0: - if self._refresh_callback: - self._refresh_callback() - self._refresh_callback = None - - def get_mem(self, id): - """Fetch the memory with the supplied id""" - for m in self.mems: - if m.id == id: - return m - - return None - - def get_mems(self, type): - """Fetch all the memories of the supplied type""" - ret = () - for m in self.mems: - if m.type == type: - ret += (m,) - - return ret - - def ow_search(self, vid=0xBC, pid=None, name=None): - """Search for specific memory id/name and return it""" - for m in self.get_mems(MemoryElement.TYPE_1W): - if pid and m.pid == pid or name and m.name == name: - return m - - return None - - def write(self, memory, addr, data, flush_queue=False): - """Write the specified data to the given memory at the given address""" - wreq = _WriteRequest(memory, addr, data, self.cf) - if memory.id not in self._write_requests: - self._write_requests[memory.id] = [] - - # Workaround until we secure the uplink and change messages for - # mems to non-blocking - self._write_requests_lock.acquire() - if flush_queue: - self._write_requests[memory.id] = self._write_requests[ - memory.id][:1] - self._write_requests[memory.id].insert(len(self._write_requests), wreq) - if len(self._write_requests[memory.id]) == 1: - wreq.start() - self._write_requests_lock.release() - - return True - - def read(self, memory, addr, length): - """ - Read the specified amount of bytes from the given memory at the given - address - """ - if memory.id in self._read_requests: - logger.warning('There is already a read operation ongoing for ' - 'memory id {}'.format(memory.id)) - return False - - rreq = _ReadRequest(memory, addr, length, self.cf) - self._read_requests[memory.id] = rreq - - rreq.start() - - return True - - def refresh(self, refresh_done_callback): - """Start fetching all the detected memories""" - self._refresh_callback = refresh_done_callback - self._fetch_id = 0 - for m in self.mems: - try: - self.mem_read_cb.remove_callback(m.new_data) - m.disconnect() - except Exception as e: - logger.info( - 'Error when removing memory after update: {}'.format(e)) - self.mems = [] - - self.nbr_of_mems = 0 - self._getting_count = False - - logger.debug('Requesting number of memories') - pk = CRTPPacket() - pk.set_header(CRTPPort.MEM, CHAN_INFO) - pk.data = (CMD_INFO_NBR,) - self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) - - def _disconnected(self, uri): - """The link to the Crazyflie has been broken. Reset state""" - self._clear_state() - - def _new_packet_cb(self, packet): - """Callback for newly arrived packets for the memory port""" - chan = packet.channel - cmd = packet.data[0] - payload = packet.data[1:] - - if chan == CHAN_INFO: - if cmd == CMD_INFO_NBR: - self.nbr_of_mems = payload[0] - logger.info('{} memories found'.format(self.nbr_of_mems)) - - # Start requesting information about the memories, - # if there are any... - if self.nbr_of_mems > 0: - if not self._getting_count: - self._getting_count = True - logger.debug('Requesting first id') - pk = CRTPPacket() - pk.set_header(CRTPPort.MEM, CHAN_INFO) - pk.data = (CMD_INFO_DETAILS, 0) - self.cf.send_packet(pk, expected_reply=( - CMD_INFO_DETAILS, 0)) - else: - self._refresh_callback() - - if cmd == CMD_INFO_DETAILS: - - # Did we get a good reply, otherwise try again: - if len(payload) < 5: - # Workaround for 1-wire bug when memory is detected - # but updating the info crashes the communication with - # the 1-wire. Fail by saying we only found 1 memory - # (the I2C). - logger.error( - '-------->Got good count, but no info on mem!') - self.nbr_of_mems = 1 - if self._refresh_callback: - self._refresh_callback() - self._refresh_callback = None - return - - # Create information about a new memory - # Id - 1 byte - mem_id = payload[0] - # Type - 1 byte - mem_type = payload[1] - # Size 4 bytes (as addr) - mem_size = struct.unpack('I', payload[2:6])[0] - # Addr (only valid for 1-wire?) - mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) - mem_addr = '' - for m in mem_addr_raw: - mem_addr += '{:02X}'.format(m) - - if (not self.get_mem(mem_id)): - if mem_type == MemoryElement.TYPE_1W: - mem = OWElement(id=mem_id, type=mem_type, - size=mem_size, - addr=mem_addr, mem_handler=self) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - self._ow_mems_left_to_update.append(mem.id) - elif mem_type == MemoryElement.TYPE_I2C: - mem = I2CElement(id=mem_id, type=mem_type, - size=mem_size, - mem_handler=self) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_DRIVER_LED: - mem = LEDDriverMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_LOCO: - mem = LocoMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - elif mem_type == MemoryElement.TYPE_TRAJ: - mem = TrajectoryMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_LOCO2: - mem = LocoMemory2(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - elif mem_type == MemoryElement.TYPE_LH: - mem = LighthouseMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_MEMORY_TESTER: - mem = MemoryTester(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_DRIVER_LEDTIMING: - mem = LEDTimingsDriverMemory(id=mem_id, type=mem_type, - size=mem_size, - mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - else: - mem = MemoryElement(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mems.append(mem) - self.mem_added_cb.call(mem) - - self._fetch_id = mem_id + 1 - - if self.nbr_of_mems - 1 >= self._fetch_id: - logger.debug( - 'Requesting information about memory {}'.format( - self._fetch_id)) - pk = CRTPPacket() - pk.set_header(CRTPPort.MEM, CHAN_INFO) - pk.data = (CMD_INFO_DETAILS, self._fetch_id) - self.cf.send_packet(pk, expected_reply=( - CMD_INFO_DETAILS, self._fetch_id)) - else: - logger.debug( - 'Done getting all the memories, start reading the OWs') - ows = self.get_mems(MemoryElement.TYPE_1W) - # If there are any OW mems start reading them, otherwise - # we are done - for ow_mem in ows: - ow_mem.update(self._mem_update_done) - if len(ows) == 0: - if self._refresh_callback: - self._refresh_callback() - self._refresh_callback = None - - if chan == CHAN_WRITE: - id = cmd - (addr, status) = struct.unpack(' 0: - self._write_requests[id][0].start() - else: - logger.debug( - 'Status {}: write resending...'.format(status)) - wreq.resend() - self._write_requests_lock.release() - - if chan == CHAN_READ: - id = cmd - (addr, status) = struct.unpack(' _ReadRequest.MAX_DATA_LENGTH: + new_len = _ReadRequest.MAX_DATA_LENGTH + + logger.debug('Requesting new chunk of {}bytes at 0x{:X}'.format( + new_len, self._current_addr)) + + # Request the data for the next address + pk = CRTPPacket() + pk.set_header(CRTPPort.MEM, CHAN_READ) + pk.data = struct.pack(' 0: + self._request_new_chunk() + return False + else: + return True + + +class _WriteRequest: + """ + Class used to handle memory reads that will split up the read in multiple + packets in necessary + """ + MAX_DATA_LENGTH = 25 + + def __init__(self, mem, addr, data, cf): + """Initialize the object with good defaults""" + self.mem = mem + self.addr = addr + self._bytes_left = len(data) + self._data = data + self.data = bytearray() + self.cf = cf + + self._current_addr = addr + + self._sent_packet = None + self._sent_reply = None + + self._addr_add = 0 + + def start(self): + """Start the fetching of the data""" + self._write_new_chunk() + + def resend(self): + logger.debug('Sending write again...') + self.cf.send_packet( + self._sent_packet, expected_reply=self._sent_reply, timeout=1) + + def _write_new_chunk(self): + """ + Called to request a new chunk of data to be read from the Crazyflie + """ + # Figure out the length of the next request + new_len = len(self._data) + if new_len > _WriteRequest.MAX_DATA_LENGTH: + new_len = _WriteRequest.MAX_DATA_LENGTH + + logger.debug('Writing new chunk of {}bytes at 0x{:X}'.format( + new_len, self._current_addr)) + + data = self._data[:new_len] + self._data = self._data[new_len:] + + pk = CRTPPacket() + pk.set_header(CRTPPort.MEM, CHAN_WRITE) + pk.data = struct.pack(' 0: + self._current_addr += self._addr_add + self._write_new_chunk() + return False + else: + logger.debug('This write request is done') + return True + + +class Memory(): + """Access memories on the Crazyflie""" + + # These codes can be decoded using os.stderror, but + # some of the text messages will look very strange + # in the UI, so they are redefined here + _err_codes = { + errno.ENOMEM: 'No more memory available', + errno.ENOEXEC: 'Command not found', + errno.ENOENT: 'No such block id', + errno.E2BIG: 'Block too large', + errno.EEXIST: 'Block already exists' + } + + def __init__(self, crazyflie=None): + """Instantiate class and connect callbacks""" + self.cf = crazyflie + self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) + self.cf.disconnected.add_callback(self._disconnected) + self._write_requests_lock = Lock() + + self._clear_state() + + def _clear_state(self): + self.mems = [] + # Called when new memories have been added + self.mem_added_cb = Caller() + # Called when new data has been read + self.mem_read_cb = Caller() + self.mem_write_cb = Caller() + + self._refresh_callback = None + self._fetch_id = 0 + self.nbr_of_mems = 0 + self._ow_mem_fetch_index = 0 + self._elem_data = () + self._read_requests = {} + self._write_requests = {} + self._ow_mems_left_to_update = [] + self._getting_count = False + + def _mem_update_done(self, mem): + """ + Callback from each individual memory (only 1-wire) when reading of + header/elements are done + """ + if mem.id in self._ow_mems_left_to_update: + self._ow_mems_left_to_update.remove(mem.id) + + logger.debug(mem) + + if len(self._ow_mems_left_to_update) == 0: + if self._refresh_callback: + self._refresh_callback() + self._refresh_callback = None + + def get_mem(self, id): + """Fetch the memory with the supplied id""" + for m in self.mems: + if m.id == id: + return m + + return None + + def get_mems(self, type): + """Fetch all the memories of the supplied type""" + ret = () + for m in self.mems: + if m.type == type: + ret += (m,) + + return ret + + def ow_search(self, vid=0xBC, pid=None, name=None): + """Search for specific memory id/name and return it""" + for m in self.get_mems(MemoryElement.TYPE_1W): + if pid and m.pid == pid or name and m.name == name: + return m + + return None + + def write(self, memory, addr, data, flush_queue=False): + """Write the specified data to the given memory at the given address""" + wreq = _WriteRequest(memory, addr, data, self.cf) + if memory.id not in self._write_requests: + self._write_requests[memory.id] = [] + + # Workaround until we secure the uplink and change messages for + # mems to non-blocking + self._write_requests_lock.acquire() + if flush_queue: + self._write_requests[memory.id] = self._write_requests[ + memory.id][:1] + self._write_requests[memory.id].insert(len(self._write_requests), wreq) + if len(self._write_requests[memory.id]) == 1: + wreq.start() + self._write_requests_lock.release() + + return True + + def read(self, memory, addr, length): + """ + Read the specified amount of bytes from the given memory at the given + address + """ + if memory.id in self._read_requests: + logger.warning('There is already a read operation ongoing for ' + 'memory id {}'.format(memory.id)) + return False + + rreq = _ReadRequest(memory, addr, length, self.cf) + self._read_requests[memory.id] = rreq + + rreq.start() + + return True + + def refresh(self, refresh_done_callback): + """Start fetching all the detected memories""" + self._refresh_callback = refresh_done_callback + self._fetch_id = 0 + for m in self.mems: + try: + self.mem_read_cb.remove_callback(m.new_data) + m.disconnect() + except Exception as e: + logger.info( + 'Error when removing memory after update: {}'.format(e)) + self.mems = [] + + self.nbr_of_mems = 0 + self._getting_count = False + + logger.debug('Requesting number of memories') + pk = CRTPPacket() + pk.set_header(CRTPPort.MEM, CHAN_INFO) + pk.data = (CMD_INFO_NBR,) + self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) + + def _disconnected(self, uri): + """The link to the Crazyflie has been broken. Reset state""" + self._clear_state() + + def _new_packet_cb(self, packet): + """Callback for newly arrived packets for the memory port""" + chan = packet.channel + cmd = packet.data[0] + payload = packet.data[1:] + + if chan == CHAN_INFO: + if cmd == CMD_INFO_NBR: + self.nbr_of_mems = payload[0] + logger.info('{} memories found'.format(self.nbr_of_mems)) + + # Start requesting information about the memories, + # if there are any... + if self.nbr_of_mems > 0: + if not self._getting_count: + self._getting_count = True + logger.debug('Requesting first id') + pk = CRTPPacket() + pk.set_header(CRTPPort.MEM, CHAN_INFO) + pk.data = (CMD_INFO_DETAILS, 0) + self.cf.send_packet(pk, expected_reply=( + CMD_INFO_DETAILS, 0)) + else: + self._refresh_callback() + + if cmd == CMD_INFO_DETAILS: + + # Did we get a good reply, otherwise try again: + if len(payload) < 5: + # Workaround for 1-wire bug when memory is detected + # but updating the info crashes the communication with + # the 1-wire. Fail by saying we only found 1 memory + # (the I2C). + logger.error( + '-------->Got good count, but no info on mem!') + self.nbr_of_mems = 1 + if self._refresh_callback: + self._refresh_callback() + self._refresh_callback = None + return + + # Create information about a new memory + # Id - 1 byte + mem_id = payload[0] + # Type - 1 byte + mem_type = payload[1] + # Size 4 bytes (as addr) + mem_size = struct.unpack('I', payload[2:6])[0] + # Addr (only valid for 1-wire?) + mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) + mem_addr = '' + for m in mem_addr_raw: + mem_addr += '{:02X}'.format(m) + + if (not self.get_mem(mem_id)): + if mem_type == MemoryElement.TYPE_1W: + mem = OWElement(id=mem_id, type=mem_type, + size=mem_size, + addr=mem_addr, mem_handler=self) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + self._ow_mems_left_to_update.append(mem.id) + elif mem_type == MemoryElement.TYPE_I2C: + mem = I2CElement(id=mem_id, type=mem_type, + size=mem_size, + mem_handler=self) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_DRIVER_LED: + mem = LEDDriverMemory(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_LOCO: + mem = LocoMemory(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + elif mem_type == MemoryElement.TYPE_TRAJ: + mem = TrajectoryMemory(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_LOCO2: + mem = LocoMemory2(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + elif mem_type == MemoryElement.TYPE_LH: + mem = LighthouseMemory(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_MEMORY_TESTER: + mem = MemoryTester(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_DRIVER_LEDTIMING: + mem = LEDTimingsDriverMemory(id=mem_id, type=mem_type, + size=mem_size, + mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + else: + mem = MemoryElement(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mems.append(mem) + self.mem_added_cb.call(mem) + + self._fetch_id = mem_id + 1 + + if self.nbr_of_mems - 1 >= self._fetch_id: + logger.debug( + 'Requesting information about memory {}'.format( + self._fetch_id)) + pk = CRTPPacket() + pk.set_header(CRTPPort.MEM, CHAN_INFO) + pk.data = (CMD_INFO_DETAILS, self._fetch_id) + self.cf.send_packet(pk, expected_reply=( + CMD_INFO_DETAILS, self._fetch_id)) + else: + logger.debug( + 'Done getting all the memories, start reading the OWs') + ows = self.get_mems(MemoryElement.TYPE_1W) + # If there are any OW mems start reading them, otherwise + # we are done + for ow_mem in ows: + ow_mem.update(self._mem_update_done) + if len(ows) == 0: + if self._refresh_callback: + self._refresh_callback() + self._refresh_callback = None + + if chan == CHAN_WRITE: + id = cmd + (addr, status) = struct.unpack(' 0: + self._write_requests[id][0].start() + else: + logger.debug( + 'Status {}: write resending...'.format(status)) + wreq.resend() + self._write_requests_lock.release() + + if chan == CHAN_READ: + id = cmd + (addr, status) = struct.unpack('. +import logging +import struct +from functools import reduce + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +EEPROM_TOKEN = b'0xBC' + + +class I2CElement(MemoryElement): + + def __init__(self, id, type, size, mem_handler): + super(I2CElement, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + self._update_finished_cb = None + self._write_finished_cb = None + self.elements = {} + self.valid = False + + def new_data(self, mem, addr, data): + """Callback for when new memory data has been fetched""" + if mem.id == self.id: + if addr == 0: + done = False + # Check for header + if data[0:4] == EEPROM_TOKEN: + logger.debug('Got new data: {}'.format(data)) + [self.elements['version'], + self.elements['radio_channel'], + self.elements['radio_speed'], + self.elements['pitch_trim'], + self.elements['roll_trim']] = struct.unpack('> 32, + self.elements['radio_address'] & 0xFFFFFFFF) + image += struct.pack('. +import logging + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class LED: + """Used to set color/intensity of one LED in the LED-ring""" + + def __init__(self): + """Initialize to off""" + self.r = 0 + self.g = 0 + self.b = 0 + self.intensity = 100 + + def set(self, r, g, b, intensity=None): + """Set the R/G/B and optionally intensity in one call""" + self.r = r + self.g = g + self.b = b + if intensity: + self.intensity = intensity + + +class LEDDriverMemory(MemoryElement): + """Memory interface for using the LED-ring mapped memory for setting RGB + values for all the LEDs in the ring""" + + def __init__(self, id, type, size, mem_handler): + """Initialize with 12 LEDs""" + super(LEDDriverMemory, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + self._update_finished_cb = None + self._write_finished_cb = None + + self.leds = [] + for i in range(12): + self.leds.append(LED()) + + def new_data(self, mem, addr, data): + """Callback for when new memory data has been fetched""" + if mem.id == self.id: + logger.debug( + "Got new data from the LED driver, but we don't care.") + + def write_data(self, write_finished_cb): + """Write the saved LED-ring data to the Crazyflie""" + self._write_finished_cb = write_finished_cb + data = bytearray() + for led in self.leds: + # In order to fit all the LEDs in one radio packet RGB565 is used + # to compress the colors. The calculations below converts 3 bytes + # RGB into 2 bytes RGB565. Then shifts the value of each color to + # LSB, applies the intensity and shifts them back for correct + # alignment on 2 bytes. + R5 = ((int)((((int(led.r) & 0xFF) * 249 + 1014) >> 11) & 0x1F) * + led.intensity / 100) + G6 = ((int)((((int(led.g) & 0xFF) * 253 + 505) >> 10) & 0x3F) * + led.intensity / 100) + B5 = ((int)((((int(led.b) & 0xFF) * 249 + 1014) >> 11) & 0x1F) * + led.intensity / 100) + tmp = (int(R5) << 11) | (int(G6) << 5) | (int(B5) << 0) + data += bytearray((tmp >> 8, tmp & 0xFF)) + self.mem_handler.write(self, 0x00, data, flush_queue=True) + + def update(self, update_finished_cb): + """Request an update of the memory content""" + if not self._update_finished_cb: + self._update_finished_cb = update_finished_cb + self.valid = False + logger.debug('Updating content of memory {}'.format(self.id)) + # Start reading the header + self.mem_handler.read(self, 0, 16) + + def write_done(self, mem, addr): + if self._write_finished_cb and mem.id == self.id: + logger.debug('Write to LED driver done') + self._write_finished_cb(self, addr) + self._write_finished_cb = None + + def disconnect(self): + self._update_finished_cb = None + self._write_finished_cb = None diff --git a/cflib/crazyflie/mem/led_timings_driver_memory.py b/cflib/crazyflie/mem/led_timings_driver_memory.py new file mode 100644 index 000000000..50e34ffab --- /dev/null +++ b/cflib/crazyflie/mem/led_timings_driver_memory.py @@ -0,0 +1,88 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2019 - 2020 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 logging + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class LEDTimingsDriverMemory(MemoryElement): + """Memory interface for using the LED-ring mapped memory for setting RGB + values over time. To upload and run a show sequence of + the LEDs in the ring""" + + def __init__(self, id, type, size, mem_handler): + super(LEDTimingsDriverMemory, self).__init__(id=id, + type=type, + size=size, + mem_handler=mem_handler) + self._update_finished_cb = None + self._write_finished_cb = None + + self.timings = [] + + def add(self, time, rgb, leds=0, fade=False, rotate=0): + self.timings.append({ + 'time': time, + 'rgb': rgb, + 'leds': leds, + 'fade': fade, + 'rotate': rotate + }) + + def write_data(self, write_finished_cb): + if write_finished_cb is not None: + self._write_finished_cb = write_finished_cb + + data = [] + for timing in self.timings: + # In order to fit all the LEDs in one radio packet RGB565 is used + # to compress the colors. The calculations below converts 3 bytes + # RGB into 2 bytes RGB565. Then shifts the value of each color to + # LSB, applies the intensity and shifts them back for correct + # alignment on 2 bytes. + R5 = ((int)((((int(timing['rgb']['r']) & 0xFF) * 249 + 1014) >> 11) + & 0x1F)) + G6 = ((int)((((int(timing['rgb']['g']) & 0xFF) * 253 + 505) >> 10) + & 0x3F)) + B5 = ((int)((((int(timing['rgb']['b']) & 0xFF) * 249 + 1014) >> 11) + & 0x1F)) + led = (int(R5) << 11) | (int(G6) << 5) | (int(B5) << 0) + extra = ((timing['leds']) & 0x0F) | ( + (timing['fade'] << 4) & 0x10) | ( + (timing['rotate'] << 5) & 0xE0) + + if (timing['time'] & 0xFF) != 0 or led != 0 or extra != 0: + data += [timing['time'] & 0xFF, led >> 8, led & 0xFF, extra] + + data += [0, 0, 0, 0] + self.mem_handler.write(self, 0x00, bytearray(data), flush_queue=True) + + def write_done(self, mem, addr): + if mem.id == self.id and self._write_finished_cb: + self._write_finished_cb(self, addr) + self._write_finished_cb = None + + def disconnect(self): + self._update_finished_cb = None + self._write_finished_cb = None diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py new file mode 100644 index 000000000..27a5c2ec7 --- /dev/null +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -0,0 +1,134 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2019 - 2020 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 logging +import struct + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class LighthouseBsGeometry: + """Container for geometry data of one Lighthouse base station""" + + SIZE_FLOAT = 4 + SIZE_VECTOR = 3 * SIZE_FLOAT + SIZE_GEOMETRY = (1 + 3) * SIZE_VECTOR + SIZE_DATA = 2 * SIZE_GEOMETRY + + def __init__(self): + self.origin = [0.0, 0.0, 0.0] + self.rotation_matrix = [ + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + ] + + def set_from_mem_data(self, data): + self.origin = self._read_vector( + data[0 * self.SIZE_VECTOR:1 * self.SIZE_VECTOR]) + self.rotation_matrix = [ + self._read_vector(data[1 * self.SIZE_VECTOR:2 * self.SIZE_VECTOR]), + self._read_vector(data[2 * self.SIZE_VECTOR:3 * self.SIZE_VECTOR]), + self._read_vector(data[3 * self.SIZE_VECTOR:4 * self.SIZE_VECTOR]), + ] + + def add_mem_data(self, data): + self._add_vector(data, self.origin) + self._add_vector(data, self.rotation_matrix[0]) + self._add_vector(data, self.rotation_matrix[1]) + self._add_vector(data, self.rotation_matrix[2]) + + def _add_vector(self, data, vector): + data += struct.pack('. +import logging +import struct + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class AnchorData: + """Holds data for one anchor""" + + def __init__(self, position=(0.0, 0.0, 0.0), is_valid=False): + self.position = position + self.is_valid = is_valid + + def set_from_mem_data(self, data): + x, y, z, self.is_valid = struct.unpack('. +import logging +import struct + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class AnchorData2: + """Holds data for one anchor""" + + def __init__(self, position=(0.0, 0.0, 0.0), is_valid=False): + self.position = position + self.is_valid = is_valid + + def set_from_mem_data(self, data): + x, y, z, self.is_valid = struct.unpack(' 0: + self._update_data_finished_cb = update_data_finished_cb + self.anchor_data = {} + + self.data_valid = False + self._nr_of_anchors_to_fetch = self.nr_of_anchors + + logger.debug('Updating anchor data of memory {}'.format(self.id)) + + # Start reading the first anchor + self._currently_fetching_index = 0 + self._request_page(self.anchor_ids[self._currently_fetching_index]) + + def disconnect(self): + self._update_ids_finished_cb = None + self._update_data_finished_cb = None + + def _handle_id_list_data(self, data): + self.nr_of_anchors = data[0] + for i in range(self.nr_of_anchors): + self.anchor_ids.append(data[1 + i]) + self.ids_valid = True + + if self._update_ids_finished_cb: + self._update_ids_finished_cb(self) + self._update_ids_finished_cb = None + + def _handle_active_id_list_data(self, data): + count = data[0] + for i in range(count): + self.active_anchor_ids.append(data[1 + i]) + self.active_ids_valid = True + + if self._update_active_ids_finished_cb: + self._update_active_ids_finished_cb(self) + self._update_active_ids_finished_cb = None + + def _handle_anchor_data(self, id, data): + anchor = AnchorData2() + anchor.set_from_mem_data(data) + self.anchor_data[id] = anchor + + self._currently_fetching_index += 1 + if self._currently_fetching_index < self.nr_of_anchors: + self._request_page(self.anchor_ids[self._currently_fetching_index]) + else: + self.data_valid = True + if self._update_data_finished_cb: + self._update_data_finished_cb(self) + self._update_data_finished_cb = None + + def _request_page(self, page): + addr = LocoMemory2.ADR_ANCHOR_BASE + \ + LocoMemory2.ANCHOR_PAGE_SIZE * page + self.mem_handler.read(self, addr, LocoMemory2.PAGE_LEN) diff --git a/cflib/crazyflie/mem/memory_element.py b/cflib/crazyflie/mem/memory_element.py new file mode 100644 index 000000000..b0d7ca5a5 --- /dev/null +++ b/cflib/crazyflie/mem/memory_element.py @@ -0,0 +1,76 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2019 - 2020 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 logging + +logger = logging.getLogger(__name__) + + +class MemoryElement(object): + """A memory """ + + TYPE_I2C = 0 + TYPE_1W = 1 + TYPE_DRIVER_LED = 0x10 + TYPE_LOCO = 0x11 + TYPE_TRAJ = 0x12 + TYPE_LOCO2 = 0x13 + TYPE_LH = 0x14 + TYPE_MEMORY_TESTER = 0x15 + TYPE_DRIVER_LEDTIMING = 0x17 + + def __init__(self, id, type, size, mem_handler): + """Initialize the element with default values""" + self.id = id + self.type = type + self.size = size + self.mem_handler = mem_handler + + @staticmethod + def type_to_string(t): + """Get string representation of memory type""" + if t == MemoryElement.TYPE_I2C: + return 'I2C' + if t == MemoryElement.TYPE_1W: + return '1-wire' + if t == MemoryElement.TYPE_DRIVER_LEDTIMING: + return 'LED memory driver' + if t == MemoryElement.TYPE_DRIVER_LED: + return 'LED driver' + if t == MemoryElement.TYPE_LOCO: + return 'Loco Positioning' + if t == MemoryElement.TYPE_TRAJ: + return 'Trajectory' + if t == MemoryElement.TYPE_LOCO2: + return 'Loco Positioning 2' + if t == MemoryElement.TYPE_LH: + return 'Lighthouse positioning' + if t == MemoryElement.TYPE_MEMORY_TESTER: + return 'Memory tester' + return 'Unknown' + + def new_data(self, mem, addr, data): + logger.debug('New data, but not OW mem') + + def __str__(self): + """Generate debug string for memory""" + return ('Memory: id={}, type={}, size={}'.format( + self.id, MemoryElement.type_to_string(self.type), self.size)) diff --git a/cflib/crazyflie/mem/memory_tester.py b/cflib/crazyflie/mem/memory_tester.py new file mode 100644 index 000000000..a685838d7 --- /dev/null +++ b/cflib/crazyflie/mem/memory_tester.py @@ -0,0 +1,102 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2019 - 2020 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 logging +import struct + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class MemoryTester(MemoryElement): + """ + Memory interface for testing the memory sub system, end to end. + + Usage + 1. To verify reading: + * Call read_data() + * Wait for the callback to be called + * Verify that readValidationSucess is True + + 2. To verify writing: + * Set the parameter 'memTst.resetW' in the CF + * call write_data() + * Wait for the callback + * Read the log var 'memTst.errCntW' from the CF and validate that it + is 0 + """ + + def __init__(self, id, type, size, mem_handler): + """Initialize Memory tester""" + super(MemoryTester, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + + self._update_finished_cb = None + self._write_finished_cb = None + + self.readValidationSucess = True + + def new_data(self, mem, start_address, data): + """Callback for when new memory data has been fetched""" + if mem.id == self.id: + for i in range(len(data)): + actualValue = struct.unpack('. +import array +import logging +import struct +from binascii import crc32 + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class OWElement(MemoryElement): + """Memory class with extra functionality for 1-wire memories""" + + element_mapping = { + 1: 'Board name', + 2: 'Board revision', + 3: 'Custom' + } + + def __init__(self, id, type, size, addr, mem_handler): + """Initialize the memory with good defaults""" + super(OWElement, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + self.addr = addr + + self.valid = False + + self.vid = None + self.pid = None + self.name = None + self.pins = None + self.elements = {} + + self._update_finished_cb = None + self._write_finished_cb = None + + self._rev_element_mapping = {} + for key in list(OWElement.element_mapping.keys()): + self._rev_element_mapping[OWElement.element_mapping[key]] = key + + def new_data(self, mem, addr, data): + """Callback for when new memory data has been fetched""" + if mem.id == self.id: + if addr == 0: + if self._parse_and_check_header(data[0:8]): + if self._parse_and_check_elements(data[9:11]): + self.valid = True + self._update_finished_cb(self) + self._update_finished_cb = None + else: + # We need to fetch the elements, find out the length + (elem_ver, elem_len) = struct.unpack('BB', data[8:10]) + self.mem_handler.read(self, 8, elem_len + 3) + else: + # Call the update if the CRC check of the header fails, + # we're done here + if self._update_finished_cb: + self._update_finished_cb(self) + self._update_finished_cb = None + elif addr == 0x08: + if self._parse_and_check_elements(data): + self.valid = True + if self._update_finished_cb: + self._update_finished_cb(self) + self._update_finished_cb = None + + def _parse_and_check_elements(self, data): + """ + Parse and check the CRC and length of the elements part of the memory + """ + crc = data[-1] + test_crc = crc32(data[:-1]) & 0x0ff + elem_data = data[2:-1] + if test_crc == crc: + while len(elem_data) > 0: + (eid, elen) = struct.unpack('BB', elem_data[:2]) + self.elements[self.element_mapping[eid]] = \ + elem_data[2:2 + elen].decode('ISO-8859-1') + elem_data = elem_data[2 + elen:] + return True + return False + + def write_done(self, mem, addr): + if self._write_finished_cb: + self._write_finished_cb(self, addr) + self._write_finished_cb = None + + def write_data(self, write_finished_cb): + # First generate the header part + header_data = struct.pack('. +import logging +import struct + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class Poly4D: + class Poly: + def __init__(self, values=[0.0] * 8): + self.values = values + + def __init__(self, duration, x=None, y=None, z=None, yaw=None): + self.duration = duration + self.x = x if x else self.Poly() + self.y = y if y else self.Poly() + self.z = z if z else self.Poly() + self.yaw = yaw if yaw else self.Poly() + + +class TrajectoryMemory(MemoryElement): + """ + Memory interface for trajectories used by the high level commander + """ + + def __init__(self, id, type, size, mem_handler): + """Initialize trajectory memory""" + super(TrajectoryMemory, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + self._write_finished_cb = None + + # A list of Poly4D objects to write to the Crazyflie + self.poly4Ds = [] + + def write_data(self, write_finished_cb): + """Write trajectory data to the Crazyflie""" + self._write_finished_cb = write_finished_cb + data = bytearray() + + for poly4D in self.poly4Ds: + data += struct.pack(' Date: Thu, 5 Nov 2020 16:29:34 +0100 Subject: [PATCH 075/861] #175 Added error callbacks for memory read and write that are called on failure. --- cflib/crazyflie/mem/__init__.py | 25 ++++++++++++++++------ cflib/crazyflie/mem/trajectory_memory.py | 12 ++++++++++- examples/autonomous_sequence_high_level.py | 24 ++++++++++++++++----- 3 files changed, 49 insertions(+), 12 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 77642b5cf..00f96a6ce 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -232,9 +232,12 @@ def _clear_state(self): self.mems = [] # Called when new memories have been added self.mem_added_cb = Caller() - # Called when new data has been read + + # Called to signal completion of read or write self.mem_read_cb = Caller() + self.mem_read_failed_cb = Caller() self.mem_write_cb = Caller() + self.mem_write_failed_cb = Caller() self._refresh_callback = None self._fetch_id = 0 @@ -298,7 +301,7 @@ def write(self, memory, addr, data, flush_queue=False): if flush_queue: self._write_requests[memory.id] = self._write_requests[ memory.id][:1] - self._write_requests[memory.id].insert(len(self._write_requests), wreq) + self._write_requests[memory.id].append(wreq) if len(self._write_requests[memory.id]) == 1: wreq.start() self._write_requests_lock.release() @@ -432,6 +435,7 @@ def _new_packet_cb(self, packet): size=mem_size, mem_handler=self) logger.debug(mem) self.mem_write_cb.add_callback(mem.write_done) + self.mem_write_failed_cb.add_callback(mem.write_failed) elif mem_type == MemoryElement.TYPE_LOCO2: mem = LocoMemory2(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) @@ -509,8 +513,15 @@ def _new_packet_cb(self, packet): self._write_requests[id][0].start() else: logger.debug( - 'Status {}: write resending...'.format(status)) - wreq.resend() + 'Status {}: write failed.'.format(status)) + # Remove from queue + self._write_requests[id].pop(0) + self.mem_write_failed_cb.call(wreq.mem, wreq.addr) + + # Get a new one to start (if there are any) + if len(self._write_requests[id]) > 0: + self._write_requests[id][0].start() + self._write_requests_lock.release() if chan == CHAN_READ: @@ -530,5 +541,7 @@ def _new_packet_cb(self, packet): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: - logger.debug('Status {}: resending...'.format(status)) - rreq.resend() + logger.debug('Status {}: read failed.'.format(status)) + self._read_requests.pop(id, None) + self.mem_read_failed_cb.call( + rreq.mem, rreq.addr, rreq.data) diff --git a/cflib/crazyflie/mem/trajectory_memory.py b/cflib/crazyflie/mem/trajectory_memory.py index 604b107a0..07865cd31 100644 --- a/cflib/crazyflie/mem/trajectory_memory.py +++ b/cflib/crazyflie/mem/trajectory_memory.py @@ -50,13 +50,15 @@ def __init__(self, id, type, size, mem_handler): super(TrajectoryMemory, self).__init__(id=id, type=type, size=size, mem_handler=mem_handler) self._write_finished_cb = None + self._write_failed_cb = None # A list of Poly4D objects to write to the Crazyflie self.poly4Ds = [] - def write_data(self, write_finished_cb): + def write_data(self, write_finished_cb, write_failed_cb=None): """Write trajectory data to the Crazyflie""" self._write_finished_cb = write_finished_cb + self._write_failed_cb = write_failed_cb data = bytearray() for poly4D in self.poly4Ds: @@ -73,6 +75,14 @@ def write_done(self, mem, addr): logger.debug('Write trajectory data done') self._write_finished_cb(self, addr) self._write_finished_cb = None + self._write_failed_cb = None + + def write_failed(self, mem, addr): + if self._write_failed_cb and mem.id == self.id: + logger.debug('Write od trajectory data failed') + self._write_failed_cb(self, addr) + self._write_finished_cb = None + self._write_failed_cb = None def disconnect(self): self._write_finished_cb = None diff --git a/examples/autonomous_sequence_high_level.py b/examples/autonomous_sequence_high_level.py index 9132a7917..ff820a72b 100644 --- a/examples/autonomous_sequence_high_level.py +++ b/examples/autonomous_sequence_high_level.py @@ -30,6 +30,7 @@ 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 @@ -65,17 +66,27 @@ class Uploader: def __init__(self): self._is_done = False + self._sucess = True def upload(self, trajectory_mem): print('Uploading data') - trajectory_mem.write_data(self._upload_done) + trajectory_mem.write_data(self._upload_done, + write_failed_cb=self._upload_failed) while not self._is_done: time.sleep(0.2) + return self._sucess + def _upload_done(self, mem, addr): print('Data uploaded') self._is_done = True + self._sucess = True + + def _upload_failed(self, mem, addr): + print('Data upload failed') + self._is_done = True + self._sucess = False def wait_for_position_estimator(scf): @@ -148,7 +159,10 @@ def upload_trajectory(cf, trajectory_id, trajectory): trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw)) total_duration += duration - Uploader().upload(trajectory_mem) + upload_result = Uploader().upload(trajectory_mem) + if not upload_result: + print('Upload failed, aborting!') + sys.exit(1) cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.poly4Ds)) return total_duration @@ -175,8 +189,8 @@ def run_sequence(cf, trajectory_id, duration): trajectory_id = 1 activate_high_level_commander(cf) - # activate_mellinger_controller(cf) + activate_mellinger_controller(cf) duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) - reset_estimator(cf) - run_sequence(cf, trajectory_id, duration) + # reset_estimator(cf) + # run_sequence(cf, trajectory_id, duration) From 753df0b8ba3f4036c20334c273cf7889344e40cd Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 12 Nov 2020 13:01:37 +0100 Subject: [PATCH 076/861] (#174) add memory write functions to python api for LH calibration --- cflib/crazyflie/mem/__init__.py | 7 +- cflib/crazyflie/mem/lighthouse_memory.py | 172 ++++++++++++++++-- cflib/crazyflie/mem/trajectory_memory.py | 7 +- ...geometry-mem.py => read_lighthouse_mem.py} | 15 +- ...eometry-mem.py => write_lighthouse_mem.py} | 78 ++++++-- 5 files changed, 248 insertions(+), 31 deletions(-) rename examples/lighthouse/{read-geometry-mem.py => read_lighthouse_mem.py} (85%) rename examples/lighthouse/{write-geometry-mem.py => write_lighthouse_mem.py} (50%) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 00f96a6ce..39fcb513b 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -36,6 +36,7 @@ from .i2c_element import I2CElement from .led_driver_memory import LEDDriverMemory from .led_timings_driver_memory import LEDTimingsDriverMemory +from .lighthouse_memory import LighthouseBsCalibration from .lighthouse_memory import LighthouseBsGeometry from .lighthouse_memory import LighthouseMemory from .loco_memory import LocoMemory @@ -50,7 +51,8 @@ from cflib.utils.callbacks import Caller __author__ = 'Bitcraze AB' -__all__ = ['Memory', 'Poly4D', 'MemoryElement', 'LighthouseBsGeometry'] +__all__ = ['Memory', 'Poly4D', 'MemoryElement', + 'LighthouseBsGeometry', 'LighthouseBsCalibration'] # Channels used for the logging port CHAN_INFO = 0 @@ -446,7 +448,10 @@ def _new_packet_cb(self, packet): size=mem_size, mem_handler=self) logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) + self.mem_read_failed_cb.add_callback( + mem.new_data_failed) self.mem_write_cb.add_callback(mem.write_done) + self.mem_write_failed_cb.add_callback(mem.write_failed) elif mem_type == MemoryElement.TYPE_MEMORY_TESTER: mem = MemoryTester(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index 27a5c2ec7..3806f16aa 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -33,7 +33,6 @@ class LighthouseBsGeometry: SIZE_FLOAT = 4 SIZE_VECTOR = 3 * SIZE_FLOAT SIZE_GEOMETRY = (1 + 3) * SIZE_VECTOR - SIZE_DATA = 2 * SIZE_GEOMETRY def __init__(self): self.origin = [0.0, 0.0, 0.0] @@ -70,18 +69,100 @@ def dump(self): print('rotation matrix: ', self.rotation_matrix) +class LighthouseCalibrationSweep: + def __init__(self): + self.phase = 0.0 + self.tilt = 0.0 + self.curve = 0.0 + self.gibmag = 0.0 + self.gibphase = 0.0 + self.ogeemag = 0.0 + self.ogeephase = 0.0 + + def dump(self): + print(('phase: {}, tilt: {}, curve: {}, gibmag: {}, ' + + 'gibphase: {}, ogeemag: {}, ogeephase: {}').format( + self.phase, + self.tilt, + self.curve, + self.gibmag, + self.gibphase, + self.ogeemag, + self.ogeephase)) + + +class LighthouseBsCalibration: + """Container for calibration data of one Lighthouse base station""" + + SIZE_FLOAT = 4 + SIZE_BOOL = 1 + SIZE_SWEEP = 7 * SIZE_FLOAT + SIZE_CALIBRATION = 2 * SIZE_SWEEP + SIZE_BOOL + + def __init__(self): + self.sweeps = [LighthouseCalibrationSweep(), + LighthouseCalibrationSweep()] + self.is_valid = True + + def set_from_mem_data(self, data): + self.sweeps[0] = self._unpack_sweep_calibration( + data[0:self.SIZE_SWEEP]) + self.sweeps[1] = self._unpack_sweep_calibration( + data[self.SIZE_SWEEP:self.SIZE_SWEEP * 2]) + self.is_valid = struct.unpack('= self.CALIB_START_ADDR: + calibration_data = LighthouseBsCalibration() + calibration_data.set_from_mem_data(data) - def update(self, update_finished_cb): + if self._update_finished_cb: + self._update_finished_cb(self, calibration_data) + self._clear_update_cb() + + def new_data_failed(self, mem, addr, data): + if mem.id == self.id: + if self._update_failed_cb: + logger.debug('Update of data failed') + self._update_failed_cb(self) + self._clear_update_cb() + + def update(self, update_finished_cb, update_failed_cb=None): """Request an update of the memory content""" - if not self._update_finished_cb: - self._update_finished_cb = update_finished_cb - logger.debug('Updating content of memory {}'.format(self.id)) - self.mem_handler.read(self, 0, LighthouseBsGeometry.SIZE_DATA) + if self._update_finished_cb: + raise Exception('Read operation already ongoing') + self._update_finished_cb = update_finished_cb + self._update_failed_cb = update_failed_cb + logger.debug('Updating content of memory {}'.format(self.id)) + self.mem_handler.read(self, 0, self.SIZE_GEOMETRY_ALL) + + def read_calib_data(self, basestation, update_finished_cb, + update_failed_cb=None): + if self._update_finished_cb: + raise Exception('Read operation already ongoing') + self._update_finished_cb = update_finished_cb + self._update_failed_cb = update_failed_cb + self.mem_handler.read(self, self.CALIB_START_ADDR + basestation * + self.PAGE_SIZE, + LighthouseBsCalibration.SIZE_CALIBRATION) - def write_data(self, write_finished_cb): + def write_geo_data(self, write_finished_cb, write_failed_cb=None): """Write geometry data to the Crazyflie""" self._write_finished_cb = write_finished_cb + self._write_failed_cb = write_failed_cb + self._write_data_list(self.GEO_START_ADDR, self.geometry_data) + + def write_calib_data(self, calibration_data, bs_id, write_finished_cb, + write_failed_cb=None): + """Write calibration data for one basestation to the Crazyflie""" + if self._write_finished_cb: + raise Exception('Write operation already ongoing.') data = bytearray() + calibration_data.add_mem_data(data) + self._write_finished_cb = write_finished_cb + self._write_failed_cb = write_failed_cb + calib_addr = self.CALIB_START_ADDR + bs_id * self.PAGE_SIZE + self.mem_handler.write(self, calib_addr, data, flush_queue=True) - for bs in self.geometry_data: + def _write_data_list(self, addr, data_list): + data = bytearray() + for bs in data_list: bs.add_mem_data(data) - - self.mem_handler.write(self, 0x00, data, flush_queue=True) + self.mem_handler.write(self, addr, data, flush_queue=True) def write_done(self, mem, addr): if self._write_finished_cb and mem.id == self.id: - logger.debug('Write of geometry data done') + if addr == 0: + logger.debug('Write of geometry data done') + if addr is self.CALIB_START_ADDR: + logger.debug('Write of calibration data done') self._write_finished_cb(self, addr) self._write_finished_cb = None + self._write_failed_cb = None + + def write_failed(self, mem, addr): + if mem.id == self.id: + if self._write_failed_cb: + logger.debug('Write of data failed') + self._write_failed_cb(self, addr) + self._clear_write_cb() def disconnect(self): + self._clear_update_cb() + self._clear_write_cb() + + def _clear_update_cb(self): self._update_finished_cb = None + self._update_failed_cb = None + + def _clear_write_cb(self): self._write_finished_cb = None + self._write_failed_cb = None def dump(self): for data in self.geometry_data: diff --git a/cflib/crazyflie/mem/trajectory_memory.py b/cflib/crazyflie/mem/trajectory_memory.py index 07865cd31..0e0d50d68 100644 --- a/cflib/crazyflie/mem/trajectory_memory.py +++ b/cflib/crazyflie/mem/trajectory_memory.py @@ -78,9 +78,10 @@ def write_done(self, mem, addr): self._write_failed_cb = None def write_failed(self, mem, addr): - if self._write_failed_cb and mem.id == self.id: - logger.debug('Write od trajectory data failed') - self._write_failed_cb(self, addr) + if mem.id == self.id: + if self._write_failed_cb: + logger.debug('Write of trajectory data failed') + self._write_failed_cb(self, addr) self._write_finished_cb = None self._write_failed_cb = None diff --git a/examples/lighthouse/read-geometry-mem.py b/examples/lighthouse/read_lighthouse_mem.py similarity index 85% rename from examples/lighthouse/read-geometry-mem.py rename to examples/lighthouse/read_lighthouse_mem.py index 26cd9a212..6d9adc631 100644 --- a/examples/lighthouse/read-geometry-mem.py +++ b/examples/lighthouse/read_lighthouse_mem.py @@ -54,14 +54,27 @@ def __init__(self, uri): while not self.got_data: time.sleep(1) + self.got_data = False + mems[0].read_calib_data(0, self._calib_data_updated) + + while not self.got_data: + time.sleep(1) + def _data_updated(self, mem): mem.dump() self.got_data = True + def _calib_data_updated(self, mem, calib_data): + calib_data.dump() + self.got_data = True + + def _calib_data_update_failed(self, mem): + raise Exception + if __name__ == '__main__': # URI to the Crazyflie to connect to - uri = 'radio://0/80/2M' + uri = 'radio://0/80/2M/' # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) diff --git a/examples/lighthouse/write-geometry-mem.py b/examples/lighthouse/write_lighthouse_mem.py similarity index 50% rename from examples/lighthouse/write-geometry-mem.py rename to examples/lighthouse/write_lighthouse_mem.py index 65fd3161c..f34dc314e 100644 --- a/examples/lighthouse/write-geometry-mem.py +++ b/examples/lighthouse/write_lighthouse_mem.py @@ -30,6 +30,7 @@ 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 MemoryElement from cflib.crazyflie.syncCrazyflie import SyncCrazyflie @@ -39,7 +40,7 @@ class WriteMem: - def __init__(self, uri, bs1, bs2): + def __init__(self, uri, bs1geo, bs2geo, bs1calib, bs2calib): self.data_written = False with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -49,10 +50,27 @@ def __init__(self, uri, bs1, bs2): if count != 1: raise Exception('Unexpected nr of memories found:', count) - mems[0].geometry_data = [bs1, bs2] + mems[0].geometry_data = [bs1geo, bs2geo] + mems[0].calibration_data = [bs1calib, bs2calib] print('Writing data') - mems[0].write_data(self._data_written) + mems[0].write_geo_data(self._data_written) + + while not self.data_written: + time.sleep(1) + + self.data_written = False + mems[0].write_calib_data( + bs1calib, 0, self._data_written, + write_failed_cb=self._data_failed) + + while not self.data_written: + time.sleep(1) + + self.data_written = False + mems[0].write_calib_data( + bs2calib, 1, self._data_written, + write_failed_cb=self._data_failed) while not self.data_written: time.sleep(1) @@ -61,28 +79,66 @@ def _data_written(self, mem, addr): self.data_written = True print('Data written') + def _data_failed(self, mem, addr): + print('Data failed') + raise Exception() + if __name__ == '__main__': # URI to the Crazyflie to connect to - uri = 'radio://0/80/2M' + uri = 'radio://0/80/2M/' # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) - bs1 = LighthouseBsGeometry() - bs1.origin = [1.0, 2.0, 3.0] - bs1.rotation_matrix = [ + 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], ] - bs2 = LighthouseBsGeometry() - bs2.origin = [21.0, 22.0, 23.0] - bs2.rotation_matrix = [ + 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], ] - WriteMem(uri, bs1, bs2) + 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.is_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.is_valid = True + + WriteMem(uri, bs1geo, bs2geo, bs1calib, bs2calib) From b7ed6b0661aad7eda5bfefb4404ebf0a3d045d1b Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 12 Nov 2020 16:38:08 +0100 Subject: [PATCH 077/861] (#174) fixed flake8 issue --- cflib/crazyflie/mem/lighthouse_memory.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index 3806f16aa..34fe2600c 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -115,12 +115,12 @@ def _unpack_sweep_calibration(self, data): result = LighthouseCalibrationSweep() (result.phase, - result.tilt, - result.curve, - result.gibmag, - result.gibphase, - result.ogeemag, - result.ogeephase) = struct.unpack(' Date: Thu, 12 Nov 2020 19:18:42 +0100 Subject: [PATCH 078/861] #176 Updated mapping of geometry data in memory. This is a breaking API change for the LighthouseMemory class and also require an update of the Crazyflie firmware --- cflib/crazyflie/mem/lighthouse_memory.py | 70 +++++++++------------ examples/lighthouse/read_lighthouse_mem.py | 21 ++++--- examples/lighthouse/write_lighthouse_mem.py | 29 +++++---- 3 files changed, 61 insertions(+), 59 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index 34fe2600c..4a29c82a7 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -164,67 +164,67 @@ def __init__(self, id, type, size, mem_handler): self._clear_update_cb() self._clear_write_cb() - # Geometry data for two base stations - self.geometry_data = [ - LighthouseBsGeometry(), - LighthouseBsGeometry(), - ] - def new_data(self, mem, addr, data): """Callback for when new memory data has been fetched""" if mem.id == self.id: - if addr == 0: - self.geometry_data[0].set_from_mem_data( - data[0:LighthouseBsGeometry.SIZE_GEOMETRY]) - self.geometry_data[1].set_from_mem_data( - data[LighthouseBsGeometry.SIZE_GEOMETRY: - LighthouseBsGeometry.SIZE_GEOMETRY * 2]) + if addr < self.CALIB_START_ADDR: + geo_data = LighthouseBsGeometry() + geo_data.set_from_mem_data(data) if self._update_finished_cb: - self._update_finished_cb(self) - self._clear_update_cb() - - if addr >= self.CALIB_START_ADDR: + self._update_finished_cb(self, geo_data) + else: calibration_data = LighthouseBsCalibration() calibration_data.set_from_mem_data(data) if self._update_finished_cb: self._update_finished_cb(self, calibration_data) - self._clear_update_cb() + + self._clear_update_cb() def new_data_failed(self, mem, addr, data): + """Callback when a read failed""" if mem.id == self.id: if self._update_failed_cb: logger.debug('Update of data failed') self._update_failed_cb(self) self._clear_update_cb() - def update(self, update_finished_cb, update_failed_cb=None): - """Request an update of the memory content""" + def read_geo_data(self, bs_id, update_finished_cb, + update_failed_cb=None): + """Request a read of geometry data for one base station""" if self._update_finished_cb: raise Exception('Read operation already ongoing') self._update_finished_cb = update_finished_cb self._update_failed_cb = update_failed_cb - logger.debug('Updating content of memory {}'.format(self.id)) - self.mem_handler.read(self, 0, self.SIZE_GEOMETRY_ALL) + self.mem_handler.read(self, self.GEO_START_ADDR + bs_id * + self.PAGE_SIZE, + LighthouseBsGeometry.SIZE_GEOMETRY) - def read_calib_data(self, basestation, update_finished_cb, + def read_calib_data(self, bs_id, update_finished_cb, update_failed_cb=None): + """Request a read of calibration data for one base station""" if self._update_finished_cb: raise Exception('Read operation already ongoing') self._update_finished_cb = update_finished_cb self._update_failed_cb = update_failed_cb - self.mem_handler.read(self, self.CALIB_START_ADDR + basestation * + self.mem_handler.read(self, self.CALIB_START_ADDR + bs_id * self.PAGE_SIZE, LighthouseBsCalibration.SIZE_CALIBRATION) - def write_geo_data(self, write_finished_cb, write_failed_cb=None): - """Write geometry data to the Crazyflie""" + def write_geo_data(self, bs_id, geo_data, write_finished_cb, + write_failed_cb=None): + """Write geometry data for one base station to the Crazyflie""" + if self._write_finished_cb: + raise Exception('Write operation already ongoing.') + data = bytearray() + geo_data.add_mem_data(data) self._write_finished_cb = write_finished_cb self._write_failed_cb = write_failed_cb - self._write_data_list(self.GEO_START_ADDR, self.geometry_data) + geo_addr = self.GEO_START_ADDR + bs_id * self.PAGE_SIZE + self.mem_handler.write(self, geo_addr, data, flush_queue=True) - def write_calib_data(self, calibration_data, bs_id, write_finished_cb, + def write_calib_data(self, bs_id, calibration_data, write_finished_cb, write_failed_cb=None): """Write calibration data for one basestation to the Crazyflie""" if self._write_finished_cb: @@ -243,14 +243,10 @@ def _write_data_list(self, addr, data_list): self.mem_handler.write(self, addr, data, flush_queue=True) def write_done(self, mem, addr): - if self._write_finished_cb and mem.id == self.id: - if addr == 0: - logger.debug('Write of geometry data done') - if addr is self.CALIB_START_ADDR: - logger.debug('Write of calibration data done') - self._write_finished_cb(self, addr) - self._write_finished_cb = None - self._write_failed_cb = None + if mem.id == self.id: + if self._write_finished_cb: + self._write_finished_cb(self, addr) + self._clear_write_cb() def write_failed(self, mem, addr): if mem.id == self.id: @@ -270,7 +266,3 @@ def _clear_update_cb(self): def _clear_write_cb(self): self._write_finished_cb = None self._write_failed_cb = None - - def dump(self): - for data in self.geometry_data: - data.dump() diff --git a/examples/lighthouse/read_lighthouse_mem.py b/examples/lighthouse/read_lighthouse_mem.py index 6d9adc631..ee6d077fd 100644 --- a/examples/lighthouse/read_lighthouse_mem.py +++ b/examples/lighthouse/read_lighthouse_mem.py @@ -22,8 +22,8 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. """ -Example of how to read the Lighthouse base station geometry memory from a -Crazyflie +Example of how to read the Lighthouse base station geometry and +calibration memory from a Crazyflie """ import logging import time @@ -48,33 +48,36 @@ def __init__(self, uri): if count != 1: raise Exception('Unexpected nr of memories found:', count) + lh_mem = mems[0] print('Requesting data') - mems[0].update(self._data_updated) + lh_mem.read_geo_data(0, self._geo_data_updated, + update_failed_cb=self._update_failed) while not self.got_data: time.sleep(1) self.got_data = False - mems[0].read_calib_data(0, self._calib_data_updated) + lh_mem.read_calib_data(0, self._calib_data_updated, + update_failed_cb=self._update_failed) while not self.got_data: time.sleep(1) - def _data_updated(self, mem): - mem.dump() + def _geo_data_updated(self, mem, geo_data): + geo_data.dump() self.got_data = True def _calib_data_updated(self, mem, calib_data): calib_data.dump() self.got_data = True - def _calib_data_update_failed(self, mem): - raise Exception + def _update_failed(self, mem): + raise Exception("Read failed") if __name__ == '__main__': # URI to the Crazyflie to connect to - uri = 'radio://0/80/2M/' + uri = 'radio://0/80' # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) diff --git a/examples/lighthouse/write_lighthouse_mem.py b/examples/lighthouse/write_lighthouse_mem.py index f34dc314e..201c710bb 100644 --- a/examples/lighthouse/write_lighthouse_mem.py +++ b/examples/lighthouse/write_lighthouse_mem.py @@ -22,8 +22,8 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. """ -Example of how to write to the Lighthouse base station geometry memory in a -Crazyflie +Example of how to write to the Lighthouse base station geometry +and calibration memory in a Crazyflie """ import logging import time @@ -50,26 +50,33 @@ def __init__(self, uri, bs1geo, bs2geo, bs1calib, bs2calib): if count != 1: raise Exception('Unexpected nr of memories found:', count) - mems[0].geometry_data = [bs1geo, bs2geo] - mems[0].calibration_data = [bs1calib, bs2calib] + lh_mem = mems[0] print('Writing data') - mems[0].write_geo_data(self._data_written) + lh_mem.write_geo_data(0, bs1geo, self._data_written, + write_failed_cb=self._data_failed) while not self.data_written: time.sleep(1) self.data_written = False - mems[0].write_calib_data( - bs1calib, 0, self._data_written, + lh_mem.write_geo_data(1, bs2geo, self._data_written, + write_failed_cb=self._data_failed) + + while not self.data_written: + time.sleep(1) + + self.data_written = False + lh_mem.write_calib_data( + 0, bs1calib, self._data_written, write_failed_cb=self._data_failed) while not self.data_written: time.sleep(1) self.data_written = False - mems[0].write_calib_data( - bs2calib, 1, self._data_written, + lh_mem.write_calib_data( + 1, bs2calib, self._data_written, write_failed_cb=self._data_failed) while not self.data_written: @@ -80,13 +87,13 @@ def _data_written(self, mem, addr): print('Data written') def _data_failed(self, mem, addr): - print('Data failed') + print('Write failed') raise Exception() if __name__ == '__main__': # URI to the Crazyflie to connect to - uri = 'radio://0/80/2M/' + uri = 'radio://0/80' # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) From 63809da58afadbedabc57a1d9b07cac3c0fa8aab Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 12 Nov 2020 22:00:36 +0100 Subject: [PATCH 079/861] #176 Added valid flag to lighthouse geo data. This commit breaks the memory mapping API and requires the corresponding commit in the Crazyflie firmware. --- cflib/crazyflie/mem/lighthouse_memory.py | 17 +++++++++++------ examples/lighthouse/read_lighthouse_mem.py | 21 ++++++++++++++++++++- examples/lighthouse/write_lighthouse_mem.py | 6 ++++-- 3 files changed, 35 insertions(+), 9 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index 4a29c82a7..b7bb7d738 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -31,8 +31,9 @@ class LighthouseBsGeometry: """Container for geometry data of one Lighthouse base station""" SIZE_FLOAT = 4 + SIZE_BOOL = 1 SIZE_VECTOR = 3 * SIZE_FLOAT - SIZE_GEOMETRY = (1 + 3) * SIZE_VECTOR + SIZE_GEOMETRY = (1 + 3) * SIZE_VECTOR + SIZE_BOOL def __init__(self): self.origin = [0.0, 0.0, 0.0] @@ -41,6 +42,7 @@ def __init__(self): [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], ] + self.valid = False def set_from_mem_data(self, data): self.origin = self._read_vector( @@ -50,12 +52,14 @@ def set_from_mem_data(self, data): self._read_vector(data[2 * self.SIZE_VECTOR:3 * self.SIZE_VECTOR]), self._read_vector(data[3 * self.SIZE_VECTOR:4 * self.SIZE_VECTOR]), ] + self.valid = struct.unpack(' Date: Thu, 19 Nov 2020 11:28:31 +0100 Subject: [PATCH 080/861] (#177) added BS mask sending function to localization --- cflib/crazyflie/localization.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index dabd1be8c..20831e2bd 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -65,6 +65,7 @@ class Localization(): COMM_GNSS_PROPRIETARY = 7 EXT_POSE = 8 EXT_POSE_PACKED = 9 + LH_PERSIST_DATA = 11 def __init__(self, crazyflie=None): """ @@ -142,3 +143,22 @@ def send_short_lpp_packet(self, dest_id, data): pk.channel = self.GENERIC_CH pk.data = struct.pack(' Date: Thu, 19 Nov 2020 12:02:42 +0100 Subject: [PATCH 081/861] (#177) add test function to test out LH persist bs mask decoding --- test/crazyflie/test_localization.py | 70 +++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 test/crazyflie/test_localization.py diff --git a/test/crazyflie/test_localization.py b/test/crazyflie/test_localization.py new file mode 100644 index 000000000..974be1652 --- /dev/null +++ b/test/crazyflie/test_localization.py @@ -0,0 +1,70 @@ +# -*- 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. + +import unittest +from unittest.mock import MagicMock + +import struct + +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.localization import Localization + + +class LocalizationTest(unittest.TestCase): + + def setUp(self): + self.cf_mock = MagicMock(spec=Crazyflie) + self.sut = Localization(crazyflie=self.cf_mock) + + def test_that_checks_if_data_package_is_correctly_decoded(self): + + # fixture + geo_bs_list = [0, 2, 4, 6, 8, 10, 12, 14] + calib_bs_list = [1, 3, 5, 7, 9, 11, 13, 15] + + # test + actual = self.sut.send_lh_persit_data_packet(geo_bs_list, calib_bs_list) + + # assert + data_check = 2863289685 + expected = CRTPPacket() + expected.port = CRTPPort.LOCALIZATION + expected.channel = self.sut.GENERIC_CH + expected.data = struct.pack(' Date: Thu, 19 Nov 2020 14:30:08 +0100 Subject: [PATCH 082/861] (#177) add return packet from persit mem --- cflib/crazyflie/localization.py | 6 ++++++ test/crazyflie/test_localization.py | 21 +++++++-------------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 20831e2bd..15408e733 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -101,6 +101,12 @@ def _incoming(self, packet): anchor_id, distance = struct.unpack(' Date: Thu, 19 Nov 2020 15:07:49 +0100 Subject: [PATCH 083/861] Closes #178: Add LH angle streaming support --- cflib/crazyflie/localization.py | 24 ++++++++++++++ cflib/utils/fp16.py | 56 +++++++++++++++++++++++++++++++++ 2 files changed, 80 insertions(+) create mode 100644 cflib/utils/fp16.py diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index dabd1be8c..639ee5d56 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -35,6 +35,8 @@ from cflib.crtp.crtpstack import CRTPPort from cflib.utils.callbacks import Caller +from cflib.utils.fp16 import fp16_to_float + __author__ = 'Bitcraze AB' __all__ = ['Localization', 'LocalizationPacket'] @@ -65,6 +67,7 @@ class Localization(): COMM_GNSS_PROPRIETARY = 7 EXT_POSE = 8 EXT_POSE_PACKED = 9 + LH_ANGLE_STREAM = 10 def __init__(self, crazyflie=None): """ @@ -100,10 +103,31 @@ def _incoming(self, packet): anchor_id, distance = struct.unpack('> 15) & 0x00000001) # sign + e = int((float16 >> 10) & 0x0000001f) # exponent + f = int(float16 & 0x000003ff) # fraction + + if e == 0: + if f == 0: + return int(s << 31) + else: + while not (f & 0x00000400): + f = f << 1 + e -= 1 + e += 1 + f &= ~0x00000400 + # print(s,e,f) + elif e == 31: + if f == 0: + return int((s << 31) | 0x7f800000) + else: + return int((s << 31) | 0x7f800000 | (f << 13)) + + e = e + (127 - 15) + f = f << 13 + result = int((s << 31) | (e << 23) | f) + return struct.unpack("f", struct.pack("I", result))[0] From 2f92473cbcd786d129e85c5e09411e4e8d625039 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 19 Nov 2020 17:07:45 +0100 Subject: [PATCH 084/861] fix style and double quotes --- cflib/crazyflie/localization.py | 4 +--- cflib/utils/fp16.py | 3 +-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index d0722c4cf..a4ca462e0 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -34,7 +34,6 @@ from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort from cflib.utils.callbacks import Caller - from cflib.utils.fp16 import fp16_to_float __author__ = 'Bitcraze AB' @@ -70,7 +69,6 @@ class Localization(): LH_ANGLE_STREAM = 10 LH_PERSIST_DATA = 11 - def __init__(self, crazyflie=None): """ Initialize the Extpos object. @@ -116,7 +114,7 @@ def _incoming(self, packet): def _decode_lh_angle(self, data): decoded_data = {} - raw_data = struct.unpack(" Date: Thu, 19 Nov 2020 21:02:26 +0100 Subject: [PATCH 085/861] #177 Added missing type --- cflib/crazyflie/localization.py | 3 ++- test/crazyflie/test_localization.py | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index a4ca462e0..390167b0d 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -183,7 +183,8 @@ def send_lh_persit_data_packet(self, geo_list, calib_list): pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.GENERIC_CH - pk.data = struct.pack(' Date: Mon, 23 Nov 2020 15:19:38 +0100 Subject: [PATCH 086/861] (#177) added testfunction for validity basestation mask --- cflib/crazyflie/localization.py | 11 +++++++++- test/crazyflie/test_localization.py | 32 ++++++++++++++++++++++++++++- 2 files changed, 41 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 390167b0d..aba0dcf32 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -169,10 +169,19 @@ def send_short_lpp_packet(self, dest_id, data): pk.data = struct.pack(' max_bs_nr-1: + raise Exception('Geometry BS list is not valid') + if calib_list[-1] > max_bs_nr-1: + raise Exception('Calibration BS list is not valid') + mask_geo = 0 mask_calib = 0 for bs in geo_list: diff --git a/test/crazyflie/test_localization.py b/test/crazyflie/test_localization.py index 8e6ab4849..b08160488 100644 --- a/test/crazyflie/test_localization.py +++ b/test/crazyflie/test_localization.py @@ -46,7 +46,7 @@ def test_that_lighthouse_persist_data_is_correctly_encoded(self): calib_bs_list = [1, 3, 5, 7, 9, 11, 13, 15] # test - actual = self.sut.send_lh_persit_data_packet( + actual = self.sut.send_lh_persist_data_packet( geo_bs_list, calib_bs_list) # assert @@ -62,3 +62,33 @@ def test_that_lighthouse_persist_data_is_correctly_encoded(self): self.assertEqual(expected.port, actual.port) self.assertEqual(expected.channel, actual.channel) self.assertEqual(expected.data, actual.data) + + def test_that_checks_if_list_of_bs_is_valid(self): + + # fixture + max_bs_nr = 16 + geo_bs_list_good = [0, max_bs_nr-1] + geo_bs_list_bad = [0, max_bs_nr] + calib_bs_list_good = [0, max_bs_nr-1] + calib_bs_list_bad = [0, max_bs_nr] + + # tests and results + try: + self.sut.send_lh_persist_data_packet( + geo_bs_list_bad, calib_bs_list_good) + except Exception as e: + actual = e.args[0] + expected = 'Geometry BS list is not valid' + self.assertEqual(expected, actual) + else: + self.fail('Expect exception') + + try: + self.sut.send_lh_persist_data_packet( + geo_bs_list_good, calib_bs_list_bad) + except Exception as e: + actual = e.args[0] + expected = 'Calibration BS list is not valid' + self.assertEqual(expected, actual) + else: + self.fail('Expect exception') From bab4e7e29b76734f5cb2f1e8e29f3cddae57c739 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 23 Nov 2020 16:22:08 +0100 Subject: [PATCH 087/861] (#177) add fail for empty lists --- cflib/crazyflie/localization.py | 4 ++-- test/crazyflie/test_localization.py | 22 ++++++++++++++++++++++ 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index aba0dcf32..0343fcc02 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -177,9 +177,9 @@ def send_lh_persist_data_packet(self, geo_list, calib_list): geo_list.sort() calib_list.sort() max_bs_nr = 16 - if geo_list[-1] > max_bs_nr-1: + if len(geo_list) == 0 or geo_list[-1] > max_bs_nr-1: raise Exception('Geometry BS list is not valid') - if calib_list[-1] > max_bs_nr-1: + if len(calib_list) == 0 or calib_list[-1] > max_bs_nr-1: raise Exception('Calibration BS list is not valid') mask_geo = 0 diff --git a/test/crazyflie/test_localization.py b/test/crazyflie/test_localization.py index b08160488..c3bee72aa 100644 --- a/test/crazyflie/test_localization.py +++ b/test/crazyflie/test_localization.py @@ -69,8 +69,10 @@ def test_that_checks_if_list_of_bs_is_valid(self): max_bs_nr = 16 geo_bs_list_good = [0, max_bs_nr-1] geo_bs_list_bad = [0, max_bs_nr] + geo_bs_list_empty = [] calib_bs_list_good = [0, max_bs_nr-1] calib_bs_list_bad = [0, max_bs_nr] + calib_bs_list_empty = [] # tests and results try: @@ -83,6 +85,16 @@ def test_that_checks_if_list_of_bs_is_valid(self): else: self.fail('Expect exception') + try: + self.sut.send_lh_persist_data_packet( + geo_bs_list_empty, calib_bs_list_good) + except Exception as e: + actual = e.args[0] + expected = 'Geometry BS list is not valid' + self.assertEqual(expected, actual) + else: + self.fail('Expect exception') + try: self.sut.send_lh_persist_data_packet( geo_bs_list_good, calib_bs_list_bad) @@ -92,3 +104,13 @@ def test_that_checks_if_list_of_bs_is_valid(self): self.assertEqual(expected, actual) else: self.fail('Expect exception') + + try: + self.sut.send_lh_persist_data_packet( + geo_bs_list_good, calib_bs_list_empty) + except Exception as e: + actual = e.args[0] + expected = 'Calibration BS list is not valid' + self.assertEqual(expected, actual) + else: + self.fail('Expect exception') From 402db4efc876924ec0f75007c166ef334ec2d954 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 23 Nov 2020 18:00:29 +0100 Subject: [PATCH 088/861] #177 Improved validation of base station lists --- cflib/crazyflie/localization.py | 12 +++--- test/crazyflie/test_localization.py | 57 ++++++++++++++++++++--------- 2 files changed, 46 insertions(+), 23 deletions(-) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 0343fcc02..0b4c8ae3b 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -176,11 +176,13 @@ def send_lh_persist_data_packet(self, geo_list, calib_list): geo_list.sort() calib_list.sort() - max_bs_nr = 16 - if len(geo_list) == 0 or geo_list[-1] > max_bs_nr-1: - raise Exception('Geometry BS list is not valid') - if len(calib_list) == 0 or calib_list[-1] > max_bs_nr-1: - raise Exception('Calibration BS list is not valid') + max_bs_nr = 15 + if len(geo_list) > 0: + if geo_list[0] < 0 or geo_list[-1] > max_bs_nr: + raise Exception('Geometry BS list is not valid') + if len(calib_list) > 0: + if calib_list[0] < 0 or calib_list[-1] > max_bs_nr: + raise Exception('Calibration BS list is not valid') mask_geo = 0 mask_calib = 0 diff --git a/test/crazyflie/test_localization.py b/test/crazyflie/test_localization.py index c3bee72aa..731865a89 100644 --- a/test/crazyflie/test_localization.py +++ b/test/crazyflie/test_localization.py @@ -63,21 +63,26 @@ def test_that_lighthouse_persist_data_is_correctly_encoded(self): self.assertEqual(expected.channel, actual.channel) self.assertEqual(expected.data, actual.data) - def test_that_checks_if_list_of_bs_is_valid(self): + def test_that_lighthouse_persist_accepts_empty_lists(self): # fixture - max_bs_nr = 16 - geo_bs_list_good = [0, max_bs_nr-1] - geo_bs_list_bad = [0, max_bs_nr] - geo_bs_list_empty = [] - calib_bs_list_good = [0, max_bs_nr-1] - calib_bs_list_bad = [0, max_bs_nr] - calib_bs_list_empty = [] # tests and results + self.sut.send_lh_persist_data_packet([], []) + + # Assert + # The test should not raise an exception, so if we get here all is + # good + + def test_that_lighthouse_persist_raises_for_geo_list_bs_too_high(self): + # fixture + max_bs_nr = 15 + invalid_list = [max_bs_nr + 1] + + # test + # assert try: - self.sut.send_lh_persist_data_packet( - geo_bs_list_bad, calib_bs_list_good) + self.sut.send_lh_persist_data_packet(invalid_list, []) except Exception as e: actual = e.args[0] expected = 'Geometry BS list is not valid' @@ -85,29 +90,45 @@ def test_that_checks_if_list_of_bs_is_valid(self): else: self.fail('Expect exception') + def test_that_lighthouse_persist_raises_for_calib_list_bs_too_high(self): + # fixture + max_bs_nr = 15 + invalid_list = [max_bs_nr + 1] + + # test + # assert try: - self.sut.send_lh_persist_data_packet( - geo_bs_list_empty, calib_bs_list_good) + self.sut.send_lh_persist_data_packet([], invalid_list) except Exception as e: actual = e.args[0] - expected = 'Geometry BS list is not valid' + expected = 'Calibration BS list is not valid' self.assertEqual(expected, actual) else: self.fail('Expect exception') + def test_that_lighthouse_persist_raises_for_geo_list_bs_negative(self): + # fixture + invalid_list = [-1] + + # test + # assert try: - self.sut.send_lh_persist_data_packet( - geo_bs_list_good, calib_bs_list_bad) + self.sut.send_lh_persist_data_packet(invalid_list, []) except Exception as e: actual = e.args[0] - expected = 'Calibration BS list is not valid' + expected = 'Geometry BS list is not valid' self.assertEqual(expected, actual) else: self.fail('Expect exception') + def test_that_lighthouse_persist_raises_for_calib_list_bs_negative(self): + # fixture + invalid_list = [-1] + + # test + # assert try: - self.sut.send_lh_persist_data_packet( - geo_bs_list_good, calib_bs_list_empty) + self.sut.send_lh_persist_data_packet([], invalid_list) except Exception as e: actual = e.args[0] expected = 'Calibration BS list is not valid' From f6fac511e28b48b60863b127b9870e67773cb929 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 2 Dec 2020 11:50:05 +0100 Subject: [PATCH 089/861] #179 Improved error reporting when threads raise an exception --- cflib/crazyflie/swarm.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index 55bdc3252..5355623c8 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -180,16 +180,19 @@ def parallel_safe(self, func, args_dict=None): thread.join() if reporter.is_error_reported(): + first_error = reporter.errors[0] raise Exception('One or more threads raised an exception when ' - 'executing parallel task') + 'executing parallel task') from first_error def _thread_function_wrapper(self, *args): + reporter = None try: func = args[0] reporter = args[1] func(*args[2:]) - except Exception: - reporter.report_error() + except Exception as e: + if reporter: + reporter.report_error(e) def _process_args_dict(self, scf, uri, args_dict): args = [scf] @@ -200,12 +203,17 @@ def _process_args_dict(self, scf, uri, args_dict): return args class Reporter: - def __init__(self): self.error_reported = False + self._errors = [] + + @property + def errors(self): + return self._errors - def report_error(self): + def report_error(self, e): self.error_reported = True + self._errors.append(e) def is_error_reported(self): return self.error_reported From 6e3f0e3c2870ab96a0276aaaf0d14bf49b758a34 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 2 Dec 2020 16:42:28 +0100 Subject: [PATCH 090/861] #180 Updated interaction with the Crazyflie during initialization and take off to fix problems when starting from positions other than the origin. --- cflib/positioning/position_hl_commander.py | 29 ++++++++----------- .../positioning/test_position_hl_commander.py | 28 ++++-------------- 2 files changed, 17 insertions(+), 40 deletions(-) diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index 5d8c58fb2..0b4807cab 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -73,12 +73,18 @@ def __init__(self, crazyflie, self._default_height = default_height self._controller = controller + self._activate_controller() + self._activate_high_level_commander() + self._hl_commander = self._cf.high_level_commander + self._x = x self._y = y self._z = z self._is_flying = False + self._init_time = time.time() + def take_off(self, height=DEFAULT, velocity=DEFAULT): """ Takes off, that is starts the motors, goes straight up and hovers. @@ -96,11 +102,13 @@ def take_off(self, height=DEFAULT, velocity=DEFAULT): if not self._cf.is_connected(): raise Exception('Crazyflie is not connected') + # Wait a bit to let the HL commander record the current position + now = time.time() + hold_back = self._init_time + 1.0 - now + if (hold_back > 0.0): + time.sleep(hold_back) + self._is_flying = True - self._reset_position_estimator() - self._activate_controller() - self._activate_high_level_commander() - self._hl_commander = self._cf.high_level_commander height = self._height(height) @@ -260,9 +268,6 @@ def set_default_height(self, height): """ self._default_height = height - def set_controller(self, controller): - self._controller = controller - def get_position(self): """ Get the current position @@ -270,16 +275,6 @@ def get_position(self): """ return self._x, self._y, self._z - def _reset_position_estimator(self): - self._cf.param.set_value('kalman.initialX', '{:.2f}'.format(self._x)) - self._cf.param.set_value('kalman.initialY', '{:.2f}'.format(self._y)) - self._cf.param.set_value('kalman.initialZ', '{:.2f}'.format(self._z)) - - self._cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - self._cf.param.set_value('kalman.resetEstimation', '0') - time.sleep(2) - def _activate_high_level_commander(self): self._cf.param.set_value('commander.enHighLevel', '1') diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index 6b7c28ee0..003d86b54 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -45,43 +45,25 @@ def setUp(self): self.sut = PositionHlCommander(self.cf_mock) - def test_that_the_estimator_is_reset_on_take_off( + def test_that_the_hi_level_commander_is_activated_on_creation( self, sleep_mock): # Fixture - sut = PositionHlCommander(self.cf_mock, 1.0, 2.0, 3.0) # Test - sut.take_off() - - # Assert - self.param_mock.set_value.assert_has_calls([ - call('kalman.initialX', '{:.2f}'.format(1.0)), - call('kalman.initialY', '{:.2f}'.format(2.0)), - call('kalman.initialZ', '{:.2f}'.format(3.0)), - - call('kalman.resetEstimation', '1'), - call('kalman.resetEstimation', '0') - ]) - - def test_that_the_hi_level_commander_is_activated_on_take_off( - self, sleep_mock): - # Fixture - - # Test - self.sut.take_off() # Assert self.param_mock.set_value.assert_has_calls([ call('commander.enHighLevel', '1') ]) - def test_that_controller_is_selected_on_take_off( + def test_that_controller_is_selected_on_creation( self, sleep_mock): # Fixture - self.sut.set_controller(PositionHlCommander.CONTROLLER_MELLINGER) # Test - self.sut.take_off() + PositionHlCommander( + self.cf_mock, + controller=PositionHlCommander.CONTROLLER_MELLINGER) # Assert self.param_mock.set_value.assert_has_calls([ From 6e72582bafec6816dc4aa70914cd0ee1925acebd Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 3 Dec 2020 16:01:54 +0100 Subject: [PATCH 091/861] Closes #182: Add lib support for appchanne packets --- cflib/crazyflie/__init__.py | 2 + cflib/crazyflie/appchannel.py | 59 ++++++++++++++++++++++++++++++ cflib/crazyflie/platformservice.py | 1 + 3 files changed, 62 insertions(+) create mode 100644 cflib/crazyflie/appchannel.py diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index caea384e0..8be0227cb 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -49,6 +49,7 @@ from .mem import Memory from .param import Param from .platformservice import PlatformService +from .appchannel import Appchannel from .toccache import TocCache from cflib.crazyflie.high_level_commander import HighLevelCommander from cflib.utils.callbacks import Caller @@ -118,6 +119,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.param = Param(self) self.mem = Memory(self) self.platform = PlatformService(self) + self.appchannel = Appchannel(self) self.link_uri = '' diff --git a/cflib/crazyflie/appchannel.py b/cflib/crazyflie/appchannel.py new file mode 100644 index 000000000..09b1e3deb --- /dev/null +++ b/cflib/crazyflie/appchannel.py @@ -0,0 +1,59 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2020 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +""" +Data channel to communicate with an application running in the Crazyflie +""" +from cflib.utils.callbacks import Caller +import logging + +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort +import cflib.crazyflie.platformservice +# from . import Crazyflie + +__author__ = 'Bitcraze AB' +__all__ = ['Appchannel'] + +logger = logging.getLogger(__name__) + + +class Appchannel: + def __init__(self, crazyflie): + self._cf = crazyflie + + self.packet_received = Caller() + + self._cf.add_port_callback(CRTPPort.PLATFORM, + self._incoming) + + def send_packet(self, data): + packet = CRTPPacket() + packet.port = CRTPPort.PLATFORM + packet.channel = cflib.crazyflie.platformservice.APP_CHANNEL + packet.data = data + self._cf.send_packet(packet) + + def _incoming(self, packet: CRTPPacket): + if packet.channel == cflib.crazyflie.platformservice.APP_CHANNEL: + self.packet_received.call(packet.data) diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index 3434ebc98..db0dbd219 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -39,6 +39,7 @@ PLATFORM_COMMAND = 0 VERSION_COMMAND = 1 +APP_CHANNEL = 2 PLATFORM_SET_CONT_WAVE = 0 From 4e7ca346d50c511571d28aaa7354bf182a9aeaa9 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 3 Dec 2020 22:04:38 +0100 Subject: [PATCH 092/861] Fix CI check --- cflib/crazyflie/__init__.py | 2 +- cflib/crazyflie/appchannel.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 8be0227cb..56e5e6c02 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -41,6 +41,7 @@ from threading import Timer import cflib.crtp +from .appchannel import Appchannel from .commander import Commander from .console import Console from .extpos import Extpos @@ -49,7 +50,6 @@ from .mem import Memory from .param import Param from .platformservice import PlatformService -from .appchannel import Appchannel from .toccache import TocCache from cflib.crazyflie.high_level_commander import HighLevelCommander from cflib.utils.callbacks import Caller diff --git a/cflib/crazyflie/appchannel.py b/cflib/crazyflie/appchannel.py index 09b1e3deb..9a8206a90 100644 --- a/cflib/crazyflie/appchannel.py +++ b/cflib/crazyflie/appchannel.py @@ -24,12 +24,12 @@ """ Data channel to communicate with an application running in the Crazyflie """ -from cflib.utils.callbacks import Caller import logging +import cflib.crazyflie.platformservice from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort -import cflib.crazyflie.platformservice +from cflib.utils.callbacks import Caller # from . import Crazyflie __author__ = 'Bitcraze AB' From d728792ea1f992621db0c033e70bd7fc9219584d Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 8 Dec 2020 11:08:50 +0100 Subject: [PATCH 093/861] Improve send_setpoint() documentation --- docs/user-guides/python_api.md | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 140327006..852494b58 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -224,9 +224,9 @@ Then you can use the following to close the link again: crazyflie.close_link() ``` -## Sending control commands +## Sending control setpoints -The control commands are not implemented as parameters, instead they +The control setpoints are not implemented as parameters, instead they have a special API. ``` {.python} @@ -250,12 +250,22 @@ To send a new control set-point use the following: ``` Thrust is an integer value ranging from 10001 (next to no power) to -60000 (full power). Sending a command makes it apply for 500 ms, after -which the firmware will cut out the power. With this in mind, you need -to try and maintain a thrust level, with a tick being sent at least once -every 2 seconds. Ideally you should be sending one tick every 10 ms, for -100 commands a second. This has a nice added benefit of allowing for -very precise control. +60000 (full power). It corresponds to the mean thrust that will be +appied to the motors. There is a battery compensation algorythm +applied to make the thrust mostly independent of battery voltage. +Roll/pitch are in degree and yarate in degree/seconds. + +This command will set the attitude controller setpoint for the next +500ms. After 500ms without net setpoint, the Crazyflie will apply a +setpoint with the same thrust but with roll/pitch/yawrate = 0, this +will make the Crazyflie stop accelerate. After 2secons without new +setpoint the Crazyflie will cut power to the motors. + +Note that this command implements a motor lock mechanism that is +intended to avoid flyaway when connecting a gamepad. You must send +one command with thrust = 0 in order to unlock the command. This +unlock procedure needs to be repeated if the watchdog describe above +kicks-in. ## Parameters From f7704d77ab8d712fdd92531bb6d33b36d3965233 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 10 Dec 2020 09:25:36 +0100 Subject: [PATCH 094/861] Closes #181: Replace isAlive() with is_alive() --- cflib/crazyflie/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 56e5e6c02..24ef331f9 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -231,7 +231,7 @@ def open_link(self, link_uri): logger.warning(message) self.connection_failed.call(link_uri, message) else: - if not self.incoming.isAlive(): + if not self.incoming.is_alive(): self.incoming.start() # Add a callback so we can check that any data is coming # back from the copter From 8be6fbd05ef0b72cab151b3782c6209db98ebfc0 Mon Sep 17 00:00:00 2001 From: Janco Kock Date: Thu, 17 Dec 2020 09:13:16 +0100 Subject: [PATCH 095/861] Change connected callback to link_established Fixes https://github.com/bitcraze/crazyflie-lib-python/issues/183 --- examples/cfbridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/cfbridge.py b/examples/cfbridge.py index 0ff43a13f..5456bdee7 100755 --- a/examples/cfbridge.py +++ b/examples/cfbridge.py @@ -42,7 +42,7 @@ def __init__(self, link_uri): self._cf = Crazyflie() # Connect some callbacks from the Crazyflie API - self._cf.connected.add_callback(self._connected) + 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) From cd60bdd36e7a8736d48570c3c465634475dbb684 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 17 Dec 2020 14:21:53 +0100 Subject: [PATCH 096/861] Fix log packet payload lenght documentation --- docs/user-guides/python_api.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 852494b58..609d723ed 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -62,10 +62,9 @@ following way: There\'s a few limitations that needs to be taken into account: -- Each packet is limited to 32bytes, which means that the data that is - logged and the packet that is sent to set it up cannot be larger - than this. It limits the logging to about 14 variables, but this is - dependent on what types they are +- The maximum byte lenght for a log packet is of 26 bytes. This for + for example allows to log 6 floats and one uint16_t (6*4 + 2 bytes) + in a single packet. - The minimum period of a for a log configuration is multiples of 10ms ### Parameters From 460ae8ac2225fc0810a7f606aeada12480199b3e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 22 Dec 2020 14:27:15 +0100 Subject: [PATCH 097/861] (#159) motion commander fix of deck detection --- docs/user-guides/sbs_motion_commander.md | 15 +++++++++------ examples/step-by-step/sbs_motion_commander.py | 9 +++++---- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 66058942d..4e9e30ce8 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -58,9 +58,10 @@ We want to know if the deck is correctly attached before flying, therefore we wi Above `__main__`, start a parameter callback function: ``` -def param_deck_flow(name, value): - global is_deck_attached +def param_deck_flow(name, value_str): + value = int(value_str) print(value) + global is_deck_attached if value: is_deck_attached = True print('Deck is attached!') @@ -69,7 +70,7 @@ def param_deck_flow(name, value): print('Deck is NOT attached!') ``` -The `is_deck_attached` is a global variable which should be defined under `URI` +The `is_deck_attached` is a global variable which should be defined under `URI`. Note that the value type that the `param_deck_flow()` is a string type, so you will need to convert it to a number first before you can do any operations with it. ``` ... @@ -95,9 +96,10 @@ is_deck_attached = False logging.basicConfig(level=logging.ERROR) -def param_deck_flow(name, value): - global is_deck_attached +def param_deck_flow(name, value_str): + value = int(value_str) print(value) + global is_deck_attached if value: is_deck_attached = True print('Deck is attached!') @@ -594,8 +596,9 @@ def log_pos_callback(timestamp, data, logconf): def param_deck_flow(name, value): - global is_deck_attached + value = int(value_str) print(value) + global is_deck_attached if value: is_deck_attached = True print('Deck is attached!') diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index 552455455..012e4b4d4 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -95,9 +95,10 @@ def log_pos_callback(timestamp, data, logconf): position_estimate[1] = data['stateEstimate.y'] -def param_deck_flow(name, value): - global is_deck_attached +def param_deck_flow(name, value_str): + value = int(value_str) print(value) + global is_deck_attached if value: is_deck_attached = True print('Deck is attached!') @@ -126,5 +127,5 @@ def param_deck_flow(name, value): # take_off_simple(scf) # move_linear_simple(scf) - move_box_limit(scf) - logconf.stop() + #move_box_limit(scf) + #logconf.stop() From 4698f53b2974b1ed274ed6706c5675d28b853cce Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 22 Dec 2020 14:36:40 +0100 Subject: [PATCH 098/861] fixed pep8 error --- examples/step-by-step/sbs_motion_commander.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index 012e4b4d4..aa0f44efb 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -127,5 +127,5 @@ def param_deck_flow(name, value_str): # take_off_simple(scf) # move_linear_simple(scf) - #move_box_limit(scf) - #logconf.stop() + # move_box_limit(scf) + # logconf.stop() From a2f4c1294b93e5bb424296521477089d645a72f8 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 22 Dec 2020 14:53:58 +0100 Subject: [PATCH 099/861] (#159) replaced things in the doc --- docs/user-guides/sbs_motion_commander.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 4e9e30ce8..9b34ce9d6 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -192,7 +192,7 @@ def take_off_simple(scf): time.sleep(3) mc.stop() -def param_deck_flow(name, value): +def param_deck_flow(name, value_str): ... if __name__ == '__main__': @@ -275,7 +275,7 @@ def move_linear_simple(scf): def take_off_simple(scf): ... -def param_deck_flow(name, value): +def param_deck_flow(name, value_str): ... @@ -370,7 +370,7 @@ def log_pos_callback(timestamp, data, logconf): position_estimate[1] = data['stateEstimate.y'] -def param_deck_flow(name, value): +def param_deck_flow(name, value_str): ... if __name__ == '__main__': @@ -481,7 +481,7 @@ def take_off_simple(scf): def log_pos_callback(timestamp, data, logconf): ... -def param_deck_flow(name, value): +def param_deck_flow(name, value_str): ... if __name__ == '__main__': @@ -595,7 +595,7 @@ def log_pos_callback(timestamp, data, logconf): position_estimate[1] = data['stateEstimate.y'] -def param_deck_flow(name, value): +def param_deck_flow(name, value_str): value = int(value_str) print(value) global is_deck_attached From 68c20a041dca36a440795ed43a5ac9683299096e Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 4 Jan 2021 14:49:53 +0100 Subject: [PATCH 100/861] Closes #170: Rename GH workflow, remove travis --- .github/workflows/{build.yml => CI.yml} | 3 +-- .travis.yml | 6 ------ 2 files changed, 1 insertion(+), 8 deletions(-) rename .github/workflows/{build.yml => CI.yml} (96%) delete mode 100644 .travis.yml diff --git a/.github/workflows/build.yml b/.github/workflows/CI.yml similarity index 96% rename from .github/workflows/build.yml rename to .github/workflows/CI.yml index df919ef74..0b158d4ff 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/CI.yml @@ -1,6 +1,5 @@ # Run check and build of the lib using the Bitcraze builder docker image - -name: build +name: CI on: push: diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index cea5d62e7..000000000 --- a/.travis.yml +++ /dev/null @@ -1,6 +0,0 @@ -matrix: - include: - - os: linux - sudo: required - services: docker - script: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build From 38ddff833547c1d47b373e5d61e74f14491f04e3 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 4 Jan 2021 14:51:30 +0100 Subject: [PATCH 101/861] #170: Remove Travis from Readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 1d171cfac..b73d45a9b 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# cflib: Crazyflie python library [![Build Status](https://api.travis-ci.org/bitcraze/crazyflie-lib-python.svg)](https://travis-ci.org/bitcraze/crazyflie-lib-python) [![build](https://github.com/bitcraze/crazyflie-lib-python/workflows/build/badge.svg)](https://github.com/bitcraze/crazyflie-lib-python/actions) +# cflib: Crazyflie python library [![CI](https://github.com/bitcraze/crazyflie-lib-python/workflows/CI/badge.svg)](https://github.com/bitcraze/crazyflie-lib-python/actions) cflib is an API written in Python that is used to communicate with the Crazyflie and Crazyflie 2.0 quadcopters. It is intended to be used by client software to From b97913762f6b5dae33c4d1767827d524f21ac1b7 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 26 Jan 2021 10:30:11 +0100 Subject: [PATCH 102/861] Update setup.py version to 0.13 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 43cda1bb5..43cdf5344 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.12.1', + version='0.1.13', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 25d6a9737c2f3eb56f6ede8812758b976283484a Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 26 Jan 2021 11:56:13 +0100 Subject: [PATCH 103/861] Restore functionality of autonomous_sequence_high_level.py example. Closes #186 --- examples/autonomous_sequence_high_level.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/autonomous_sequence_high_level.py b/examples/autonomous_sequence_high_level.py index ff820a72b..e59ca0e80 100644 --- a/examples/autonomous_sequence_high_level.py +++ b/examples/autonomous_sequence_high_level.py @@ -189,8 +189,8 @@ def run_sequence(cf, trajectory_id, duration): trajectory_id = 1 activate_high_level_commander(cf) - activate_mellinger_controller(cf) + # activate_mellinger_controller(cf) duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) - # reset_estimator(cf) - # run_sequence(cf, trajectory_id, duration) + reset_estimator(cf) + run_sequence(cf, trajectory_id, duration) From 4f3525e8bd2e55e7203753e0639e9f85c2128f9d Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 26 Jan 2021 15:42:35 +0100 Subject: [PATCH 104/861] Closes #187: Fix radio USB timeout on Linux --- cflib/drivers/crazyradio.py | 3 +++ cflib/utils/power_switch.py | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index c3d470689..85539d23c 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -29,6 +29,7 @@ """ import logging import os +import platform import usb @@ -154,6 +155,8 @@ def __init__(self, device=None, devid=0, serial=None): logger.warning('You should update to Crazyradio firmware V0.4+') # Reset the dongle to power up settings + if platform.system() == "Linux": + self.handle.reset() self.set_data_rate(self.DR_2MPS) self.set_channel(2) self.arc = -1 diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index e7959dcb5..9bcd14cbe 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -76,7 +76,7 @@ def _send(self, cmd): success = False for i in range(50): res = cr.send_packet(packet) - if res.ack: + if res and res.ack: success = True break From 0bcb554084db500de200b5a3660f17bebd5333db Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 26 Jan 2021 15:50:47 +0100 Subject: [PATCH 105/861] Fix flake8 --- cflib/utils/power_switch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index 9bcd14cbe..1371e6392 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -76,7 +76,7 @@ def _send(self, cmd): success = False for i in range(50): res = cr.send_packet(packet) - if res and res.ack: + if res and res.ack: success = True break From 4e9ff3e4fd9141d53a17a78ce08ac8b233b1fb7b Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 26 Jan 2021 15:56:46 +0100 Subject: [PATCH 106/861] Fix doublequote --- cflib/drivers/crazyradio.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 85539d23c..0d57c11d4 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -155,7 +155,7 @@ def __init__(self, device=None, devid=0, serial=None): logger.warning('You should update to Crazyradio firmware V0.4+') # Reset the dongle to power up settings - if platform.system() == "Linux": + if platform.system() == 'Linux': self.handle.reset() self.set_data_rate(self.DR_2MPS) self.set_channel(2) From 6a78cc255a03281d7206154efaee5cbf121882c2 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 27 Jan 2021 17:19:34 +0100 Subject: [PATCH 107/861] #189: Solve race condition in shared radio --- cflib/crtp/radiodriver.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index fc1683588..50415055e 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -148,6 +148,8 @@ def __init__(self, devid: int): self._devid = devid self.version = self._radio.version + self.name = "Shared Radio" + self._cmd_queue = Queue() # type: Queue[Tuple[int, _RadioCommands, Any]] # noqa self._rsp_queues = {} # type: Dict[int, Queue[Any]] self._next_instance_id = 0 @@ -162,10 +164,14 @@ def open_instance(self) -> _SharedRadioInstance: instance_id = self._next_instance_id self._rsp_queues[instance_id] = rsp_queue self._next_instance_id += 1 + + if self._radio is None: + self._radio = Crazyradio(devid=self._devid) + return _SharedRadioInstance(instance_id, self._cmd_queue, rsp_queue, - self._radio.version) + self.version) def run(self): while True: @@ -176,8 +182,7 @@ def run(self): del self._rsp_queues[command[0]] if len(self._rsp_queues) == 0: self._radio.close() - _RadioManager.remove(self._devid) - return + self._radio = None elif command[1] == _RadioCommands.SEND_PACKET: channel, address, datarate, data = command[2] self._radio.set_channel(channel) @@ -217,7 +222,7 @@ def open(devid: int) -> _SharedRadioInstance: shared_radio = _SharedRadio(devid) _RadioManager._radios[devid] = shared_radio - return shared_radio.open_instance() + return shared_radio.open_instance() @staticmethod def remove(devid: int): From 6678e85cfff9e4a92a80598c6eafee5ab371c472 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 28 Jan 2021 10:06:22 +0100 Subject: [PATCH 108/861] Use sharedradio in powerswitch --- cflib/crtp/radiodriver.py | 24 ++++++++++++------------ cflib/utils/power_switch.py | 6 +++--- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 50415055e..870a2af07 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -206,28 +206,28 @@ def run(self): self._rsp_queues[command[0]].put(resp) -class _RadioManager: +class RadioManager: _radios = [] # type: List[Union[_SharedRadio, None]] _lock = Semaphore(1) @staticmethod def open(devid: int) -> _SharedRadioInstance: - with _RadioManager._lock: - if len(_RadioManager._radios) <= devid: - padding = [None] * (devid - len(_RadioManager._radios) + 1) - _RadioManager._radios.extend(padding) + with RadioManager._lock: + if len(RadioManager._radios) <= devid: + padding = [None] * (devid - len(RadioManager._radios) + 1) + RadioManager._radios.extend(padding) - shared_radio = _RadioManager._radios[devid] + shared_radio = RadioManager._radios[devid] if not shared_radio: shared_radio = _SharedRadio(devid) - _RadioManager._radios[devid] = shared_radio + RadioManager._radios[devid] = shared_radio return shared_radio.open_instance() @staticmethod def remove(devid: int): - with _RadioManager._lock: - _RadioManager._radios[devid] = None + with RadioManager._lock: + RadioManager._radios[devid] = None class RadioDriver(CRTPDriver): @@ -260,7 +260,7 @@ def connect(self, uri, link_quality_callback, link_error_callback): self.uri = uri if self._radio is None: - self._radio = _RadioManager.open(devid) + self._radio = RadioManager.open(devid) self._radio.set_channel(channel) self._radio.set_data_rate(datarate) self._radio.set_address(address) @@ -442,7 +442,7 @@ def scan_interface(self, address): if self._radio is None: try: - self._radio = _RadioManager.open(0) + self._radio = RadioManager.open(0) except Exception as e: print(e) return [] @@ -489,7 +489,7 @@ def scan_interface(self, address): def get_status(self): try: - radio = _RadioManager.open(0) + radio = RadioManager.open(0) ver = radio.version diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index 1371e6392..76ad891c5 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -26,7 +26,7 @@ import time import cflib.crtp -from cflib.drivers.crazyradio import Crazyradio +from cflib.crtp.radiodriver import RadioManager class PowerSwitch: @@ -65,9 +65,9 @@ def stm_power_cycle(self): self.stm_power_up() def _send(self, cmd): - packet = (0xf3, 0xfe, cmd) + packet = [0xf3, 0xfe, cmd] - cr = Crazyradio(devid=self.devid) + cr = RadioManager.open(devid=self.devid) cr.set_channel(self.channel) cr.set_data_rate(self.datarate) cr.set_address(self.address) From f4d65bb4067ad25379ebc08cf660f00a6d835079 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 28 Jan 2021 10:06:34 +0100 Subject: [PATCH 109/861] Launch radiodriver threads as deamon --- cflib/crtp/radiodriver.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 870a2af07..4d0026929 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -156,6 +156,8 @@ def __init__(self, devid: int): self._lock = Semaphore(1) + self.setDaemon(True) + self.start() def open_instance(self) -> _SharedRadioInstance: From 738ec1b7fa7825cacbc12004079537f85a00f9f9 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 28 Jan 2021 11:20:07 +0100 Subject: [PATCH 110/861] Fix precomit double quote --- cflib/crtp/radiodriver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 4d0026929..d28b342ed 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -148,7 +148,7 @@ def __init__(self, devid: int): self._devid = devid self.version = self._radio.version - self.name = "Shared Radio" + self.name = 'Shared Radio' self._cmd_queue = Queue() # type: Queue[Tuple[int, _RadioCommands, Any]] # noqa self._rsp_queues = {} # type: Dict[int, Queue[Any]] From 68b27f1772f906ac76cb941ef8834ed4adb2c1a9 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 28 Jan 2021 11:27:16 +0100 Subject: [PATCH 111/861] Update version to 0.1.13.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 43cdf5344..81d595268 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.13', + version='0.1.13.1', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From f985b0066dc6f3be32e3488e4ae496a6511eb14c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 1 Feb 2021 15:24:12 +0100 Subject: [PATCH 112/861] Make sure all examples use the PID controller and not the Mellinger controller. Closes #190 --- examples/position_commander_demo.py | 2 +- examples/qualisys/qualisys_hl_commander.py | 2 +- examples/swarm/synchronizedSequence.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/position_commander_demo.py b/examples/position_commander_demo.py index e00a28f0b..d2e1adfaa 100644 --- a/examples/position_commander_demo.py +++ b/examples/position_commander_demo.py @@ -47,7 +47,7 @@ def slightly_more_complex_usage(): x=0.0, y=0.0, z=0.0, default_velocity=0.3, default_height=0.5, - controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: + controller=PositionHlCommander.CONTROLLER_PID) as pc: # Go to a coordinate pc.go_to(1.0, 1.0, 1.0) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index dc30c68b4..5d95e03cf 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -306,7 +306,7 @@ def run_sequence(cf, trajectory_id, duration): activate_kalman_estimator(cf) activate_high_level_commander(cf) - activate_mellinger_controller(cf) + # activate_mellinger_controller(cf) duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) reset_estimator(cf) diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index fc9e6b601..fe97e13ad 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -178,7 +178,7 @@ def crazyflie_control(scf): cf = scf.cf control = controlQueues[uris.index(cf.link_uri)] - activate_mellinger_controller(scf, True) + activate_mellinger_controller(scf, False) commander = scf.cf.high_level_commander From 2293a4f70df72c16f6815b8216e26206b3dabfaf Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 1 Feb 2021 17:37:05 +0100 Subject: [PATCH 113/861] Increased accepted max line length to 120 chars for flake8. Closes #191 --- tox.ini | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tox.ini b/tox.ini index 4e48e7391..16eaa95fa 100644 --- a/tox.ini +++ b/tox.ini @@ -7,10 +7,13 @@ deps = -rrequirements-dev.txt commands= coverage erase coverage run -m unittest discover ./test - coverage report --show-missing + coverage report --show-missing pre-commit run --all-files [testenv:venv] envdir = venv-{[tox]project} commands = + +[flake8] +max-line-length:120 From 8aedc08c34fba1da2db19ecf01b2bd6c7ac56dc1 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 2 Feb 2021 16:42:14 +0100 Subject: [PATCH 114/861] #192 Improved support for lighthouse base station geometry handling --- cflib/crazyflie/mem/lighthouse_memory.py | 62 +++++++++++++++++++----- 1 file changed, 51 insertions(+), 11 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index b7bb7d738..b3a3e96a1 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -172,31 +172,33 @@ def __init__(self, id, type, size, mem_handler): def new_data(self, mem, addr, data): """Callback for when new memory data has been fetched""" if mem.id == self.id: + tmp_update_finished_cb = self._update_finished_cb + self._clear_update_cb() + if addr < self.CALIB_START_ADDR: geo_data = LighthouseBsGeometry() geo_data.set_from_mem_data(data) - if self._update_finished_cb: - self._update_finished_cb(self, geo_data) + if tmp_update_finished_cb: + tmp_update_finished_cb(self, geo_data) else: calibration_data = LighthouseBsCalibration() calibration_data.set_from_mem_data(data) - if self._update_finished_cb: - self._update_finished_cb(self, calibration_data) - - self._clear_update_cb() + if tmp_update_finished_cb: + tmp_update_finished_cb(self, calibration_data) def new_data_failed(self, mem, addr, data): """Callback when a read failed""" if mem.id == self.id: - if self._update_failed_cb: - logger.debug('Update of data failed') - self._update_failed_cb(self) + tmp_update_failed_cb = self._update_failed_cb self._clear_update_cb() - def read_geo_data(self, bs_id, update_finished_cb, - update_failed_cb=None): + if tmp_update_failed_cb: + logger.debug('Update of data failed') + tmp_update_failed_cb(self) + + def read_geo_data(self, bs_id, update_finished_cb, update_failed_cb=None): """Request a read of geometry data for one base station""" if self._update_finished_cb: raise Exception('Read operation already ongoing') @@ -271,3 +273,41 @@ def _clear_update_cb(self): def _clear_write_cb(self): self._write_finished_cb = None self._write_failed_cb = None + + +class LighthouseMemHelper: + """Helper to access all geometry data located in crazyflie memory subsystem""" + + NR_OF_CHANNELS = 16 + + def __init__(self, cf): + self._cf = cf + self._result_geos = None + + mems = self._cf.mem.get_mems(MemoryElement.TYPE_LH) + count = len(mems) + if count != 1: + raise Exception('Unexpected nr of memories found:', count) + + self._lh_mem = mems[0] + + def read_all_geos(self, read_done_cb): + self._result_geos = {} + self._next_geo_get_id = 0 + self._read_geos_done_cb = read_done_cb + self._get_geo(0) + + def _geo_data_updated(self, mem, geo_data): + self._result_geos[self._next_geo_get_id] = geo_data + self._next_geo_get_id += 1 + self._get_geo(self._next_geo_get_id) + + def _update_failed(self, mem): + self._next_geo_get_id += 1 + self._get_geo(self._next_geo_get_id) + + def _get_geo(self, channel): + if channel < self.NR_OF_CHANNELS: + self._lh_mem.read_geo_data(channel, self._geo_data_updated, update_failed_cb=self._update_failed) + else: + self._read_geos_done_cb(self._result_geos) From 3c87a8b9df85705c6f981ac8ca3b23c46cbf2972 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 10 Feb 2021 14:19:53 +0100 Subject: [PATCH 115/861] #192 Moved core functionality of base station geometry estimation from the crazyflie firmware tools to the python lib --- cflib/localization/__init__.py | 23 ++ cflib/localization/lighthouse_bs_geo.py | 211 ++++++++++++++++++ cflib/localization/lighthouse_bs_vector.py | 109 +++++++++ setup.py | 5 +- test/localization/__init__.py | 0 test/localization/test_lighthouse_bs_geo.py | 97 ++++++++ .../localization/test_lighthouse_bs_vector.py | 119 ++++++++++ 7 files changed, 563 insertions(+), 1 deletion(-) create mode 100644 cflib/localization/__init__.py create mode 100644 cflib/localization/lighthouse_bs_geo.py create mode 100644 cflib/localization/lighthouse_bs_vector.py create mode 100644 test/localization/__init__.py create mode 100644 test/localization/test_lighthouse_bs_geo.py create mode 100644 test/localization/test_lighthouse_bs_vector.py diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py new file mode 100644 index 000000000..d776760fa --- /dev/null +++ b/cflib/localization/__init__.py @@ -0,0 +1,23 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2017 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py new file mode 100644 index 000000000..8771063d2 --- /dev/null +++ b/cflib/localization/lighthouse_bs_geo.py @@ -0,0 +1,211 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +""" +Functionality to handle base station geometry in the lighthouse poistioning system +""" +import math + +import cv2 as cv +import numpy as np + + +class LighthouseBsGeoEstimator: + """ + This class is used to estimate the geometry (position and attitude) + of a lighthouse base station, given angles measured using a lighthouse deck. + """ + + def __init__(self): + self._directions = { + self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), + self._hash_sensor_order([2, 0, 3, 1]): math.radians(25), + self._hash_sensor_order([2, 3, 0, 1]): math.radians(65), + self._hash_sensor_order([3, 2, 0, 1]): math.radians(90), + self._hash_sensor_order([3, 2, 1, 0]): math.radians(115), + self._hash_sensor_order([3, 1, 2, 0]): math.radians(155), + self._hash_sensor_order([1, 3, 2, 0]): math.radians(180), + self._hash_sensor_order([1, 3, 0, 2]): math.radians(205), + self._hash_sensor_order([1, 0, 3, 2]): math.radians(245), + self._hash_sensor_order([0, 1, 3, 2]): math.radians(270), + self._hash_sensor_order([0, 1, 2, 3]): math.radians(295), + self._hash_sensor_order([0, 2, 1, 3]): math.radians(335), + } + + # Sensor distances on the lighthouse deck + sensor_distance_width = 0.015 + sensor_distance_length = 0.03 + + # Sensor positions in world coordinates, open cv style + self._lighthouse_3d = np.float32( + [ + [-sensor_distance_width / 2, 0, -sensor_distance_length / 2], + [sensor_distance_width / 2, 0, -sensor_distance_length / 2], + [-sensor_distance_width / 2, 0, sensor_distance_length / 2], + [sensor_distance_width / 2, 0, sensor_distance_length / 2] + ]) + + # Camera matrix + self._K = np.float64( + [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0] + ]) + + self._dist_coef = np.zeros(4) + + # Sanity check maximum pos + self._sanity_max_pos = 10 + + def estimate_geometry(self, bs_vectors): + """ + Estimate the full pose of a base station based on angles from the 4 sensors + on a lighthouse deck. The result is a rotation matrix and position of the + base station, in the Crazyflie reference frame. + + :param bs_vectors A list of 4 LighthouseBsVector objects specifying vectors to the 4 sensors + :return rot_bs_in_cf_coord: Rotation matrix of the BS in the CFs coordinate system + :return pos_bs_in_cf_coord: Position vector of the BS in the CFs coordinate system + """ + guess_yaw = self._find_initial_yaw_guess(bs_vectors) + rvec_guess, tvec_guess = self._convert_yaw_to_open_cv(guess_yaw) + rw_ocv, tw_ocv = self._estimate_pose_by_pnp(bs_vectors, rvec_guess, tvec_guess) + rot_bs_in_cf_coord, pos_bs_in_cf_coord = self._opencv_to_cf(rw_ocv, tw_ocv) + return rot_bs_in_cf_coord, pos_bs_in_cf_coord + + def sanity_check_result(self, pos_bs_in_cf_coord): + """ + Checks if the estimated geometry is within reasonable bounds. It returns + true if it seems reasonable or false if it doesn't + """ + for coord in pos_bs_in_cf_coord: + if (abs(coord) > self._sanity_max_pos): + return False + return True + + def _find_initial_yaw_guess(self, bs_vectors): + # Assume bs is faicing slighly downwards and fairly horizontal + # Sort sensors in the order they are hit by the horizontal sweep + # and use the order to figure out roughly the direction to the + # base station + sweeps_x = { + 0: bs_vectors[0].lh_v1_horiz_angle, + 1: bs_vectors[1].lh_v1_horiz_angle, + 2: bs_vectors[2].lh_v1_horiz_angle, + 3: bs_vectors[3].lh_v1_horiz_angle + } + + ordered_map = {k: v for k, v in sorted(sweeps_x.items(), key=lambda item: item[1])} + sensor_order = list(ordered_map.keys()) + + # The base station is roughly in this direction, in CF (world) coordinates + return self._directions[self._hash_sensor_order(sensor_order)] + + def _hash_sensor_order(self, order): + hash = 0 + for i in range(4): + hash += order[i] * 4 ** i + return hash + + def _convert_yaw_to_open_cv(self, yaw): + # Base station height + bs_h = 2.0 + # Distance to base station along the floor + bs_fd = 3.0 + # Distance to base station + bs_dist = math.sqrt(bs_h ** 2 + bs_fd ** 2) + elevation = math.atan2(bs_h, bs_fd) + + # Initial position of the CF in camera coordinate system, open cv style + tvec_start = np.array([0, 0, bs_dist]) + + # Calculate rotation matrix + d_c = math.cos(-yaw + math.pi) + d_s = math.sin(-yaw + math.pi) + R_rot_y = np.array([ + [d_c, 0.0, d_s], + [0.0, 1.0, 0.0], + [-d_s, 0.0, d_c], + ]) + + e_c = math.cos(elevation) + e_s = math.sin(elevation) + R_rot_x = np.array([ + [1.0, 0.0, 0.0], + [0.0, e_c, -e_s], + [0.0, e_s, e_c], + ]) + + R = np.dot(R_rot_x, R_rot_y) + rvec_start, _ = cv.Rodrigues(R) + + return rvec_start, tvec_start + + def _estimate_pose_by_pnp(self, bs_vectors, rvec_start, tvec_start): + # Sensors as seen by the "camera" + lighthouse_image_projection = np.float32( + [ + [-math.tan(bs_vectors[0].lh_v1_horiz_angle), -math.tan(bs_vectors[0].lh_v1_vert_angle)], + [-math.tan(bs_vectors[1].lh_v1_horiz_angle), -math.tan(bs_vectors[1].lh_v1_vert_angle)], + [-math.tan(bs_vectors[2].lh_v1_horiz_angle), -math.tan(bs_vectors[2].lh_v1_vert_angle)], + [-math.tan(bs_vectors[3].lh_v1_horiz_angle), -math.tan(bs_vectors[3].lh_v1_vert_angle)] + ]) + + _ret, rvec_est, tvec_est = cv.solvePnP( + self._lighthouse_3d, + lighthouse_image_projection, + self._K, + self._dist_coef, + flags=cv.SOLVEPNP_ITERATIVE, + rvec=rvec_start, + tvec=tvec_start, + useExtrinsicGuess=True) + + if not _ret: + raise Exception('No solution found') + + Rw_ocv, Tw_ocv = self._cam_to_world(rvec_est, tvec_est) + return Rw_ocv, Tw_ocv + + def _cam_to_world(self, rvec_c, tvec_c): + R_c, _ = cv.Rodrigues(rvec_c) + R_w = np.linalg.inv(R_c) + tvec_w = -np.matmul(R_w, tvec_c) + return R_w, tvec_w + + def _opencv_to_cf(self, R_cv, t_cv): + R_opencv_to_cf = np.array([ + [0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + ]) + + R_cf_to_opencv = np.array([ + [0.0, -1.0, 0.0], + [0.0, 0.0, -1.0], + [1.0, 0.0, 0.0], + ]) + + t_cf = np.dot(R_opencv_to_cf, t_cv) + R_cf = np.dot(R_opencv_to_cf, np.dot(R_cv, R_cf_to_opencv)) + + return R_cf, t_cf diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py new file mode 100644 index 000000000..f0bd00d01 --- /dev/null +++ b/cflib/localization/lighthouse_bs_vector.py @@ -0,0 +1,109 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 math +import numpy as np + + +class LighthouseBsVector: + """ + This class is representing a vector from a base station into space, + in the base station reference frame. Typically the intersection of + two light planes. + It also provides functionality to convert between lighthouse V1 angles, + V2 angles and cartesian coordinates. + """ + + T = math.pi / 6 + + def __init__(self, lh_v1_horiz_angle, lh_v1_vert_angle): + """ + Initialize from lighthouse V1 angles + :param lh_v1_horiz_angle: horizontal sweep angle, 0 straight forward. Right (seen from the bs) is negative, + left is positive + :param lh_v1_vert_angle: vertical sweep angle, 0 straight forward. Down is negative, up is positive. + """ + self._lh_v1_horiz_angle = lh_v1_horiz_angle + self._lh_v1_vert_angle = lh_v1_vert_angle + + @classmethod + def from_lh2(cls, lh_v2_angle_1, lh_v2_angle_2): + """ + Create a LighthouseBsVector object from lighthouse V2 angles + :param lh_v2_angle_1: First sweep angles, 0 straight ahead + :param lh_v2_angle_2: Second sweep angles, 0 straight ahead + """ + a1 = lh_v2_angle_1 + a2 = lh_v2_angle_2 + lh_v1_horiz_angle = (a1 + a2) / 2.0 + lh_v1_vert_angle = math.atan2(math.sin(a2 - a1), math.tan(cls.T) * (math.cos(a1) + math.cos(a2))) + + return cls(lh_v1_horiz_angle, lh_v1_vert_angle) + + @classmethod + def from_cart(cls, cart_vector): + """ + Create a LighthouseBsVector object from cartesian coordinates. + :param cart_vector: (x, y, z) to a point + """ + lh_v1_horiz_angle = math.atan2(cart_vector[1], cart_vector[0]) + lh_v1_vert_angle = math.atan2(cart_vector[2], cart_vector[0]) + + return cls(lh_v1_horiz_angle, lh_v1_vert_angle) + + @property + def lh_v1_horiz_angle(self): + """ + Lightouse V1 horizontal sweep angle + """ + return self._lh_v1_horiz_angle + + @property + def lh_v1_vert_angle(self): + """ + Lightouse V1 vertical sweep angle + """ + return self._lh_v1_vert_angle + + @property + def lh_v2_angle_1(self): + """ + Lightouse V2 first sweep angle + """ + return self._lh_v1_horiz_angle + math.asin(self._q() * math.tan(-self.T)) + + @property + def lh_v2_angle_2(self): + """ + Lightouse V2 second sweep angle + """ + return self._lh_v1_horiz_angle + math.asin(self._q() * math.tan(self.T)) + + @property + def cart(self): + """ + A normalized vector in cartesian coordinates + """ + v = np.array((1, math.tan(self._lh_v1_horiz_angle), math.tan(self._lh_v1_vert_angle))) + return v / np.linalg.norm(v) + + def _q(self): + return math.tan(self._lh_v1_vert_angle) / math.sqrt(1 + math.tan(self._lh_v1_horiz_angle) ** 2) diff --git a/setup.py b/setup.py index 81d595268..b959f6bf1 100644 --- a/setup.py +++ b/setup.py @@ -24,5 +24,8 @@ keywords='driver crazyflie quadcopter', - install_requires='pyusb>=1.0.0b2', + install_requires=[ + 'pyusb>=1.0.0b2', + 'opencv-python==4.5.1.48' + ] ) diff --git a/test/localization/__init__.py b/test/localization/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/test/localization/test_lighthouse_bs_geo.py b/test/localization/test_lighthouse_bs_geo.py new file mode 100644 index 000000000..99ae190b2 --- /dev/null +++ b/test/localization/test_lighthouse_bs_geo.py @@ -0,0 +1,97 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 math +import unittest + +from cflib.localization.lighthouse_bs_geo import LighthouseBsGeoEstimator +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector + + +class TestLighthouseBsGeoEstimator(unittest.TestCase): + def setUp(self): + + self.sut = LighthouseBsGeoEstimator() + + def test_that_initial_yaw_guess_is_correct_ba_is_behind(self): + # Fixture + bs_vectors = [ + LighthouseBsVector(1, 0), + LighthouseBsVector(2, 0), + LighthouseBsVector(0, 0), + LighthouseBsVector(3, 0), + ] + + # Test + actual = self.sut._find_initial_yaw_guess(bs_vectors) + + # Assert + self.assertEqual(0.0, actual) + + def test_that_initial_yaw_guess_is_correct_ba_is_front(self): + # Fixture 1, 3, 2, 0 + bs_vectors = [ + LighthouseBsVector(3, 0), + LighthouseBsVector(0, 0), + LighthouseBsVector(2, 0), + LighthouseBsVector(1, 0), + ] + + # Test + actual = self.sut._find_initial_yaw_guess(bs_vectors) + + # Assert + self.assertEqual(math.radians(180), actual) + + def test_that_initial_yaw_guess_is_correct_bs_left_behind(self): + # Fixture + bs_vectors = [ + LighthouseBsVector(1.0, 0), + LighthouseBsVector(-0.5, 0), + LighthouseBsVector(0.5, 0), + LighthouseBsVector(-1.0, 0), + ] + + # Test + actual = self.sut._find_initial_yaw_guess(bs_vectors) + + # Assert + self.assertEqual(math.radians(155), actual) + + def test_that_sanity_check_finds_coordinate_out_of_bounds(self): + # Fixture + pos_bs_in_cf_coord = [0, -20, 0] + + # Test + actual = self.sut.sanity_check_result(pos_bs_in_cf_coord) + + # Assert + self.assertFalse(actual) + + def test_that_sanity_check_passes_ok_position(self): + # Fixture + pos_bs_in_cf_coord = [0, 1, 2] + + # Test + actual = self.sut.sanity_check_result(pos_bs_in_cf_coord) + + # Assert + self.assertTrue(actual) diff --git a/test/localization/test_lighthouse_bs_vector.py b/test/localization/test_lighthouse_bs_vector.py new file mode 100644 index 000000000..a55705e16 --- /dev/null +++ b/test/localization/test_lighthouse_bs_vector.py @@ -0,0 +1,119 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 unittest +import math +import numpy as np + +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector + + +class TestLighthouseBsVector(unittest.TestCase): + def setUp(self): + pass + + def test_init_from_lh1_angles(self): + # Fixture + horiz = 0.123 + vert = -1.23 + + # Test + actual = LighthouseBsVector(horiz, vert) + + # Assert + self.assertEqual(horiz, actual.lh_v1_horiz_angle) + self.assertEqual(vert, actual.lh_v1_vert_angle) + + def test_conversion_to_lh2_angles_are_zero_straight_forward(self): + # Fixture + horiz = 0 + vert = 0 + + # Test + actual = LighthouseBsVector(horiz, vert) + + # Assert + self.assertEqual(0.0, actual.lh_v2_angle_1) + self.assertEqual(0.0, actual.lh_v2_angle_2) + + def test_conversion_to_lh2_angles_are_equal_with_vert_zero(self): + # Fixture + horiz = 1.0 + vert = 0.0 + + # Test + actual = LighthouseBsVector(horiz, vert) + + # Assert + self.assertEqual(actual.lh_v2_angle_1, actual.lh_v2_angle_2) + + def test_conversion_to_from_lh2(self): + # Fixture + horiz = 0.123 + vert = -0.987 + v1 = LighthouseBsVector(horiz, vert) + + # Test + actual = LighthouseBsVector.from_lh2(v1.lh_v2_angle_1, v1.lh_v2_angle_2) + + # Assert + self.assertAlmostEqual(horiz, actual.lh_v1_horiz_angle) + self.assertAlmostEqual(vert, actual.lh_v1_vert_angle) + + def test_conversion_to_cartesian_straight_forward(self): + # Fixture + horiz = 0.0 + vert = 0.0 + vector = LighthouseBsVector(horiz, vert) + + # Test + actual = vector.cart + + # Assert + self.assertAlmostEqual(1.0, actual[0]) + self.assertAlmostEqual(0.0, actual[1]) + self.assertAlmostEqual(0.0, actual[2]) + + def test_conversion_to_from_cartesian(self): + # Fixture + horiz = 0.123 + vert = -0.987 + v1 = LighthouseBsVector(horiz, vert) + + # Test + actual = LighthouseBsVector.from_cart(v1.cart) + + # Assert + self.assertAlmostEqual(horiz, actual.lh_v1_horiz_angle) + self.assertAlmostEqual(vert, actual.lh_v1_vert_angle) + + def test_cartesian_is_normalized(self): + # Fixture + horiz = 0.123 + vert = 0.456 + vector = LighthouseBsVector(horiz, vert) + + # Test + actual = np.linalg.norm(vector.cart) + + # Assert + self.assertAlmostEqual(1.0, actual) From 680e7241e536568b2481fcbeda0866e23fa966f8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 10 Feb 2021 14:50:07 +0100 Subject: [PATCH 116/861] #192 Fixed imports --- cflib/localization/lighthouse_bs_vector.py | 1 + test/localization/test_lighthouse_bs_vector.py | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py index f0bd00d01..c57bbcb0c 100644 --- a/cflib/localization/lighthouse_bs_vector.py +++ b/cflib/localization/lighthouse_bs_vector.py @@ -20,6 +20,7 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import math + import numpy as np diff --git a/test/localization/test_lighthouse_bs_vector.py b/test/localization/test_lighthouse_bs_vector.py index a55705e16..ff24f9684 100644 --- a/test/localization/test_lighthouse_bs_vector.py +++ b/test/localization/test_lighthouse_bs_vector.py @@ -19,9 +19,8 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . - import unittest -import math + import numpy as np from cflib.localization.lighthouse_bs_vector import LighthouseBsVector From 0aad9af9007413b9d23cef4c5903ed6668a1d3d4 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 Feb 2021 18:00:25 +0100 Subject: [PATCH 117/861] #192 Added functionality to write geometry data for multiple lighthouse base stations --- cflib/crazyflie/mem/lighthouse_memory.py | 56 +++++++++++++++++++++++- 1 file changed, 55 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index b3a3e96a1..4877e10e4 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -282,7 +282,14 @@ class LighthouseMemHelper: def __init__(self, cf): self._cf = cf + self._result_geos = None + self._next_geo_get_id = None + self._read_geos_done_cb = None + + self._geos_to_write = None + self._write_done_cb = None + self._write_failed_for_one_or_more_geos = False mems = self._cf.mem.get_mems(MemoryElement.TYPE_LH) count = len(mems) @@ -292,17 +299,46 @@ def __init__(self, cf): self._lh_mem = mems[0] def read_all_geos(self, read_done_cb): + if self._read_geos_done_cb is not None: + raise Exception('Read operation not finished') + self._result_geos = {} self._next_geo_get_id = 0 self._read_geos_done_cb = read_done_cb self._get_geo(0) + def write_geos(self, geometry_dict, write_done_cb): + if self._geos_to_write is not None: + raise Exception('Write operation not finished') + + self._write_done_cb = write_done_cb + # Make a copy of the dictionary + self._geos_to_write = dict(geometry_dict) + self._write_failed_for_one_or_more_geos = False + self._write_next_geo() + + def _write_next_geo(self): + if len(self._geos_to_write) > 0: + id = list(self._geos_to_write.keys())[0] + geo_data = self._geos_to_write.pop(id) + self._lh_mem.write_geo_data(id, geo_data, self._geo_data_written, write_failed_cb=self._write_failed) + else: + tmp_cb = self._write_done_cb + is_sucess = not self._write_failed_for_one_or_more_geos + + self._geos_to_write = None + self._write_done_cb = None + + tmp_cb(is_sucess) + def _geo_data_updated(self, mem, geo_data): self._result_geos[self._next_geo_get_id] = geo_data self._next_geo_get_id += 1 self._get_geo(self._next_geo_get_id) def _update_failed(self, mem): + # Update failes if the geo is not available, that is if we try to read a base station id that is + # not supported by the firmware. Try to read the next one until we're done. self._next_geo_get_id += 1 self._get_geo(self._next_geo_get_id) @@ -310,4 +346,22 @@ def _get_geo(self, channel): if channel < self.NR_OF_CHANNELS: self._lh_mem.read_geo_data(channel, self._geo_data_updated, update_failed_cb=self._update_failed) else: - self._read_geos_done_cb(self._result_geos) + tmp_cb = self._read_geos_done_cb + tmp_result = self._result_geos + + self._read_geos_done_cb = None + self._result_geos = None + self._next_geo_get_id = None + self._write_failed_for_one_or_more_geos = False + + tmp_cb(tmp_result) + + def _geo_data_written(self, mem, addr): + self._write_next_geo() + + def _write_failed(self, mem, addr): + # Write failes if we try to write data for a base station that is not supported by the fw. + # Try to write the next one until we have tried them all, but record the problem and + # report that not all base stations were written. + self._write_failed_for_one_or_more_geos = True + self._write_next_geo() From 49f6db8d3b88375a5c9ee2db5708ea9f4b2231d0 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 12 Feb 2021 12:41:53 +0100 Subject: [PATCH 118/861] #192 Use headless version of open cv to avoid clash with qt --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index b959f6bf1..590e6eea0 100644 --- a/setup.py +++ b/setup.py @@ -26,6 +26,6 @@ install_requires=[ 'pyusb>=1.0.0b2', - 'opencv-python==4.5.1.48' + 'opencv-python-headless==4.5.1.48' ] ) From fd05a08439184dd58c4995bdbece80b5cce8d87d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 17 Feb 2021 17:01:27 +0100 Subject: [PATCH 119/861] #192 Correction in lighthouse geometry writer for multiple base stations. Clear callback before calling it to enable registration of new callback from called code --- cflib/crazyflie/mem/lighthouse_memory.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index 4877e10e4..a74057c50 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -251,16 +251,18 @@ def _write_data_list(self, addr, data_list): def write_done(self, mem, addr): if mem.id == self.id: - if self._write_finished_cb: - self._write_finished_cb(self, addr) + tmp_cb = self._write_finished_cb self._clear_write_cb() + if tmp_cb: + tmp_cb(self, addr) def write_failed(self, mem, addr): if mem.id == self.id: - if self._write_failed_cb: - logger.debug('Write of data failed') - self._write_failed_cb(self, addr) + tmp_cb = self._write_failed_cb self._clear_write_cb() + if tmp_cb: + logger.debug('Write of data failed') + tmp_cb(self, addr) def disconnect(self): self._clear_update_cb() @@ -352,7 +354,6 @@ def _get_geo(self, channel): self._read_geos_done_cb = None self._result_geos = None self._next_geo_get_id = None - self._write_failed_for_one_or_more_geos = False tmp_cb(tmp_result) From 881c839592e40b8013a5cd029961c0d51fcc8587 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 18 Feb 2021 08:24:34 +0100 Subject: [PATCH 120/861] #194 Added uid to lighthouse calibration data --- cflib/crazyflie/mem/lighthouse_memory.py | 9 ++++++--- examples/lighthouse/write_lighthouse_mem.py | 2 ++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index a74057c50..e95668574 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -100,13 +100,15 @@ class LighthouseBsCalibration: """Container for calibration data of one Lighthouse base station""" SIZE_FLOAT = 4 + SIZE_UINT_32 = 4 SIZE_BOOL = 1 SIZE_SWEEP = 7 * SIZE_FLOAT - SIZE_CALIBRATION = 2 * SIZE_SWEEP + SIZE_BOOL + SIZE_CALIBRATION = 2 * SIZE_SWEEP + SIZE_UINT_32 + SIZE_BOOL def __init__(self): self.sweeps = [LighthouseCalibrationSweep(), LighthouseCalibrationSweep()] + self.uid = 0 self.valid = True def set_from_mem_data(self, data): @@ -114,7 +116,7 @@ def set_from_mem_data(self, data): data[0:self.SIZE_SWEEP]) self.sweeps[1] = self._unpack_sweep_calibration( data[self.SIZE_SWEEP:self.SIZE_SWEEP * 2]) - self.valid = struct.unpack(' Date: Thu, 18 Feb 2021 09:59:40 +0100 Subject: [PATCH 121/861] Call callbacks after the lock has been released. Closes #193 --- cflib/crazyflie/mem/__init__.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 39fcb513b..24d2164da 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -505,13 +505,15 @@ def _new_packet_cb(self, packet): # Find the read request if id in self._write_requests: self._write_requests_lock.acquire() + do_call_sucess_cb = False + do_call_fail_cb = False wreq = self._write_requests[id][0] if status == 0: if wreq.write_done(addr): # self._write_requests.pop(id, None) # Remove the first item self._write_requests[id].pop(0) - self.mem_write_cb.call(wreq.mem, wreq.addr) + do_call_sucess_cb = True # Get a new one to start (if there are any) if len(self._write_requests[id]) > 0: @@ -521,7 +523,7 @@ def _new_packet_cb(self, packet): 'Status {}: write failed.'.format(status)) # Remove from queue self._write_requests[id].pop(0) - self.mem_write_failed_cb.call(wreq.mem, wreq.addr) + do_call_fail_cb = True # Get a new one to start (if there are any) if len(self._write_requests[id]) > 0: @@ -529,6 +531,13 @@ def _new_packet_cb(self, packet): self._write_requests_lock.release() + # Call callbacks after the lock has been released to alow for new writes + # to be initiated from the callback. + if do_call_sucess_cb: + self.mem_write_cb.call(wreq.mem, wreq.addr) + if do_call_fail_cb: + self.mem_write_failed_cb.call(wreq.mem, wreq.addr) + if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack(' Date: Thu, 18 Feb 2021 13:57:58 +0100 Subject: [PATCH 122/861] #192 Added support functionality for geometry estimation --- cflib/crazyflie/mem/__init__.py | 3 +- cflib/localization/__init__.py | 10 ++ .../lighthouse_sweep_angle_reader.py | 156 ++++++++++++++++++ test/localization/test_lighthouse_bs_geo.py | 4 +- .../localization/test_lighthouse_bs_vector.py | 2 +- 5 files changed, 171 insertions(+), 4 deletions(-) create mode 100644 cflib/localization/lighthouse_sweep_angle_reader.py diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 24d2164da..212755139 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -38,6 +38,7 @@ from .led_timings_driver_memory import LEDTimingsDriverMemory from .lighthouse_memory import LighthouseBsCalibration from .lighthouse_memory import LighthouseBsGeometry +from .lighthouse_memory import LighthouseMemHelper from .lighthouse_memory import LighthouseMemory from .loco_memory import LocoMemory from .loco_memory_2 import LocoMemory2 @@ -52,7 +53,7 @@ __author__ = 'Bitcraze AB' __all__ = ['Memory', 'Poly4D', 'MemoryElement', - 'LighthouseBsGeometry', 'LighthouseBsCalibration'] + 'LighthouseBsGeometry', 'LighthouseBsCalibration', 'LighthouseMemHelper'] # Channels used for the logging port CHAN_INFO = 0 diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py index d776760fa..fc644ffd5 100644 --- a/cflib/localization/__init__.py +++ b/cflib/localization/__init__.py @@ -21,3 +21,13 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. +from .lighthouse_bs_geo import LighthouseBsGeoEstimator +from .lighthouse_bs_vector import LighthouseBsVector +from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader +from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader + +__all__ = [ + 'LighthouseBsGeoEstimator', + 'LighthouseBsVector', + 'LighthouseSweepAngleAverageReader', + 'LighthouseSweepAngleReader', ] diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py new file mode 100644 index 000000000..90562067d --- /dev/null +++ b/cflib/localization/lighthouse_sweep_angle_reader.py @@ -0,0 +1,156 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . + +from cflib.localization import LighthouseBsVector + + +class LighthouseSweepAngleReader(): + """ + Wrapper to simplify reading of lighthouse sweep angles from the locSrv stream + """ + ANGLE_STREAM_PARAM = 'locSrv.enLhAngleStream' + NR_OF_SENSORS = 4 + + def __init__(self, cf, data_recevied_cb): + self._cf = cf + self._cb = data_recevied_cb + self._is_active = False + + def start(self): + """Start reading sweep angles""" + self._cf.loc.receivedLocationPacket.add_callback(self._packet_received_cb) + self._angle_stream_activate(True) + self._is_active = True + + def stop(self): + """Stop reading sweep angles""" + if self._is_active: + self._is_active = False + self._cf.loc.receivedLocationPacket.remove_callback(self._packet_received_cb) + self._angle_stream_activate(False) + + def _angle_stream_activate(self, is_active): + value = 0 + if is_active: + value = 1 + self._cf.param.set_value(self.ANGLE_STREAM_PARAM, value) + + def _packet_received_cb(self, packet): + if packet.type != self._cf.loc.LH_ANGLE_STREAM: + return + + if self._cb: + base_station_id = packet.data["basestation"] + horiz_angles = packet.data['x'] + vert_angles = packet.data['y'] + + result = [] + for i in range(self.NR_OF_SENSORS): + result.append(LighthouseBsVector(horiz_angles[i], vert_angles[i])) + + self._cb(base_station_id, result) + + +class LighthouseSweepAngleAverageReader(): + """ + Helper class to make it easy read sweep angles for multiple base stations and average the result + """ + def __init__(self, cf, ready_cb): + self._reader = LighthouseSweepAngleReader(cf, self._data_recevied_cb) + self._ready_cb = ready_cb + self.nr_of_samples_required = 50 + + # We store all samples in the storage for averaging when data is collected + # The storage is a dictionary keyed on the base station channel + # Each entry is a list of 4 lists, one per sensor. + # Each list contains LighthouseBsVector objects, representing the sampled sweep angles + self._sample_storage = None + + def start_angle_collection(self): + """ + Start collecting angles. The process will terminate when nr_of_samples_required have been + received + """ + self._sample_storage = {} + self._reader.start() + + def stop_angle_collection(self): + """Premature stop of data collection""" + self._reader.stop() + self._sample_storage = None + + def is_collecting(self): + """True if data collection is in progress""" + return self._sample_storage is not None + + def _data_recevied_cb(self, base_station_id, bs_vectors): + self._store_sample(base_station_id, bs_vectors, self._sample_storage) + if self._has_collected_enough_data(self._sample_storage): + self._reader.stop() + if self._ready_cb: + averages = self._average_all_lists(self._sample_storage) + self._ready_cb(averages) + self._sample_storage = None + + def _store_sample(self, base_station_id, bs_vectors, storage): + if base_station_id not in storage: + storage[base_station_id] = [] + for sensor in range(self._reader.NR_OF_SENSORS): + storage[base_station_id].append([]) + + for sensor in range(self._reader.NR_OF_SENSORS): + storage[base_station_id][sensor].append(bs_vectors[sensor]) + + def _has_collected_enough_data(self, storage): + for sample_list in storage.values(): + if len(sample_list[0]) >= self.nr_of_samples_required: + return True + return False + + def _average_all_lists(self, storage): + result = {} + + for id, sample_lists in storage.items(): + averages = self._average_sample_lists(sample_lists) + count = len(sample_lists[0]) + result[id] = (count, averages) + + return result + + def _average_sample_lists(self, sample_lists): + result = [] + + for i in range(self._reader.NR_OF_SENSORS): + result.append(self._average_sample_list(sample_lists[i])) + + return result + + def _average_sample_list(self, sample_list): + sum_horiz = 0.0 + sum_vert = 0.0 + + for bs_vector in sample_list: + sum_horiz += bs_vector.lh_v1_horiz_angle + sum_vert += bs_vector.lh_v1_vert_angle + + count = len(sample_list) + return LighthouseBsVector(sum_horiz / count, sum_vert / count) diff --git a/test/localization/test_lighthouse_bs_geo.py b/test/localization/test_lighthouse_bs_geo.py index 99ae190b2..2ea388b04 100644 --- a/test/localization/test_lighthouse_bs_geo.py +++ b/test/localization/test_lighthouse_bs_geo.py @@ -22,8 +22,8 @@ import math import unittest -from cflib.localization.lighthouse_bs_geo import LighthouseBsGeoEstimator -from cflib.localization.lighthouse_bs_vector import LighthouseBsVector +from cflib.localization import LighthouseBsGeoEstimator +from cflib.localization import LighthouseBsVector class TestLighthouseBsGeoEstimator(unittest.TestCase): diff --git a/test/localization/test_lighthouse_bs_vector.py b/test/localization/test_lighthouse_bs_vector.py index ff24f9684..27eb2080a 100644 --- a/test/localization/test_lighthouse_bs_vector.py +++ b/test/localization/test_lighthouse_bs_vector.py @@ -23,7 +23,7 @@ import numpy as np -from cflib.localization.lighthouse_bs_vector import LighthouseBsVector +from cflib.localization import LighthouseBsVector class TestLighthouseBsVector(unittest.TestCase): From ebfa126d9ee1a0f29d3dbcb3c7de165337de87dd Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 18 Feb 2021 20:14:07 +0100 Subject: [PATCH 123/861] #192 Added calibration support to LighthouseMemHelper. Re-wrote read_lighthouse_mem.py and write_lighthouse_mem.py to use the LighthouseMemHelper class --- cflib/crazyflie/mem/lighthouse_memory.py | 199 +++++++++++------- .../lighthouse_sweep_angle_reader.py | 4 +- examples/lighthouse/read_lighthouse_mem.py | 71 ++----- examples/lighthouse/write_lighthouse_mem.py | 68 ++---- 4 files changed, 171 insertions(+), 171 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index e95668574..aa6a331e9 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -281,91 +281,142 @@ def _clear_write_cb(self): class LighthouseMemHelper: - """Helper to access all geometry data located in crazyflie memory subsystem""" + """Helper to access all geometry and calibration data located in crazyflie memory subsystem""" NR_OF_CHANNELS = 16 - def __init__(self, cf): - self._cf = cf + class _ObjectReader: + """Internal class that reads all geos or calib objects""" + NR_OF_CHANNELS = 16 + + def __init__(self, read_fcn): + self._read_fcn = read_fcn + + self._result = None + self._next_id = None + self._read_done_cb = None + + def read_all(self, read_done_cb): + if self._read_done_cb is not None: + raise Exception('Read operation not finished') + + self._result = {} + self._next_id = 0 + self._read_done_cb = read_done_cb + self._get_object(0) + + def _data_updated(self, mem, data): + self._result[self._next_id] = data + self._next_id += 1 + self._get_object(self._next_id) - self._result_geos = None - self._next_geo_get_id = None - self._read_geos_done_cb = None + def _update_failed(self, mem): + # Update failes if the object is not available, that is if we try to read a base station id that is + # not supported by the firmware. Try to read the next one until we're done. + self._next_id += 1 + self._get_object(self._next_id) - self._geos_to_write = None - self._write_done_cb = None - self._write_failed_for_one_or_more_geos = False + def _get_object(self, channel): + if channel < self.NR_OF_CHANNELS: + self._read_fcn(channel, self._data_updated, update_failed_cb=self._update_failed) + else: + tmp_cb = self._read_done_cb + tmp_result = self._result + + self._read_done_cb = None + self._result = None + self._next_id = None + + tmp_cb(tmp_result) - mems = self._cf.mem.get_mems(MemoryElement.TYPE_LH) + class _ObjectWriter: + """Internal class that writes all geos or calib objects""" + + def __init__(self, write_fcn): + self._objects_to_write = None + self._write_done_cb = None + self._write_failed_for_one_or_more_objects = False + self._write_fcn = write_fcn + + def write(self, object_dict, write_done_cb): + if self._objects_to_write is not None: + raise Exception('Write operation not finished') + + self._write_done_cb = write_done_cb + # Make a copy of the dictionary + self._objects_to_write = dict(object_dict) + self._write_failed_for_one_or_more_objects = False + self._write_next_object() + + def _write_next_object(self): + if len(self._objects_to_write) > 0: + id = list(self._objects_to_write.keys())[0] + data = self._objects_to_write.pop(id) + self._write_fcn(id, data, self._data_written, write_failed_cb=self._write_failed) + else: + tmp_cb = self._write_done_cb + is_sucess = not self._write_failed_for_one_or_more_objects + + self._objects_to_write = None + self._write_done_cb = None + self._write_failed_for_one_or_more_objects = False + + tmp_cb(is_sucess) + + def _data_written(self, mem, addr): + self._write_next_object() + + def _write_failed(self, mem, addr): + # Write failes if we try to write data for a base station that is not supported by the fw. + # Try to write the next one until we have tried them all, but record the problem and + # report that not all base stations were written. + self._write_failed_for_one_or_more_objects = True + self._write_next_object() + + def __init__(self, cf): + mems = cf.mem.get_mems(MemoryElement.TYPE_LH) count = len(mems) if count != 1: raise Exception('Unexpected nr of memories found:', count) - self._lh_mem = mems[0] + lh_mem = mems[0] - def read_all_geos(self, read_done_cb): - if self._read_geos_done_cb is not None: - raise Exception('Read operation not finished') + self.geo_reader = self._ObjectReader(lh_mem.read_geo_data) + self.geo_writer = self._ObjectWriter(lh_mem.write_geo_data) - self._result_geos = {} - self._next_geo_get_id = 0 - self._read_geos_done_cb = read_done_cb - self._get_geo(0) + self.calib_reader = self._ObjectReader(lh_mem.read_calib_data) + self.calib_writer = self._ObjectWriter(lh_mem.write_calib_data) - def write_geos(self, geometry_dict, write_done_cb): - if self._geos_to_write is not None: - raise Exception('Write operation not finished') - - self._write_done_cb = write_done_cb - # Make a copy of the dictionary - self._geos_to_write = dict(geometry_dict) - self._write_failed_for_one_or_more_geos = False - self._write_next_geo() - - def _write_next_geo(self): - if len(self._geos_to_write) > 0: - id = list(self._geos_to_write.keys())[0] - geo_data = self._geos_to_write.pop(id) - self._lh_mem.write_geo_data(id, geo_data, self._geo_data_written, write_failed_cb=self._write_failed) - else: - tmp_cb = self._write_done_cb - is_sucess = not self._write_failed_for_one_or_more_geos - - self._geos_to_write = None - self._write_done_cb = None + def read_all_geos(self, read_done_cb): + """ + Read geometry data for all base stations. The result is returned + as a dictionary keyed on base station channel (0-indexed) with + geometry data as values + """ + self.geo_reader.read_all(read_done_cb) - tmp_cb(is_sucess) - - def _geo_data_updated(self, mem, geo_data): - self._result_geos[self._next_geo_get_id] = geo_data - self._next_geo_get_id += 1 - self._get_geo(self._next_geo_get_id) - - def _update_failed(self, mem): - # Update failes if the geo is not available, that is if we try to read a base station id that is - # not supported by the firmware. Try to read the next one until we're done. - self._next_geo_get_id += 1 - self._get_geo(self._next_geo_get_id) - - def _get_geo(self, channel): - if channel < self.NR_OF_CHANNELS: - self._lh_mem.read_geo_data(channel, self._geo_data_updated, update_failed_cb=self._update_failed) - else: - tmp_cb = self._read_geos_done_cb - tmp_result = self._result_geos - - self._read_geos_done_cb = None - self._result_geos = None - self._next_geo_get_id = None - - tmp_cb(tmp_result) - - def _geo_data_written(self, mem, addr): - self._write_next_geo() - - def _write_failed(self, mem, addr): - # Write failes if we try to write data for a base station that is not supported by the fw. - # Try to write the next one until we have tried them all, but record the problem and - # report that not all base stations were written. - self._write_failed_for_one_or_more_geos = True - self._write_next_geo() + def write_geos(self, geometry_dict, write_done_cb): + """ + Write geometry data for one or more base stations. Input is + a dictionary keyed on base station channel (0-indexed) with + geometry data as values. The callback is called with a boolean + indicating if all items were successfully written. + """ + self.geo_writer.write(geometry_dict, write_done_cb) + + def read_all_calibs(self, read_done_cb): + """ + Read calibration data for all base stations. The result is returned + as a dictionary keyed on base station channel (0-indexed) with + calibration data as values + """ + self.calib_reader.read_all(read_done_cb) + + def write_calibs(self, calibration_dict, write_done_cb): + """ + Write calibration data for one or more base stations. Input is + a dictionary keyed on base station channel (0-indexed) with + calibration data as values. The callback is called with a boolean + indicating if all items were successfully written. + """ + self.calib_writer.write(calibration_dict, write_done_cb) diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py index 90562067d..a2e47a891 100644 --- a/cflib/localization/lighthouse_sweep_angle_reader.py +++ b/cflib/localization/lighthouse_sweep_angle_reader.py @@ -19,7 +19,6 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . - from cflib.localization import LighthouseBsVector @@ -59,7 +58,7 @@ def _packet_received_cb(self, packet): return if self._cb: - base_station_id = packet.data["basestation"] + base_station_id = packet.data['basestation'] horiz_angles = packet.data['x'] vert_angles = packet.data['y'] @@ -74,6 +73,7 @@ class LighthouseSweepAngleAverageReader(): """ Helper class to make it easy read sweep angles for multiple base stations and average the result """ + def __init__(self, cf, ready_cb): self._reader = LighthouseSweepAngleReader(cf, self._data_recevied_cb) self._ready_cb = ready_cb diff --git a/examples/lighthouse/read_lighthouse_mem.py b/examples/lighthouse/read_lighthouse_mem.py index 185b096d5..20bd09b7d 100644 --- a/examples/lighthouse/read_lighthouse_mem.py +++ b/examples/lighthouse/read_lighthouse_mem.py @@ -26,72 +26,45 @@ calibration memory from a Crazyflie """ import logging -import time +from threading import Event import cflib.crtp # noqa from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.mem import LighthouseMemHelper from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -# Only output errors from the logging framework +# Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) class ReadMem: def __init__(self, uri): - self.got_data = False + self._event = Event() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH) - - count = len(mems) - if count != 1: - raise Exception('Unexpected nr of memories found:', count) - - lh_mem = mems[0] - print('Requesting data') - print('-- Geo 0') - self.got_data = False - lh_mem.read_geo_data(0, self._geo_data_updated, - update_failed_cb=self._update_failed) - - while not self.got_data: - time.sleep(1) - - print('-- Geo 1') - self.got_data = False - lh_mem.read_geo_data(1, self._geo_data_updated, - update_failed_cb=self._update_failed) - - while not self.got_data: - time.sleep(1) - - print('-- Calibration 0') - self.got_data = False - lh_mem.read_calib_data(0, self._calib_data_updated, - update_failed_cb=self._update_failed) - - while not self.got_data: - time.sleep(1) + helper = LighthouseMemHelper(scf.cf) - print('-- Calibration 1') - self.got_data = False - lh_mem.read_calib_data(1, self._calib_data_updated, - update_failed_cb=self._update_failed) + helper.read_all_geos(self._geo_read_ready) + self._event.wait() - while not self.got_data: - time.sleep(1) + self._event.clear() - def _geo_data_updated(self, mem, geo_data): - geo_data.dump() - self.got_data = True + helper.read_all_calibs(self._calib_read_ready) + self._event.wait() - def _calib_data_updated(self, mem, calib_data): - calib_data.dump() - self.got_data = True + 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 _update_failed(self, mem): - raise Exception('Read failed') + 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__': diff --git a/examples/lighthouse/write_lighthouse_mem.py b/examples/lighthouse/write_lighthouse_mem.py index 87ff18ae4..b0fc5d12d 100644 --- a/examples/lighthouse/write_lighthouse_mem.py +++ b/examples/lighthouse/write_lighthouse_mem.py @@ -26,69 +26,41 @@ and calibration memory in a Crazyflie """ import logging -import time +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 MemoryElement +from cflib.crazyflie.mem import LighthouseMemHelper from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -# Only output errors from the logging framework +# Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) class WriteMem: - def __init__(self, uri, bs1geo, bs2geo, bs1calib, bs2calib): - self.data_written = False + def __init__(self, uri, geo_dict, calib_dict): + self._event = Event() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH) - - count = len(mems) - if count != 1: - raise Exception('Unexpected nr of memories found:', count) - - lh_mem = mems[0] - - print('Writing data') - lh_mem.write_geo_data(0, bs1geo, self._data_written, - write_failed_cb=self._data_failed) + helper = LighthouseMemHelper(scf.cf) - while not self.data_written: - time.sleep(1) + helper.write_geos(geo_dict, self._data_written) + self._event.wait() - self.data_written = False - lh_mem.write_geo_data(1, bs2geo, self._data_written, - write_failed_cb=self._data_failed) + self._event.clear() - while not self.data_written: - time.sleep(1) + helper.write_calibs(calib_dict, self._data_written) + self._event.wait() - self.data_written = False - lh_mem.write_calib_data( - 0, bs1calib, self._data_written, - write_failed_cb=self._data_failed) + def _data_written(self, success): + if success: + print('Data written') + else: + print('Write failed') - while not self.data_written: - time.sleep(1) - - self.data_written = False - lh_mem.write_calib_data( - 1, bs2calib, self._data_written, - write_failed_cb=self._data_failed) - - while not self.data_written: - time.sleep(1) - - def _data_written(self, mem, addr): - self.data_written = True - print('Data written') - - def _data_failed(self, mem, addr): - print('Write failed') - raise Exception() + self._event.set() if __name__ == '__main__': @@ -152,4 +124,8 @@ def _data_failed(self, mem, addr): bs2calib.uid = 9876 bs2calib.valid = True - WriteMem(uri, bs1geo, bs2geo, bs1calib, bs2calib) + # 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) From 76c71cafc8be0617ff71ffbbdfeba075425f6c7a Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Feb 2021 11:10:06 +0100 Subject: [PATCH 124/861] Fix argument name of RadioDriver.receive_packet The original definition is that function is to have an argument called wait, not time, see https://github.com/bitcraze/crazyflie-lib-python/blob/master/cflib/crtp/crtpdriver.py#L59 Part of issue #196. --- cflib/crtp/radiodriver.py | 8 ++++---- sys_test/swarm_test_rig/test_response_time.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index d28b342ed..fe6c97838 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -334,24 +334,24 @@ def parse_uri(uri): return devid, channel, datarate, address - def receive_packet(self, time=0): + def receive_packet(self, wait=0): """ Receive a packet though the link. This call is blocking but will timeout and return None if a timeout is supplied. """ - if time == 0: + if wait == 0: try: return self.in_queue.get(False) except queue.Empty: return None - elif time < 0: + elif wait < 0: try: return self.in_queue.get(True) except queue.Empty: return None else: try: - return self.in_queue.get(True, time) + return self.in_queue.get(True, wait) except queue.Empty: return None diff --git a/sys_test/swarm_test_rig/test_response_time.py b/sys_test/swarm_test_rig/test_response_time.py index 159e2c14f..f00378464 100644 --- a/sys_test/swarm_test_rig/test_response_time.py +++ b/sys_test/swarm_test_rig/test_response_time.py @@ -108,7 +108,7 @@ def assert_wait_for_all_seq_nrs(self, links, seq_nr, timeout=1): while time.time() < time_end: for link in links: if link.uri not in response_timestamps: - response = link.receive_packet(time=NO_BLOCKING) + response = link.receive_packet(wait=NO_BLOCKING) if self._is_response_correct_seq_nr(response, seq_nr): response_timestamps[link.uri] = time.time() From d68d420eff9cced401be0a7885a865ebbdd8d86b Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Feb 2021 11:17:28 +0100 Subject: [PATCH 125/861] Bootloader: more explicit about address and safelink requirement. Notes: * The current radiodriver is not enforcing safelink, but rather lazily initializing it. This change does not change the behavior and the [noSafelink] hint in the URI is simply ignored. * The current scan_selected function will only consider channel and datarate. While more can be passed in (e.g., address), this is ignored. Part of issue #196. --- cflib/bootloader/cloader.py | 15 ++++++++------- cflib/crtp/radiodriver.py | 6 +++--- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index cdea76eaf..54c529aa7 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -72,7 +72,7 @@ def __init__(self, link, info_cb=None, in_boot_cb=None): self.targets = {} self.mapping = None - self._available_boot_uri = ('radio://0/110/2M', 'radio://0/0/2M') + self._available_boot_uri = ('radio://0/110/2M/E7E7E7E7E7', 'radio://0/0/2M/E7E7E7E7E7') def close(self): """ Close the link """ @@ -80,7 +80,7 @@ def close(self): self.link.close() def scan_for_bootloader(self): - link = cflib.crtp.get_link_driver('radio://0') + link = cflib.crtp.get_link_driver('radio://0/80/2M/E7E7E7E7E7') ts = time.time() res = () while len(res) == 0 and (time.time() - ts) < 10: @@ -128,7 +128,7 @@ def reset_to_bootloader(self, target_id): self.link.close() time.sleep(0.2) self.link = cflib.crtp.get_link_driver( - 'radio://0/0/2M/{:X}'.format(addr)) + 'radio://0/0/2M/{:X}[noSafelink]'.format(addr)) return True else: @@ -224,9 +224,10 @@ def open_bootloader_uri(self, uri=None): if self.link: self.link.close() if uri: - self.link = cflib.crtp.get_link_driver(uri) + self.link = cflib.crtp.get_link_driver(uri + "[noSafelink]") else: - self.link = cflib.crtp.get_link_driver(self.clink_address) + self.link = cflib.crtp.get_link_driver( + self.clink_address + "[noSafelink]") def check_link_and_get_info(self, target_id=0xFF): """Try to get a connection with the bootloader by requesting info @@ -296,7 +297,7 @@ def _update_mapping(self, target_id): pk = self.link.receive_packet(2) - if (pk and pk.header == 0xFF and struct.unpack('=2 and struct.unpack('= 0): pk = CRTPPacket() diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index d28b342ed..d9d39a90b 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -298,11 +298,11 @@ def parse_uri(uri): # Open the USB dongle if not re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))' - '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri): + '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\[noSafelink\])?$', uri): raise WrongUriType('Wrong radio URI format!') uri_data = re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))' - '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri) + '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\[noSafelink\])?$', uri) if len(uri_data.group(1)) < 10 and uri_data.group(1).isdigit(): devid = int(uri_data.group(1)) @@ -406,7 +406,7 @@ def scan_selected(self, links): for link in links: one_to_scan = {} uri_data = re.search('^radio://([0-9]+)((/([0-9]+))' - '(/(250K|1M|2M))?)?$', + '(/(250K|1M|2M))?)?', link) one_to_scan['channel'] = int(uri_data.group(4)) From da7fb5a1325f21bcc2db5f40d426375b526f88b0 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Feb 2021 11:49:47 +0100 Subject: [PATCH 126/861] Fix flake8 --- cflib/bootloader/cloader.py | 6 +++--- cflib/crtp/radiodriver.py | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 54c529aa7..79284c5e6 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -224,10 +224,10 @@ def open_bootloader_uri(self, uri=None): if self.link: self.link.close() if uri: - self.link = cflib.crtp.get_link_driver(uri + "[noSafelink]") + self.link = cflib.crtp.get_link_driver(uri + '[noSafelink]') else: self.link = cflib.crtp.get_link_driver( - self.clink_address + "[noSafelink]") + self.clink_address + '[noSafelink]') def check_link_and_get_info(self, target_id=0xFF): """Try to get a connection with the bootloader by requesting info @@ -297,7 +297,7 @@ def _update_mapping(self, target_id): pk = self.link.receive_packet(2) - if (pk and pk.header == 0xFF and len(pk.data) >=2 and struct.unpack('= 2 and struct.unpack(' Date: Fri, 19 Feb 2021 12:00:19 +0100 Subject: [PATCH 127/861] Add CfLinkCppDriver This backend is an alternative to both RadioDriver and UsbDriver and uses cflinkcpp, a C++ library with Python bindings. It can be enabled by setting the environment variable USE_CFLINK=cpp. The default is to use the existing pure Python implementation. Part of issue #196. --- cflib/crtp/__init__.py | 30 +++--- cflib/crtp/cflinkcppdriver.py | 197 ++++++++++++++++++++++++++++++++++ cflib/utils/power_switch.py | 64 +++++++---- 3 files changed, 255 insertions(+), 36 deletions(-) create mode 100644 cflib/crtp/cflinkcppdriver.py diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 0e509c365..f45ee9f1d 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -26,6 +26,7 @@ # MA 02110-1301, USA. """Scans and creates communication interfaces.""" import logging +import os from .debugdriver import DebugDriver from .exceptions import WrongUriType @@ -41,25 +42,26 @@ logger = logging.getLogger(__name__) -DRIVERS = [RadioDriver, SerialDriver, UdpDriver, - DebugDriver, UsbDriver, PrrtDriver] CLASSES = [] def init_drivers(enable_debug_driver=False, enable_serial_driver=False): """Initialize all the drivers.""" - for driver in DRIVERS: - try: - enable = True - if driver == DebugDriver: - enable = enable_debug_driver - elif driver == SerialDriver: - enable = enable_serial_driver - - if enable: - CLASSES.append(driver) - except Exception: # pylint: disable=W0703 - continue + + env = os.getenv('USE_CFLINK') + if env is not None and env == 'cpp': + from .cflinkcppdriver import CfLinkCppDriver + CLASSES.append(CfLinkCppDriver) + else: + CLASSES.extend([RadioDriver, UsbDriver]) + + if enable_debug_driver: + CLASSES.append(DebugDriver) + + if enable_serial_driver: + CLASSES.append(SerialDriver) + + CLASSES.extend([UdpDriver, PrrtDriver]) def scan_interfaces(address=None): diff --git a/cflib/crtp/cflinkcppdriver.py b/cflib/crtp/cflinkcppdriver.py new file mode 100644 index 000000000..d4b5e6ada --- /dev/null +++ b/cflib/crtp/cflinkcppdriver.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2011-2021 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +""" +Crazyflie driver using the cflinkcpp implementation. + +This driver is used to communicate over the radio or USB. +""" +import threading +import logging +import os + +from .crtpstack import CRTPPacket +from cflib.crtp.crtpdriver import CRTPDriver +import cflinkcpp + +__author__ = 'Bitcraze AB' +__all__ = ['CfLinkCppDriver'] + +logger = logging.getLogger(__name__) + + +class CfLinkCppDriver(CRTPDriver): + """ cflinkcpp driver """ + + def __init__(self): + """Driver constructor. Throw an exception if the driver is unable to + open the URI + """ + self.uri = '' + + # cflinkcpp resends packets internally + self.needs_resending = False + + self._connection = None + + def connect(self, uri, link_quality_callback, link_error_callback): + """Connect the driver to a specified URI + + @param uri Uri of the link to open + @param link_quality_callback Callback to report link quality in percent + @param link_error_callback Callback to report errors (will result in + disconnection) + """ + + self._connection = cflinkcpp.Connection(uri) + self.uri = uri + self._link_quality_callback = link_quality_callback + self._link_error_callback = link_error_callback + if link_quality_callback is not None or link_error_callback is not None: + self._last_connection_stats = self._connection.statistics + self._recompute_link_quality_timer() + + def send_packet(self, pk): + """Send a CRTP packet""" + nativePk = cflinkcpp.Packet() + nativePk.port = pk.port + nativePk.channel = pk.channel + nativePk.payload = bytes(pk.data) + try: + self._connection.send(nativePk) + except Exception as e: + if self._link_error_callback is not None: + import traceback + self._link_error_callback( + 'Error communicating! Perhaps your device has been unplugged?\n' + 'Exception:{}\n\n{}'.format(e, traceback.format_exc())) + + + def receive_packet(self, wait=0): + """Receive a CRTP packet. + + @param wait The time to wait for a packet in second. -1 means forever + + @return One CRTP packet or None if no packet has been received. + """ + if wait < 0: + # Since we block in the native lib, break up infinity timeouts into smaller + # pieces, so Ctrl+C keeps working as expected + timeout = 100 + elif wait == 0: + timeout = 1 + else: + timeout = int(wait*1000) + + try: + while True: + nativePk = self._connection.recv(timeout=timeout) + if wait>=0 or nativePk.valid: + break + + if not nativePk.valid: + return None + + pk = CRTPPacket() + pk.port = nativePk.port + pk.channel = nativePk.channel + pk.data = nativePk.payload + return pk + except Exception as e: + if self._link_error_callback is not None: + import traceback + self._link_error_callback( + 'Error communicating! Perhaps your device has been unplugged?\n' + 'Exception:{}\n\n{}'.format(e, traceback.format_exc())) + + def get_status(self): + """ + Return a status string from the interface. + """ + "okay" + + def get_name(self): + """ + Return a human readable name of the interface. + """ + "cflinkcpp" + + def scan_interface(self, address=None): + """ + Scan interface for available Crazyflie quadcopters and return a list + with them. + """ + if address: + uris = cflinkcpp.Connection.scan(address) + else: + uris = cflinkcpp.Connection.scan() + # convert to list of tuples, where the second part is a comment + result = [(uri, '') for uri in uris] + return result + + def scan_selected(self, uris): + """ + Scan interface for available Crazyflie quadcopters and return a list + with them. + """ + return cflinkcpp.Connection.scan_selected(uris) + + def enum(self): + """Enumerate, and return a list, of the available link URI on this + system + """ + return self.scan_interface() + + def get_help(self): + """return the help message on how to form the URI for this driver + None means no help + """ + "" + + def close(self): + """Close the link""" + self._connection.close() + self._connection = None + + def _recompute_link_quality_timer(self): + if self._connection is None: + return + stats = self._connection.statistics + sent_count = stats.sent_count - self._last_connection_stats.sent_count + ack_count = stats.ack_count - self._last_connection_stats.ack_count + if sent_count > 0: + link_quality = min(ack_count, sent_count) / sent_count * 100.0 + else: + link_quality = 1 + self._last_connection_stats = stats + + if self._link_quality_callback is not None: + self._link_quality_callback(link_quality) + + if sent_count > 10 and ack_count == 0 and self._link_error_callback is not None: + self._link_error_callback('Too many packets lost') + + threading.Timer(1.0, self._recompute_link_quality_timer).start() diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index 76ad891c5..e62d4637b 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -26,6 +26,7 @@ import time import cflib.crtp +from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.radiodriver import RadioManager @@ -36,11 +37,15 @@ class PowerSwitch: def __init__(self, uri): self.uri = uri - uri_parts = cflib.crtp.RadioDriver.parse_uri(uri) - self.devid = uri_parts[0] - self.channel = uri_parts[1] - self.datarate = uri_parts[2] - self.address = uri_parts[3] + uri_augmented = uri+'[noSafelink][noAutoPing][noAckFilter]' + self.link = cflib.crtp.get_link_driver(uri_augmented) + # Switch to legacy mode, if uri options are not available + if not self.link: + uri_parts = cflib.crtp.RadioDriver.parse_uri(uri) + self.devid = uri_parts[0] + self.channel = uri_parts[1] + self.datarate = uri_parts[2] + self.address = uri_parts[3] def platform_power_down(self): """ Power down the platform, both NRF and STM MCUs. @@ -64,26 +69,41 @@ def stm_power_cycle(self): time.sleep(1) self.stm_power_up() + def close(self): + if self.link: + self.link.close() + def _send(self, cmd): - packet = [0xf3, 0xfe, cmd] + if not self.link: + packet = [0xf3, 0xfe, cmd] + + cr = RadioManager.open(devid=self.devid) + cr.set_channel(self.channel) + cr.set_data_rate(self.datarate) + cr.set_address(self.address) + cr.set_arc(3) - cr = RadioManager.open(devid=self.devid) - cr.set_channel(self.channel) - cr.set_data_rate(self.datarate) - cr.set_address(self.address) - cr.set_arc(3) + success = False + for i in range(50): + res = cr.send_packet(packet) + if res and res.ack: + success = True + break - success = False - for i in range(50): - res = cr.send_packet(packet) - if res and res.ack: - success = True - break + time.sleep(0.01) - time.sleep(0.01) + cr.close() - cr.close() + if not success: + raise Exception( + 'Failed to connect to Crazyflie at {}'.format(self.uri)) + else: - if not success: - raise Exception( - 'Failed to connect to Crazyflie at {}'.format(self.uri)) + # send command (will be repeated until acked) + pk = CRTPPacket(0xFF, [0xfe, cmd]) + self.link.send_packet(pk) + # wait up to 1s + pk = self.link.receive_packet(0.1) + if pk is None: + raise Exception( + 'Failed to connect to Crazyflie at {}'.format(self.uri)) From 19a68308c881905068e288795ed82fc418008d83 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Feb 2021 12:09:31 +0100 Subject: [PATCH 128/861] Fix CI issues --- cflib/crtp/cflinkcppdriver.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/cflib/crtp/cflinkcppdriver.py b/cflib/crtp/cflinkcppdriver.py index d4b5e6ada..756278a26 100644 --- a/cflib/crtp/cflinkcppdriver.py +++ b/cflib/crtp/cflinkcppdriver.py @@ -29,13 +29,13 @@ This driver is used to communicate over the radio or USB. """ -import threading import logging -import os +import threading + +import cflinkcpp from .crtpstack import CRTPPacket from cflib.crtp.crtpdriver import CRTPDriver -import cflinkcpp __author__ = 'Bitcraze AB' __all__ = ['CfLinkCppDriver'] @@ -89,7 +89,6 @@ def send_packet(self, pk): 'Error communicating! Perhaps your device has been unplugged?\n' 'Exception:{}\n\n{}'.format(e, traceback.format_exc())) - def receive_packet(self, wait=0): """Receive a CRTP packet. @@ -109,7 +108,7 @@ def receive_packet(self, wait=0): try: while True: nativePk = self._connection.recv(timeout=timeout) - if wait>=0 or nativePk.valid: + if wait >= 0 or nativePk.valid: break if not nativePk.valid: @@ -126,18 +125,18 @@ def receive_packet(self, wait=0): self._link_error_callback( 'Error communicating! Perhaps your device has been unplugged?\n' 'Exception:{}\n\n{}'.format(e, traceback.format_exc())) - + def get_status(self): """ Return a status string from the interface. """ - "okay" + 'okay' def get_name(self): """ Return a human readable name of the interface. """ - "cflinkcpp" + 'cflinkcpp' def scan_interface(self, address=None): """ @@ -169,7 +168,7 @@ def get_help(self): """return the help message on how to form the URI for this driver None means no help """ - "" + '' def close(self): """Close the link""" From 14f39db3400f4c0918a97e1d4d2c5fb35f52589e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 21 Feb 2021 08:40:38 +0100 Subject: [PATCH 129/861] #192 Added file management for lighthouse system configuration files --- cflib/crazyflie/mem/lighthouse_memory.py | 73 +++++++++ cflib/localization/__init__.py | 4 +- .../localization/lighthouse_config_manager.py | 91 ++++++++++++ test/crazyflie/mem/__init__.py | 0 test/crazyflie/mem/test_lighthouse_memory.py | 109 ++++++++++++++ test/localization/fixtures/system_config.yaml | 68 +++++++++ .../test_lighthouse_config_manager.py | 139 ++++++++++++++++++ 7 files changed, 483 insertions(+), 1 deletion(-) create mode 100644 cflib/localization/lighthouse_config_manager.py create mode 100644 test/crazyflie/mem/__init__.py create mode 100644 test/crazyflie/mem/test_lighthouse_memory.py create mode 100644 test/localization/fixtures/system_config.yaml create mode 100644 test/localization/test_lighthouse_config_manager.py diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index aa6a331e9..247eb2818 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -35,6 +35,11 @@ class LighthouseBsGeometry: SIZE_VECTOR = 3 * SIZE_FLOAT SIZE_GEOMETRY = (1 + 3) * SIZE_VECTOR + SIZE_BOOL + FILE_ID_ORIGIN = 'origin' + FILE_ID_ROTATION = 'rotation' + + yaml_tag = 'LighthouseBsGeometry' + def __init__(self): self.origin = [0.0, 0.0, 0.0] self.rotation_matrix = [ @@ -68,6 +73,20 @@ def _read_vector(self, data): x, y, z = struct.unpack('. +""" +Functionality to manage lighthouse system configuration (geometry and calibration data). +""" + +import yaml +from cflib.crazyflie.mem import LighthouseBsCalibration +from cflib.crazyflie.mem import LighthouseBsGeometry + + +class LighthouseConfigFileManager: + TYPE_ID = 'type' + TYPE = 'lighthouse_system_configuration' + VERSION_ID = 'version' + VERSION = '1' + GEOS_ID = 'geos' + CALIBS_ID = 'calibs' + + @staticmethod + def write(file_name, geos={}, calibs={}): + file = open(file_name, 'w') + with file: + file_geos = {} + for id, geo in geos.items(): + if geo.valid: + file_geos[id] = geo.as_file_object() + + file_calibs = {} + for id, calib in calibs.items(): + if calib.valid: + file_calibs[id] = calib.as_file_object() + + data = { + LighthouseConfigFileManager.TYPE_ID: LighthouseConfigFileManager.TYPE, + LighthouseConfigFileManager.VERSION_ID: LighthouseConfigFileManager.VERSION, + LighthouseConfigFileManager.GEOS_ID: file_geos, + LighthouseConfigFileManager.CALIBS_ID: file_calibs + } + + yaml.dump(data, file) + + @staticmethod + def read(file_name): + file = open(file_name, 'r') + with file: + data = yaml.safe_load(file) + + if LighthouseConfigFileManager.TYPE_ID not in data: + raise Exception("Type field missing") + + if data[LighthouseConfigFileManager.TYPE_ID] != LighthouseConfigFileManager.TYPE: + raise Exception("Unsupported file type") + + if LighthouseConfigFileManager.VERSION_ID not in data: + raise Exception("Version field missing") + + if data[LighthouseConfigFileManager.VERSION_ID] != LighthouseConfigFileManager.VERSION: + raise Exception("Unsupported file version") + + result_geos = {} + result_calibs = {} + + if LighthouseConfigFileManager.GEOS_ID in data: + for id, geo in data[LighthouseConfigFileManager.GEOS_ID].items(): + result_geos[id] = LighthouseBsGeometry.from_file_object(geo) + + if LighthouseConfigFileManager.CALIBS_ID in data: + for id, calib in data[LighthouseConfigFileManager.CALIBS_ID].items(): + result_calibs[id] = LighthouseBsCalibration.from_file_object(calib) + + return result_geos, result_calibs diff --git a/test/crazyflie/mem/__init__.py b/test/crazyflie/mem/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/test/crazyflie/mem/test_lighthouse_memory.py b/test/crazyflie/mem/test_lighthouse_memory.py new file mode 100644 index 000000000..f9fdbae9c --- /dev/null +++ b/test/crazyflie/mem/test_lighthouse_memory.py @@ -0,0 +1,109 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry +from cflib.crazyflie.mem import LighthouseBsCalibration +import unittest + + +class TestLighthouseMemory(unittest.TestCase): + def test_bs_calibration_file_format(self): + # Fixture + calib = LighthouseBsCalibration() + calib.uid = 1234 + + calib.sweeps[0].curve = 1.0 + calib.sweeps[0].phase = 2.0 + calib.sweeps[0].tilt = 3.0 + calib.sweeps[0].gibmag = 4.0 + calib.sweeps[0].gibphase = 5.0 + calib.sweeps[0].ogeemag = 6.0 + calib.sweeps[0].ogeephase = 7.0 + + calib.sweeps[1].curve = 8.0 + + # Test + actual = calib.as_file_object() + + # Assert + self.assertEqual(1234, actual['uid']) + self.assertEqual(1.0, actual['sweeps'][0]['curve']) + self.assertEqual(2.0, actual['sweeps'][0]['phase']) + self.assertEqual(3.0, actual['sweeps'][0]['tilt']) + self.assertEqual(4.0, actual['sweeps'][0]['gibmag']) + self.assertEqual(5.0, actual['sweeps'][0]['gibphase']) + self.assertEqual(6.0, actual['sweeps'][0]['ogeemag']) + self.assertEqual(7.0, actual['sweeps'][0]['ogeephase']) + self.assertEqual(8.0, actual['sweeps'][1]['curve']) + + def test_bs_calibration_file_write_read(self): + # Fixture + calib = LighthouseBsCalibration() + calib.uid = 1234 + + calib.sweeps[0].curve = 1.0 + calib.sweeps[0].phase = 2.0 + calib.sweeps[0].tilt = 3.0 + calib.sweeps[0].gibmag = 4.0 + calib.sweeps[0].gibphase = 5.0 + calib.sweeps[0].ogeemag = 6.0 + calib.sweeps[0].ogeephase = 7.0 + + calib.sweeps[1].curve = 8.0 + + file_object = calib.as_file_object() + + # Test + actual = LighthouseBsCalibration.from_file_object(file_object) + + # Assert + actual_file_object = actual.as_file_object() + self.assertEqual(file_object, actual_file_object) + self.assertTrue(actual.valid) + + def test_bs_geometry_file_format(self): + # Fixture + geo = LighthouseBsGeometry() + geo.origin = [1.0, 2.0, 3.0] + geo.rotation_matrix = [[1.0, 2.0, 3.0], [1.1, 2.1, 3.1], [1.2, 2.2, 3.2]] + + # Test + actual = geo.as_file_object() + + # Assert + self.assertEqual([1.0, 2.0, 3.0], actual['origin']) + self.assertEqual([[1.0, 2.0, 3.0], [1.1, 2.1, 3.1], [1.2, 2.2, 3.2]], actual['rotation']) + + def test_bs_geometry_file_write_read(self): + # Fixture + geo = LighthouseBsGeometry() + geo.origin = [1.0, 2.0, 3.0] + geo.rotation_matrix = [[1.0, 2.0, 3.0], [1.1, 2.1, 3.1], [1.2, 2.2, 3.2]] + + file_object = geo.as_file_object() + + # Test + actual = LighthouseBsGeometry.from_file_object(file_object) + + # Assert + actual_file_object = actual.as_file_object() + self.assertEqual(file_object, actual_file_object) + self.assertTrue(actual.valid) diff --git a/test/localization/fixtures/system_config.yaml b/test/localization/fixtures/system_config.yaml new file mode 100644 index 000000000..3930516cd --- /dev/null +++ b/test/localization/fixtures/system_config.yaml @@ -0,0 +1,68 @@ +calibs: + 0: + sweeps: + - curve: 3.0 + gibmag: 4.0 + gibphase: 5.0 + ogeemag: 6.0 + ogeephase: 7.0 + phase: 1.0 + tilt: 2.0 + - curve: 3.1 + gibmag: 4.1 + gibphase: 5.1 + ogeemag: 6.1 + ogeephase: 7.1 + phase: 1.1 + tilt: 2.1 + uid: 1234 + 1: + sweeps: + - curve: 3.5 + gibmag: 4.5 + gibphase: 5.5 + ogeemag: 6.5 + ogeephase: 7.5 + phase: 1.5 + tilt: 2.5 + - curve: 3.51 + gibmag: 4.51 + gibphase: 5.51 + ogeemag: 6.51 + ogeephase: 7.51 + phase: 1.51 + tilt: 2.51 + uid: 9876 +geos: + 0: + origin: + - 1.0 + - 2.0 + - 3.0 + rotation: + - - 4.0 + - 5.0 + - 6.0 + - - 7.0 + - 8.0 + - 9.0 + - - 10.0 + - 11.0 + - 12.0 + 1: + origin: + - 21.0 + - 22.0 + - 23.0 + rotation: + - - 24.0 + - 25.0 + - 26.0 + - - 27.0 + - 28.0 + - 29.0 + - - 30.0 + - 31.0 + - 32.0 +type: lighthouse_system_configuration +version: '1' diff --git a/test/localization/test_lighthouse_config_manager.py b/test/localization/test_lighthouse_config_manager.py new file mode 100644 index 000000000..7a477c0f1 --- /dev/null +++ b/test/localization/test_lighthouse_config_manager.py @@ -0,0 +1,139 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 yaml +import unittest +from unittest.mock import patch +from unittest.mock import mock_open +from unittest.mock import ANY + +from cflib.localization import LighthouseConfigFileManager + + +class TestLighthouseConfigFileManager(unittest.TestCase): + def setUp(self): + self.data = { + 'type': 'lighthouse_system_configuration', + 'version': '1', + } + + @patch('yaml.safe_load') + def test_that_read_open_correct_file(self, mock_yaml_load): + # Fixture + mock_yaml_load.return_value = self.data + file_name = 'some/name.yaml' + + # Test + with patch('builtins.open', new_callable=mock_open()) as mock_file: + LighthouseConfigFileManager.read(file_name) + + # Assert + mock_file.assert_called_with(file_name, 'r') + + @patch('yaml.safe_load') + def test_that_missing_file_type_raises(self, mock_yaml_load): + # Fixture + self.data.pop('type') + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + LighthouseConfigFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_wrong_file_type_raises(self, mock_yaml_load): + # Fixture + self.data['type'] = 'wrong type' + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + LighthouseConfigFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_missing_version_raises(self, mock_yaml_load): + # Fixture + self.data.pop('version') + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + LighthouseConfigFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_wrong_version_raises(self, mock_yaml_load): + # Fixture + self.data['version'] = 'wrong version' + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + LighthouseConfigFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_no_data_returns_empty_dictionaries(self, mock_yaml_load): + # Fixture + mock_yaml_load.return_value = self.data + + # Test + with patch('builtins.open', new_callable=mock_open()): + actual_geos, actual_calibs = LighthouseConfigFileManager.read('some/name.yaml') + + # Assert + self.assertEqual(0, len(actual_geos)) + self.assertEqual(0, len(actual_calibs)) + + @patch('yaml.dump') + def test_file_end_to_end_write_read(self, mock_yaml_dump): + # Fixture + fixture_file = 'test/localization/fixtures/system_config.yaml' + + file = open(fixture_file, 'r') + expected = yaml.safe_load(file) + file.close() + + # Test + geos, calibs = LighthouseConfigFileManager.read(fixture_file) + with patch('builtins.open', new_callable=mock_open()): + LighthouseConfigFileManager.write('some/name.yaml', geos=geos, calibs=calibs) + + # Assert + mock_yaml_dump.assert_called_with(expected, ANY) + + @patch('yaml.dump') + def test_file_write_to_correct_file(self, mock_yaml_dump): + # Fixture + file_name = 'some/name.yaml' + + # Test + with patch('builtins.open', new_callable=mock_open()) as mock_file: + LighthouseConfigFileManager.write(file_name) + + # Assert + mock_file.assert_called_with(file_name, 'w') From 10a8d0b3c839816233309186b0bdac22715a1e3d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 21 Feb 2021 08:53:12 +0100 Subject: [PATCH 130/861] #192 Styling --- cflib/localization/lighthouse_config_manager.py | 10 +++++----- test/crazyflie/mem/test_lighthouse_memory.py | 5 +++-- test/localization/test_lighthouse_config_manager.py | 7 ++++--- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index 47d6e3cfe..1583ec150 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -22,8 +22,8 @@ """ Functionality to manage lighthouse system configuration (geometry and calibration data). """ - import yaml + from cflib.crazyflie.mem import LighthouseBsCalibration from cflib.crazyflie.mem import LighthouseBsGeometry @@ -66,16 +66,16 @@ def read(file_name): data = yaml.safe_load(file) if LighthouseConfigFileManager.TYPE_ID not in data: - raise Exception("Type field missing") + raise Exception('Type field missing') if data[LighthouseConfigFileManager.TYPE_ID] != LighthouseConfigFileManager.TYPE: - raise Exception("Unsupported file type") + raise Exception('Unsupported file type') if LighthouseConfigFileManager.VERSION_ID not in data: - raise Exception("Version field missing") + raise Exception('Version field missing') if data[LighthouseConfigFileManager.VERSION_ID] != LighthouseConfigFileManager.VERSION: - raise Exception("Unsupported file version") + raise Exception('Unsupported file version') result_geos = {} result_calibs = {} diff --git a/test/crazyflie/mem/test_lighthouse_memory.py b/test/crazyflie/mem/test_lighthouse_memory.py index f9fdbae9c..042bd4c7d 100644 --- a/test/crazyflie/mem/test_lighthouse_memory.py +++ b/test/crazyflie/mem/test_lighthouse_memory.py @@ -19,10 +19,11 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . -from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry -from cflib.crazyflie.mem import LighthouseBsCalibration import unittest +from cflib.crazyflie.mem import LighthouseBsCalibration +from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry + class TestLighthouseMemory(unittest.TestCase): def test_bs_calibration_file_format(self): diff --git a/test/localization/test_lighthouse_config_manager.py b/test/localization/test_lighthouse_config_manager.py index 7a477c0f1..bff79d1de 100644 --- a/test/localization/test_lighthouse_config_manager.py +++ b/test/localization/test_lighthouse_config_manager.py @@ -19,11 +19,12 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import yaml import unittest -from unittest.mock import patch -from unittest.mock import mock_open from unittest.mock import ANY +from unittest.mock import mock_open +from unittest.mock import patch + +import yaml from cflib.localization import LighthouseConfigFileManager From 2e2eca77fcd409de4da89aaf40970ace3941275b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 21 Feb 2021 20:03:29 +0100 Subject: [PATCH 131/861] #192 Added LighthouseConfigWriter class to make it easier to write lighthouse system configuration data to the Crazyflie --- cflib/crazyflie/mem/lighthouse_memory.py | 2 +- cflib/localization/__init__.py | 4 +- .../localization/lighthouse_config_manager.py | 114 ++++++++++++++++++ 3 files changed, 118 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index 247eb2818..e7fdd9703 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -164,7 +164,7 @@ def __init__(self): self.sweeps = [LighthouseCalibrationSweep(), LighthouseCalibrationSweep()] self.uid = 0 - self.valid = True + self.valid = False def set_from_mem_data(self, data): self.sweeps[0] = self._unpack_sweep_calibration( diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py index 15213b0dc..b39ec338f 100644 --- a/cflib/localization/__init__.py +++ b/cflib/localization/__init__.py @@ -24,6 +24,7 @@ from .lighthouse_bs_geo import LighthouseBsGeoEstimator from .lighthouse_bs_vector import LighthouseBsVector from .lighthouse_config_manager import LighthouseConfigFileManager +from .lighthouse_config_manager import LighthouseConfigWriter from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader @@ -32,4 +33,5 @@ 'LighthouseBsVector', 'LighthouseSweepAngleAverageReader', 'LighthouseSweepAngleReader', - 'LighthouseConfigFileManager'] + 'LighthouseConfigFileManager', + 'LighthouseConfigWriter'] diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index 1583ec150..e0d0d89af 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -26,6 +26,7 @@ from cflib.crazyflie.mem import LighthouseBsCalibration from cflib.crazyflie.mem import LighthouseBsGeometry +from cflib.crazyflie.mem import LighthouseMemHelper class LighthouseConfigFileManager: @@ -89,3 +90,116 @@ def read(file_name): result_calibs[id] = LighthouseBsCalibration.from_file_object(calib) return result_geos, result_calibs + + +class LighthouseConfigWriter: + """ + This class is used to write system config data to the Crazyflie RAM and persis to permanent storage + """ + + def __init__(self, cf, nr_of_base_stations=16): + self._cf = cf + self._helper = LighthouseMemHelper(cf) + self._data_stored_cb = None + self._geos_to_write = None + self._geos_to_persist = [] + self._calibs_to_persist = [] + self._write_failed_for_one_or_more_objects = False + self._nr_of_base_stations = nr_of_base_stations + + def write_and_store_config(self, data_stored_cb, geos=None, calibs=None): + """ + Transfer geometry and calibration data to the Crazyflie and persist to permanent storage. + The callback is called when done. + If geos or calibs is None, no data will be written for that data type. + If geos or calibs is a dictionary, the values for the base stations in the dictionary will + transfered to the Crazyflie, data for all other base stations will be invalidated. + """ + if self._data_stored_cb is not None: + raise Exception('Write already in prgress') + self._data_stored_cb = data_stored_cb + + self._cf.loc.receivedLocationPacket.add_callback(self._received_location_packet) + + self._geos_to_write = self._prepare_geos(geos) + self._calibs_to_write = self._prepare_calibs(calibs) + + self._geos_to_persist = [] + if self._geos_to_write is not None: + self._geos_to_persist = list(range(self._nr_of_base_stations)) + + self._calibs_to_persist = [] + if self._calibs_to_write is not None: + self._calibs_to_persist = list(range(self._nr_of_base_stations)) + + self._write_failed_for_one_or_more_objects = False + + self._next() + + def write_and_store_config_from_file(self, data_stored_cb, file_name): + """ + Read system configuration data from file and write/persist to the Crazyflie. + Geometry and calibration data for base stations that are not in the config file will be invalidated. + """ + geos, calibs = LighthouseConfigFileManager.read(file_name) + self.write_and_store_config(data_stored_cb, geos=geos, calibs=calibs) + + def _next(self): + if self._geos_to_write is not None: + self._helper.write_geos(self._geos_to_write, self._upload_done) + self._geos_to_write = None + return + + if self._calibs_to_write is not None: + self._helper.write_calibs(self._calibs_to_write, self._upload_done) + self._calibs_to_write = None + return + + if len(self._geos_to_persist) > 0 or len(self._calibs_to_persist) > 0: + self._cf.loc.send_lh_persist_data_packet(self._geos_to_persist, self._calibs_to_persist) + self._geos_to_persist = [] + self._calibs_to_persist = [] + return + + tmp_callback = self._data_stored_cb + self._data_stored_cb = None + if tmp_callback is not None: + tmp_callback(not self._write_failed_for_one_or_more_objects) + + def _upload_done(self, sucess): + if not sucess: + self._write_failed_for_one_or_more_objects = True + self._next() + + def _received_location_packet(self, packet): + # New geo data has been written and stored in the CF + if packet.type == self._cf.loc.LH_PERSIST_DATA: + self._next() + + def _prepare_geos(self, geos): + result = None + + if geos is not None: + result = dict(geos) + + # Pad for base stations without data + empty_geo = LighthouseBsGeometry() + for id in range(self._nr_of_base_stations): + if id not in result: + result[id] = empty_geo + + return result + + def _prepare_calibs(self, calibs): + result = None + + if calibs is not None: + result = dict(calibs) + + # Pad for base stations without data + empty_calib = LighthouseBsCalibration() + for id in range(self._nr_of_base_stations): + if id not in result: + result[id] = empty_calib + + return result From 1dd545fac0db252b132c8626e1b465ba7f520cbd Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 22 Feb 2021 14:19:22 +0100 Subject: [PATCH 132/861] Add system tests for a grounded, single Crazyflie. Initial tests focus on the communication link (regular, bootloader, and power switch). The link tests also include latency and bandwidth tests. Part of issue #196. --- sys_test/single_cf_grounded/README.md | 31 ++++ .../single_cf_grounded/single_cf_grounded.py | 53 ++++++ .../single_cf_grounded/test_bootloader.py | 52 ++++++ sys_test/single_cf_grounded/test_link.py | 156 ++++++++++++++++++ .../single_cf_grounded/test_power_switch.py | 48 ++++++ 5 files changed, 340 insertions(+) create mode 100644 sys_test/single_cf_grounded/README.md create mode 100644 sys_test/single_cf_grounded/single_cf_grounded.py create mode 100644 sys_test/single_cf_grounded/test_bootloader.py create mode 100644 sys_test/single_cf_grounded/test_link.py create mode 100644 sys_test/single_cf_grounded/test_power_switch.py diff --git a/sys_test/single_cf_grounded/README.md b/sys_test/single_cf_grounded/README.md new file mode 100644 index 000000000..2a893e02b --- /dev/null +++ b/sys_test/single_cf_grounded/README.md @@ -0,0 +1,31 @@ +# Tests for a single Crazyflie 2.X (USB & Crazyradio) without flight + +## Preparation + +* attach a single Crazyflie over USB +* attach a Crazyradio (PA) +* (Optional) update URI in `single_cf_grounded.py` + +## Execute Tests + +All tests: + +``` +python3 -m unittest discover . -v +``` + +A single test file, e.g.: + +``` +python3 test_power_switch.py +``` + +A concrete test case, e.g.: + +``` +python3 test_power_switch.py TestPowerSwitch.test_reboot +``` + +## Environment Variables + +Prepend command with `USE_CFLINK=cpp` to run with cflinkcpp native link library. diff --git a/sys_test/single_cf_grounded/single_cf_grounded.py b/sys_test/single_cf_grounded/single_cf_grounded.py new file mode 100644 index 000000000..aec105300 --- /dev/null +++ b/sys_test/single_cf_grounded/single_cf_grounded.py @@ -0,0 +1,53 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 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 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +import unittest + +import cflib.crtp +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort + + +class TestSingleCfGrounded(unittest.TestCase): + def setUp(self): + cflib.crtp.init_drivers(enable_debug_driver=False) + self.radioUri = 'radio://0/80/2M/E7E7E7E7E7' + self.usbUri = 'usb://0' + + def is_stm_connected(self): + link = cflib.crtp.get_link_driver(self.radioUri) + + pk = CRTPPacket() + pk.set_header(CRTPPort.LINKCTRL, 0) # Echo channel + pk.data = b'test' + link.send_packet(pk) + for _ in range(10): + pk_ack = link.receive_packet(0.1) + print(pk_ack) + if pk_ack is not None and pk.data == pk_ack.data: + link.close() + return True + link.close() + return False + # print(pk_ack) + # return pk_ack is not None diff --git a/sys_test/single_cf_grounded/test_bootloader.py b/sys_test/single_cf_grounded/test_bootloader.py new file mode 100644 index 000000000..07649e61b --- /dev/null +++ b/sys_test/single_cf_grounded/test_bootloader.py @@ -0,0 +1,52 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 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 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +import time +import unittest + +from single_cf_grounded import TestSingleCfGrounded + +from cflib.bootloader import Bootloader +from cflib.bootloader.boottypes import BootVersion + + +class TestBootloader(TestSingleCfGrounded): + + def test_boot_to_bootloader(self): + self.assertTrue(self.is_stm_connected()) + bl = Bootloader(self.radioUri) + started = bl.start_bootloader(warm_boot=True) + self.assertTrue(started) + + # t = bl.get_target(TargetTypes.NRF51) + # print(t) + + bl.reset_to_firmware() + bl.close() + time.sleep(1) + self.assertTrue(self.is_stm_connected()) + self.assertTrue(BootVersion.is_cf2(bl.protocol_version)) + + +if __name__ == '__main__': + unittest.main() diff --git a/sys_test/single_cf_grounded/test_link.py b/sys_test/single_cf_grounded/test_link.py new file mode 100644 index 000000000..321e9369f --- /dev/null +++ b/sys_test/single_cf_grounded/test_link.py @@ -0,0 +1,156 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 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 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +import struct +import time +import unittest + +import numpy as np +from single_cf_grounded import TestSingleCfGrounded + +import cflib.crtp +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort + + +class TestLink(TestSingleCfGrounded): + + # def test_scan(self): + # start_time = time.time() + # result = cflib.crtp.scan_interfaces() + # end_time = time.time() + # uris = [uri for (uri, msg) in result] + # self.assertEqual(len(uris), 2) + # self.assertIn("radio://*/80/2M/E7E7E7E7E7", uris) + # self.assertIn("usb://0", uris) + # self.assertLess(end_time - start_time, 2) + + # def test_latency_radio_s4(self): + # result = self.latency(self.radioUri, 4) + # self.assertLess(result, 8) + + # def test_latency_radio_s28(self): + # result = self.latency(self.radioUri, 28) + # self.assertLess(result, 8) + + def test_latency_usb_s4(self): + result = self.latency(self.usbUri, 4, 1000) + self.assertLess(result, 1) + + def test_latency_usb_s28(self): + result = self.latency(self.usbUri, 28, 1000) + self.assertLess(result, 1) + + # def test_bandwidth_radio_s4(self): + # result = self.bandwidth(self.radioUri, 4) + # self.assertGreater(result, 450) + + # def test_bandwidth_radio_s28(self): + # result = self.bandwidth(self.radioUri, 28) + # self.assertGreater(result, 450) + + def test_bandwidth_usb_s4(self): + result = self.bandwidth(self.usbUri, 4, 1000) + self.assertGreater(result, 1000) + + def test_bandwidth_usb_s28(self): + result = self.bandwidth(self.usbUri, 28, 1000) + self.assertGreater(result, 1500) + + def latency(self, uri, packet_size=4, count=500): + link = cflib.crtp.get_link_driver(uri) + # # wait until no more packets in queue + # while True: + # pk = link.receive_packet(0.5) + # print(pk) + # if not pk or pk.header == 0xFF: + # break + + pk = CRTPPacket() + pk.set_header(CRTPPort.LINKCTRL, 0) # Echo channel + + latencies = [] + for i in range(count): + pk.data = self.build_data(i, packet_size) + + start_time = time.time() + link.send_packet(pk) + while True: + pk_ack = link.receive_packet(-1) + if pk_ack.port == CRTPPort.LINKCTRL and pk_ack.channel == 0: + break + end_time = time.time() + + # make sure we actually received the expected value + i_recv, = struct.unpack(' Date: Mon, 22 Feb 2021 14:34:31 +0100 Subject: [PATCH 133/861] change URI format to be a valid URL --- cflib/bootloader/cloader.py | 6 +++--- cflib/crtp/radiodriver.py | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 79284c5e6..69db51506 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -128,7 +128,7 @@ def reset_to_bootloader(self, target_id): self.link.close() time.sleep(0.2) self.link = cflib.crtp.get_link_driver( - 'radio://0/0/2M/{:X}[noSafelink]'.format(addr)) + 'radio://0/0/2M/{:X}?safelink=0'.format(addr)) return True else: @@ -224,10 +224,10 @@ def open_bootloader_uri(self, uri=None): if self.link: self.link.close() if uri: - self.link = cflib.crtp.get_link_driver(uri + '[noSafelink]') + self.link = cflib.crtp.get_link_driver(uri + '?safelink=0') else: self.link = cflib.crtp.get_link_driver( - self.clink_address + '[noSafelink]') + self.clink_address + '?safelink=0') def check_link_and_get_info(self, target_id=0xFF): """Try to get a connection with the bootloader by requesting info diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index ee9c9604a..c68b853a3 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -298,11 +298,11 @@ def parse_uri(uri): # Open the USB dongle if not re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))' - '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\\[noSafelink\\])?$', uri): + '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\\?.+)?$', uri): raise WrongUriType('Wrong radio URI format!') uri_data = re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))' - '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\\[noSafelink\\])?$', uri) + '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\\?.+)?$', uri) if len(uri_data.group(1)) < 10 and uri_data.group(1).isdigit(): devid = int(uri_data.group(1)) From e0601b09c2bcbcb2220e2ee90e4d332e7374a18e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 22 Feb 2021 16:30:51 +0100 Subject: [PATCH 134/861] change URI format to be a valid URL --- cflib/utils/power_switch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index e62d4637b..cbb8a3fd4 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -37,7 +37,7 @@ class PowerSwitch: def __init__(self, uri): self.uri = uri - uri_augmented = uri+'[noSafelink][noAutoPing][noAckFilter]' + uri_augmented = uri+'?safelink=0&autoping=0&ackfilter=0' self.link = cflib.crtp.get_link_driver(uri_augmented) # Switch to legacy mode, if uri options are not available if not self.link: From ec5293a82191cb791d8fabaeb3a28aac2c381925 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 27 Feb 2021 18:30:00 +0100 Subject: [PATCH 135/861] #201 Added deck memory access to memory subsystem --- cflib/crazyflie/mem/__init__.py | 13 +- cflib/crazyflie/mem/deck_memory.py | 239 ++++++++++++++++++++++++++ cflib/crazyflie/mem/memory_element.py | 2 + examples/read_deck_mem.py | 99 +++++++++++ 4 files changed, 351 insertions(+), 2 deletions(-) create mode 100644 cflib/crazyflie/mem/deck_memory.py create mode 100644 examples/read_deck_mem.py diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 212755139..0851f68a4 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -25,7 +25,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. """ -Enables flash access to the Crazyflie. +Enables access to the Crazyflie memory subsystem. """ import errno @@ -33,6 +33,7 @@ import struct from threading import Lock +from .deck_memory import DeckMemoryManager from .i2c_element import I2CElement from .led_driver_memory import LEDDriverMemory from .led_timings_driver_memory import LEDTimingsDriverMemory @@ -53,7 +54,8 @@ __author__ = 'Bitcraze AB' __all__ = ['Memory', 'Poly4D', 'MemoryElement', - 'LighthouseBsGeometry', 'LighthouseBsCalibration', 'LighthouseMemHelper'] + 'LighthouseBsGeometry', 'LighthouseBsCalibration', 'LighthouseMemHelper', + 'DeckMemoryManager'] # Channels used for the logging port CHAN_INFO = 0 @@ -466,6 +468,13 @@ def _new_packet_cb(self, packet): logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_DECK_MEMORY: + mem = DeckMemoryManager(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem._new_data) + self.mem_read_failed_cb.add_callback(mem._new_data_failed) + self.mem_write_cb.add_callback(mem._write_done) + self.mem_write_failed_cb.add_callback(mem._write_failed) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py new file mode 100644 index 000000000..12e3d6006 --- /dev/null +++ b/cflib/crazyflie/mem/deck_memory.py @@ -0,0 +1,239 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 logging +import struct + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + + +class DeckMemory: + """ + This class represents the memory in one deck. It is used + to read and write to the deck memory. + """ + + MASK_IS_VALID = 1 + MASK_IS_STARTED = 2 + MASK_SUPPORTS_READ = 4 + MASK_SUPPORTS_WRITE = 8 + MASK_SUPPORTS_UPGRADE = 16 + MASK_UPGRADE_REQUIRED = 32 + MASK_BOOTLOADER_ACTIVE = 64 + + def __init__(self, deck_memory_manager): + self._deck_memory_manager = deck_memory_manager + self.required_hash = None + self.required_length = None + self.name = None + + self._base_address = None + self._bit_field = 0 + + def write(self, address, data, write_complete_cb, write_failed_cb=None): + if not self.supports_write: + raise Exception('Deck does not support write operations') + if not self.is_started: + raise Exception('Deck not ready') + + self._deck_memory_manager._write(self._base_address, address, data, write_complete_cb, write_failed_cb) + + def read(self, address, length, read_complete_cb, read_failed_cb=None): + if not self.supports_read: + raise Exception('Deck does not support read operations') + if not self.is_started: + raise Exception('Deck not ready') + + self._deck_memory_manager._read(self._base_address, address, length, read_complete_cb, read_failed_cb) + + @property + def is_valid(self): + return (self._bit_field & self.MASK_IS_VALID) != 0 + + @property + def is_started(self): + return (self._bit_field & self.MASK_IS_STARTED) != 0 + + @property + def supports_read(self): + return (self._bit_field & self.MASK_SUPPORTS_READ) != 0 + + @property + def supports_write(self): + return (self._bit_field & self.MASK_SUPPORTS_WRITE) != 0 + + @property + def supports_fw_upgrade(self): + return (self._bit_field & self.MASK_SUPPORTS_UPGRADE) != 0 + + @property + def is_fw_upgrade_required(self): + return (self._bit_field & self.MASK_UPGRADE_REQUIRED) != 0 + + @property + def is_bootloader_active(self): + return (self._bit_field & self.MASK_BOOTLOADER_ACTIVE) != 0 + + def _parse(self, data): + self._bit_field, self.required_hash, self.required_length, self._base_address, _name = struct.unpack( + ' Date: Mon, 1 Mar 2021 19:21:25 +0100 Subject: [PATCH 136/861] #201 Corrected string --- examples/read_deck_mem.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index fd8558bc5..25f0588f4 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -63,7 +63,7 @@ def __init__(self, uri): print('This deck supports FW upgrades') print( f' The required FW is: hash={deck_mem.required_hash}, ' - 'length={deck_mem.required_length}, name={deck_mem.name}') + 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') From 8d1cd6a14f59ca623391cf837dbb5036a4e11b7b Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 2 Mar 2021 10:57:32 +0100 Subject: [PATCH 137/861] Document use of pre-commit hooks --- README.md | 9 ++++++++- setup.py | 9 ++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b73d45a9b..216af2049 100644 --- a/README.md +++ b/README.md @@ -39,10 +39,17 @@ Note: For systems that support [make](https://www.gnu.org/software/make/manual/h create an environment, activate it and install dependencies. #### Install cflib dependencies -Install dependencies required by the lib: `pip install -r requirements.txt` +Install dependencies required by the lib: `pip install -r requirements.txt`. If you are planning on developing on the lib you should also run: `pip install -r requirements-dev.txt`. To verify the installation, connect the crazyflie and run an example: `python examples/basiclog` +### Pre commit hooks +If you want some extra help with keeping to the mandated python coding style you can install hooks that verify your style at commit time. This is done by running: +``` +$ pre-commit install +``` +This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes and alert you if you need to change anything. + ## Testing ### With docker and the toolbelt diff --git a/setup.py b/setup.py index 590e6eea0..532155e88 100644 --- a/setup.py +++ b/setup.py @@ -27,5 +27,12 @@ install_requires=[ 'pyusb>=1.0.0b2', 'opencv-python-headless==4.5.1.48' - ] + ], + + # $ pip install -e .[dev] + extras_require={ + 'dev': [ + 'pre-commit' + ], + }, ) From 3fb432bedf8e9283c9fc5bad3d60c12455617ddc Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 4 Mar 2021 12:09:13 +0100 Subject: [PATCH 138/861] radiodriver: Make sure we pad the address if less than 10 chars If we get an address such as 0xDEADBEEF then we need to make sure we pad it to '00DEADBEEF' to have the struct packing work as expected. fixes bitcraze/crazyflie-clients-python##475 --- cflib/crtp/radiodriver.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 566458b98..974c09e70 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -328,7 +328,9 @@ def parse_uri(uri): address = DEFAULT_ADDR_A if uri_data.group(9): - addr = str(uri_data.group(9)) + # We make sure to pad the address with zeros if we do not have the + # correct length. + addr = '{:0>10}'.format(uri_data.group(9)) new_addr = struct.unpack(' Date: Wed, 10 Mar 2021 15:57:17 +0100 Subject: [PATCH 139/861] Correction to parsing of deck memory info section. Only decode deck the name if the deck data is valid --- cflib/crazyflie/mem/deck_memory.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 12e3d6006..13d9e2b63 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -95,9 +95,10 @@ def is_bootloader_active(self): return (self._bit_field & self.MASK_BOOTLOADER_ACTIVE) != 0 def _parse(self, data): - self._bit_field, self.required_hash, self.required_length, self._base_address, _name = struct.unpack( - ' Date: Wed, 10 Mar 2021 15:58:41 +0100 Subject: [PATCH 140/861] Added class to simplify design of synchronous wrapper functions for asynchronous methods --- cflib/utils/callbacks.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/cflib/utils/callbacks.py b/cflib/utils/callbacks.py index 1bfc52ef4..4d684aecf 100644 --- a/cflib/utils/callbacks.py +++ b/cflib/utils/callbacks.py @@ -28,6 +28,8 @@ Callback objects used in the Crazyflie library """ +from threading import Event + __author__ = 'Bitcraze AB' __all__ = ['Caller'] @@ -53,3 +55,28 @@ def call(self, *args): copy_of_callbacks = list(self.callbacks) for cb in copy_of_callbacks: cb(*args) + + +class Syncer: + """A class to create syncronous behaviour for methods using callbacks""" + def __init__(self): + self._event = Event() + self.success_args = None + self.failure_args = None + self.is_success = False + + def success_cb(self, *args): + self.success_args = args + self.is_success = True + self._event.set() + + def failure_cb(self, *args): + self.failure_args = args + self.is_success = False + self._event.set() + + def wait(self): + self._event.wait() + + def clear(self): + self._event.clear() From 4773cbf7297fa16e7bf43b9d058ec8ea71fdafff Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 10 Mar 2021 15:59:50 +0100 Subject: [PATCH 141/861] Added synchronous wrappers for mapped deck memory functions --- cflib/crazyflie/mem/deck_memory.py | 32 ++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 13d9e2b63..825cded0e 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -23,6 +23,7 @@ import struct from .memory_element import MemoryElement +from cflib.utils.callbacks import Syncer logger = logging.getLogger(__name__) @@ -51,6 +52,7 @@ def __init__(self, deck_memory_manager): self._bit_field = 0 def write(self, address, data, write_complete_cb, write_failed_cb=None): + """Write a block of binary data to the deck""" if not self.supports_write: raise Exception('Deck does not support write operations') if not self.is_started: @@ -58,7 +60,15 @@ def write(self, address, data, write_complete_cb, write_failed_cb=None): self._deck_memory_manager._write(self._base_address, address, data, write_complete_cb, write_failed_cb) + def write_sync(self, address, data): + """Write a block of binary data to the deck, block until done""" + syncer = Syncer() + self.write(address, data, syncer.success_cb, write_failed_cb=syncer.failure_cb) + syncer.wait() + return syncer.is_success + def read(self, address, length, read_complete_cb, read_failed_cb=None): + """Read a block of data from a deck""" if not self.supports_read: raise Exception('Deck does not support read operations') if not self.is_started: @@ -66,6 +76,16 @@ def read(self, address, length, read_complete_cb, read_failed_cb=None): self._deck_memory_manager._read(self._base_address, address, length, read_complete_cb, read_failed_cb) + def read_sync(self, address, length): + """Read a block of data from a deck, block until done""" + syncer = Syncer() + self.read(address, length, syncer.success_cb, read_failed_cb=syncer.failure_cb) + syncer.wait() + if syncer.is_success: + return syncer.success_args[1] + else: + return None + @property def is_valid(self): return (self._bit_field & self.MASK_IS_VALID) != 0 @@ -238,3 +258,15 @@ def disconnect(self): self._clear_read_cb() self._clear_write_cb() self.deck_memories = {} + + +class SyncDeckMemoryManager: + """A wrapper for the DeckMemoryManager class to make calls synchronous and avoid callbacks""" + def __init__(self, deck_memory_manager): + self._deck_memory_manager = deck_memory_manager + + def query_decks(self): + syncer = Syncer() + self._deck_memory_manager.query_decks(syncer.success_cb) + syncer.wait() + return syncer.success_args[0] From ccf968f42c1a04ac2a7797f9a5179d4eddb6f0b7 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 10 Mar 2021 16:08:17 +0100 Subject: [PATCH 142/861] Styling fixes --- cflib/crazyflie/mem/deck_memory.py | 1 + cflib/utils/callbacks.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 825cded0e..8a619f0d7 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -262,6 +262,7 @@ def disconnect(self): class SyncDeckMemoryManager: """A wrapper for the DeckMemoryManager class to make calls synchronous and avoid callbacks""" + def __init__(self, deck_memory_manager): self._deck_memory_manager = deck_memory_manager diff --git a/cflib/utils/callbacks.py b/cflib/utils/callbacks.py index 4d684aecf..e6babb31f 100644 --- a/cflib/utils/callbacks.py +++ b/cflib/utils/callbacks.py @@ -27,7 +27,6 @@ """ Callback objects used in the Crazyflie library """ - from threading import Event __author__ = 'Bitcraze AB' @@ -59,6 +58,7 @@ def call(self, *args): class Syncer: """A class to create syncronous behaviour for methods using callbacks""" + def __init__(self): self._event = Event() self.success_args = None From 66a6fb6d7bccae9acfc1dfe2b9f1477ce3c65277 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 11 Mar 2021 06:45:27 +0100 Subject: [PATCH 143/861] debugdriver: Handle request for protcol version This is needed in order to the client to be completly in a connected state. Closes: bitcraze/crazyflie-clients-python#437 --- cflib/crtp/debugdriver.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/cflib/crtp/debugdriver.py b/cflib/crtp/debugdriver.py index 2ba70f808..25ad41775 100644 --- a/cflib/crtp/debugdriver.py +++ b/cflib/crtp/debugdriver.py @@ -452,11 +452,25 @@ def run(self): self.handleParam(pk) elif (pk.port == CRTPPort.MEM): self._handle_mem_access(pk) + elif (pk.port == CRTPPort.LINKCTRL): + self._handle_link_ctrl(pk) else: logger.warning( 'Not handling incoming packets on port [%d]', pk.port) + def _handle_link_ctrl(self, pk): + """ Handle request for protocol version """ + if pk.channel == 1: # LINKSERVICE_SOURCE + if pk.data[0] == 0: # Request protocol version + p_out = CRTPPacket() + p_out.set_header(CRTPPort.LINKCTRL, 1) + # Send protcol version 3. + # We send a version < 4 because debug driver does not yet + # support > 255 TOC entries. + p_out.data = (0, 3) + self._send_packet(p_out) + def _handle_mem_access(self, pk): chan = pk.channel cmd = pk.data[0] From d319c29b69d8e244f7ce8636f355019c88d8232e Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 11 Mar 2021 11:00:03 +0100 Subject: [PATCH 144/861] #208: Initial implementation of deck flashing The flashing sequence is implemented but no feedback to the caller is done yet. --- cflib/bootloader/__init__.py | 271 ++++++++++++++++++----------------- 1 file changed, 139 insertions(+), 132 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 6aa123c9d..0b87cea60 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -32,16 +32,26 @@ import sys import time import zipfile +from collections import namedtuple +from typing import List, Union, Callable from .boottypes import BootVersion from .boottypes import TargetTypes from .cloader import Cloader +from cflib.crazyflie.mem import deck_memory +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.mem import MemoryElement + logger = logging.getLogger(__name__) __author__ = 'Bitcraze AB' __all__ = ['Bootloader'] +Target = namedtuple('Target', ['platform', 'target', 'type']) +FlashArtifact = namedtuple('FlashArtifact', ['content', 'target']) class Bootloader: """Bootloader utility for the Crazyflie""" @@ -66,14 +76,14 @@ def __init__(self, clink=None): # Outgoing callbacks for progress # int - self.progress_cb = None + self.progress_cb = None # type: Union[None, Callable] # msg - self.error_cb = None + self.error_cb = None # type: Union[None, Callable] # bool - self.in_bootloader_cb = None + self.in_bootloader_cb = None # type: Union[None, Callable] # Target - self.dev_info_cb = None - self.terminate_flashing_cb = None + self.dev_info_cb = None # type: Union[None, Callable] + self.terminate_flashing_cb = None # type: Union[None, Callable] # self.dev_info_cb.add_callback(self._dev_info) # self.in_bootloader_cb.add_callback(self._bootloader_info) @@ -113,135 +123,73 @@ def start_bootloader(self, warm_boot=False): self._cload.request_info_update(TargetTypes.NRF51) else: print('Bootloader protocol 0x{:X} not ' - 'supported!'.self.protocol_version) + 'supported!'.format(self.protocol_version)) return started def get_target(self, target_id): return self._cload.request_info_update(target_id) - def read_cf1_config(self): - """Read a flash page from the specified target""" - target = self._cload.targets[0xFF] - config_page = target.flash_pages - 1 - - return self._cload.read_flash(addr=0xFF, page=config_page) - - def write_cf1_config(self, data): - target = self._cload.targets[0xFF] - config_page = target.flash_pages - 1 - - to_flash = {'target': target, 'data': data, 'type': 'CF1 config', - 'start_page': config_page} - - self._internal_flash(target=to_flash) - - def flash(self, filename, targets): - for target in targets: - if TargetTypes.from_string(target) not in self._cload.targets: - print('Target {} not found by bootloader'.format(target)) - return False - - files_to_flash = () - if zipfile.is_zipfile(filename): - # Read the manifest (don't forget to check so there is one!) - try: - zf = zipfile.ZipFile(filename) - js = zf.read('manifest.json').decode('UTF-8') - j = json.loads(js) - files = j['files'] - platform_id = self._get_platform_id() - files_for_platform = self._filter_platform(files, platform_id) - if len(targets) == 0: - targets = self._extract_targets_from_manifest_files( - files_for_platform) - - zip_targets = self._extract_zip_targets(files_for_platform) - except KeyError as e: - print(e) - print('No manifest.json in {}'.format(filename)) - return - - try: - # Match and create targets - for target in targets: - t = targets[target] - for type in t: - file_to_flash = {} - current_target = '{}-{}'.format(target, type) - file_to_flash['type'] = type - # Read the data, if this fails we bail - file_to_flash['target'] = self._cload.targets[ - TargetTypes.from_string(target)] - file_to_flash['data'] = zf.read( - zip_targets[target][type]['filename']) - file_to_flash['start_page'] = file_to_flash[ - 'target'].start_page - files_to_flash += (file_to_flash,) - except KeyError: - print('Could not find a file for {} in {}'.format( - current_target, filename)) - return False - - else: - if len(targets) != 1: - print('Not an archive, must supply one target to flash') + def flash(self, filename, targets: List[Target]): + # Separate flash targets from decks + platform = self._get_platform_id() + flash_targets = [t for t in targets if t.platform == platform] + deck_targets = [t for t in targets if t.platform == "deck"] + + # Fetch artifacts from source file + artifacts = self._get_flash_artifacts_from_zip(filename) + if len(artifacts) == 0: + if len(targets) == 1: + content = open(filename, 'br').read() + artifacts = [FlashArtifact(content, targets[0])] else: - file_to_flash = {} - file_to_flash['type'] = 'binary' - f = open(filename, 'rb') - for t in targets: - file_to_flash['target'] = self._cload.targets[ - TargetTypes.from_string(t)] - file_to_flash['type'] = targets[t][0] - file_to_flash['start_page'] = file_to_flash[ - 'target'].start_page - file_to_flash['data'] = f.read() - f.close() - files_to_flash += (file_to_flash,) - - if not self.progress_cb: - print('') - - file_counter = 0 - for target in files_to_flash: - file_counter += 1 - self._internal_flash(target, file_counter, len(files_to_flash)) - - def _filter_platform(self, files, platform_id): - result = {} - for file in files: - file_info = files[file] - file_platform = file_info['platform'] - if platform_id == file_platform: - result[file] = file_info - return result - - def _extract_zip_targets(self, files): - zip_targets = {} - for file in files: - file_name = file - file_info = files[file] - file_target = file_info['target'] - file_type = file_info['type'] - if file_target not in zip_targets: - zip_targets[file_target] = {} - zip_targets[file_target][file_type] = { - 'filename': file_name} - return zip_targets - - def _extract_targets_from_manifest_files(self, files): - targets = {} - for file in files: - file_info = files[file] - file_target = file_info['target'] - file_type = file_info['type'] - if file_target in targets: - targets[file_target] += (file_type,) - else: - targets[file_target] = (file_type,) - - return targets + raise("Cannot flash a .bin to more than one target!") + + # Separate artifacts for flash and decks + flash_artifacts = [a for a in artifacts if a.target.platform == platform] + deck_artifacts = [a for a in artifacts if a.target.platform == "deck"] + + # Flash the MCU flash + if len(targets) == 0 or len(flash_targets) > 0: + self._flash_flash(flash_artifacts, flash_targets) + + # Flash the decks + if len(targets) == 0 or len(deck_targets) > 0: + # Reset to firmware mode + self.reset_to_firmware() + self.close() + time.sleep(3) + + self._flash_deck(deck_artifacts, deck_targets) + + # Reset the firmware after flashing the decks + self.start_bootloader(warm_boot=True) + self.reset_to_firmware() + self.close() + + def _get_flash_artifacts_from_zip(self, filename): + if not zipfile.is_zipfile(filename): + return [] + + zf = zipfile.ZipFile(filename) + + manifest = zf.read("manifest.json").decode("utf8") + manifest = json.loads(manifest) + + if manifest['version'] != 1: + raise Exception("Wrong manifest version") + + flash_artifacts = [] + for (file, metadata) in manifest['files'].items(): + content = zf.read(file) + target = Target(metadata['platform'], metadata['target'], metadata['type']) + flash_artifacts.append(FlashArtifact(content, target)) + + return flash_artifacts + + def _flash_flash(self, artifacts: List[FlashArtifact], targets: List[Target]): + for (i, artifact) in enumerate(artifacts): + self._internal_flash(artifact, i, len(artifacts)) def reset_to_firmware(self): if self._cload.protocol_version == BootVersion.CF2_PROTO_VER: @@ -253,12 +201,14 @@ def close(self): if self._cload: self._cload.close() - def _internal_flash(self, target, current_file_number=1, total_files=1): + def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_files=1): + + target_info = self._cload.targets[TargetTypes.from_string(artifact.target.target)] - image = target['data'] - t_data = target['target'] + image = artifact.content + t_data = target_info - start_page = target['start_page'] + start_page = target_info.start_page # If used from a UI we need some extra things for reporting progress factor = (100.0 * t_data.page_size) / len(image) @@ -272,7 +222,7 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): sys.stdout.write( 'Flashing {} of {} to {} ({}): '.format( current_file_number, total_files, - TargetTypes.to_string(t_data.id), target['type'])) + TargetTypes.to_string(t_data.id), artifact.target.type)) sys.stdout.flush() if len(image) > ((t_data.flash_pages - start_page) * @@ -388,3 +338,60 @@ def _get_platform_id(self): identifier = 'cf2' return identifier + + def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): + flash_all_targets = len(targets) == 0 + + with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: + deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) + deck_mems_count = len(deck_mems) + if deck_mems_count == 0: + return + + mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0]) + decks = mgr.query_decks() + + for (deck_index, deck) in decks.items(): + # Check that we want to flash this deck + deck_target = [t for t in targets if t == Target("deck", deck.name, "fw")] + if (not flash_all_targets) and len(deck_target) == 0: + print(f"Skipping {deck.name}") + continue + + # Check that we have an artifact for this deck + deck_artifacts = [a for a in artifacts if a.target == Target("deck", deck.name, "fw")] + if len(deck_artifacts) == 0: + print(f"Skipping {deck.name}, no artifact for it in the .zip") + continue + deck_artifact = deck_artifacts[0] + + print(f"Handling {deck.name}") + + # Test and wait for the deck to be started + while not deck.is_started: + print("Deck not yet started ...") + time.sleep(500) + deck = mgr.query_decks()[deck_index] + + # Run a brunch of sanity checks ... + if not deck.supports_fw_upgrade: + print(f"Deck {deck.name} does not support firmware update, skipping!") + continue + + if not deck.is_fw_upgrade_required: + print(f"Deck {deck.name} firmware up to date, skipping") + continue + + if not deck.is_bootloader_active: + print(f"Error: Deck {deck.name} bootloader not active, skipping!") + continue + + # ToDo, white the correct file there ... + result = deck.write_sync(0, deck_artifact.content) + if result: + print("Wrote binary to the deck") + else: + print("Failed to write to the deck") + + def _read_manifest(self, manifest): + pass From c257929d2310d22712e317dc8a1d8e946446fd7f Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 11 Mar 2021 13:35:59 +0100 Subject: [PATCH 145/861] #208: Add progress update when updating decks --- cflib/bootloader/__init__.py | 105 ++++++++++++++++++----------------- 1 file changed, 54 insertions(+), 51 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 0b87cea60..a2ba3999d 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -33,7 +33,7 @@ import time import zipfile from collections import namedtuple -from typing import List, Union, Callable +from typing import List, Optional, Callable from .boottypes import BootVersion from .boottypes import TargetTypes @@ -74,25 +74,16 @@ def __init__(self, clink=None): self.error_code = 0 self.protocol_version = 0 - # Outgoing callbacks for progress - # int - self.progress_cb = None # type: Union[None, Callable] - # msg - self.error_cb = None # type: Union[None, Callable] - # bool - self.in_bootloader_cb = None # type: Union[None, Callable] - # Target - self.dev_info_cb = None # type: Union[None, Callable] - self.terminate_flashing_cb = None # type: Union[None, Callable] - - # self.dev_info_cb.add_callback(self._dev_info) - # self.in_bootloader_cb.add_callback(self._bootloader_info) + # Outgoing callbacks for progress and flash termination + self.progress_cb = None # type: Optional[Callable[[str, int], None]] + self.error_cb = None # type: Optional[Callable[[str], None]] + self.terminate_flashing_cb = None # type: Optional[Callable[[], bool]] self._boot_plat = None self._cload = Cloader(clink, - info_cb=self.dev_info_cb, - in_boot_cb=self.in_bootloader_cb) + info_cb=None, + in_boot_cb=None) def start_bootloader(self, warm_boot=False): if warm_boot: @@ -134,7 +125,7 @@ def flash(self, filename, targets: List[Target]): # Separate flash targets from decks platform = self._get_platform_id() flash_targets = [t for t in targets if t.platform == platform] - deck_targets = [t for t in targets if t.platform == "deck"] + deck_targets = [t for t in targets if t.platform == 'deck'] # Fetch artifacts from source file artifacts = self._get_flash_artifacts_from_zip(filename) @@ -143,11 +134,11 @@ def flash(self, filename, targets: List[Target]): content = open(filename, 'br').read() artifacts = [FlashArtifact(content, targets[0])] else: - raise("Cannot flash a .bin to more than one target!") + raise(Exception('Cannot flash a .bin to more than one target!')) # Separate artifacts for flash and decks flash_artifacts = [a for a in artifacts if a.target.platform == platform] - deck_artifacts = [a for a in artifacts if a.target.platform == "deck"] + deck_artifacts = [a for a in artifacts if a.target.platform == 'deck'] # Flash the MCU flash if len(targets) == 0 or len(flash_targets) > 0: @@ -155,6 +146,9 @@ def flash(self, filename, targets: List[Target]): # Flash the decks if len(targets) == 0 or len(deck_targets) > 0: + if self.progress_cb: + self.progress_cb('Restarting firmware to update decks.', int(0)) + # Reset to firmware mode self.reset_to_firmware() self.close() @@ -162,10 +156,20 @@ def flash(self, filename, targets: List[Target]): self._flash_deck(deck_artifacts, deck_targets) + if self.progress_cb: + self.progress_cb('Deck updated! Restarting firmware.', int(100)) + # Reset the firmware after flashing the decks self.start_bootloader(warm_boot=True) self.reset_to_firmware() self.close() + + if self.progress_cb: + self.progress_cb( + '({}/{}) Flashing done!'.format(len(flash_artifacts), len(flash_artifacts)), + int(100)) + else: + print('') def _get_flash_artifacts_from_zip(self, filename): if not zipfile.is_zipfile(filename): @@ -173,11 +177,11 @@ def _get_flash_artifacts_from_zip(self, filename): zf = zipfile.ZipFile(filename) - manifest = zf.read("manifest.json").decode("utf8") + manifest = zf.read('manifest.json').decode('utf8') manifest = json.loads(manifest) if manifest['version'] != 1: - raise Exception("Wrong manifest version") + raise Exception('Wrong manifest version') flash_artifacts = [] for (file, metadata) in manifest['files'].items(): @@ -189,7 +193,7 @@ def _get_flash_artifacts_from_zip(self, filename): def _flash_flash(self, artifacts: List[FlashArtifact], targets: List[Target]): for (i, artifact) in enumerate(artifacts): - self._internal_flash(artifact, i, len(artifacts)) + self._internal_flash(artifact, i + 1, len(artifacts)) def reset_to_firmware(self): if self._cload.protocol_version == BootVersion.CF2_PROTO_VER: @@ -216,7 +220,7 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ if self.progress_cb: self.progress_cb( - '({}/{}) Starting...'.format(current_file_number, total_files), + 'Firmware ({}/{}) Starting...'.format(current_file_number, total_files), int(progress)) else: sys.stdout.write( @@ -228,9 +232,7 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ if len(image) > ((t_data.flash_pages - start_page) * t_data.page_size): if self.progress_cb: - self.progress_cb( - 'Error: Not enough space to flash the image file.', - int(progress)) + self.progress_cb('Error: Not enough space to flash the image file.', int(progress)) else: print('Error: Not enough space to flash the image file.') raise Exception() @@ -261,7 +263,7 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ if self.progress_cb: progress += factor - self.progress_cb('({}/{}) Uploading buffer to {}...'.format( + self.progress_cb('Firmware ({}/{}) Uploading buffer to {}...'.format( current_file_number, total_files, TargetTypes.to_string(t_data.id)), @@ -274,7 +276,7 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ # Flash when the complete buffers are full if ctr >= t_data.buffer_pages: if self.progress_cb: - self.progress_cb('({}/{}) Writing buffer to {}...'.format( + self.progress_cb('Firmware ({}/{}) Writing buffer to {}...'.format( current_file_number, total_files, TargetTypes.to_string(t_data.id)), @@ -301,7 +303,7 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ if ctr > 0: if self.progress_cb: - self.progress_cb('({}/{}) Writing buffer to {}...'.format( + self.progress_cb('Firmware ({}/{}) Writing buffer to {}...'.format( current_file_number, total_files, TargetTypes.to_string(t_data.id)), @@ -323,14 +325,6 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ ' wrong radio link?' % self._cload.error_code) raise Exception() - if self.progress_cb: - self.progress_cb( - '({}/{}) Flashing done!'.format(current_file_number, - total_files), - int(100)) - else: - print('') - def _get_platform_id(self): """Get platform identifier used in the zip manifest for curr copter""" identifier = 'cf1' @@ -342,6 +336,9 @@ def _get_platform_id(self): def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): flash_all_targets = len(targets) == 0 + if self.progress_cb: + self.progress_cb('Detecting deck to be updated', int(25)) + with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) deck_mems_count = len(deck_mems) @@ -352,46 +349,52 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): decks = mgr.query_decks() for (deck_index, deck) in decks.items(): + if self.terminate_flashing_cb and self.terminate_flashing_cb(): + raise Exception('Flashing terminated') + # Check that we want to flash this deck - deck_target = [t for t in targets if t == Target("deck", deck.name, "fw")] + deck_target = [t for t in targets if t == Target('deck', deck.name, 'fw')] if (not flash_all_targets) and len(deck_target) == 0: - print(f"Skipping {deck.name}") + print(f'Skipping {deck.name}') continue # Check that we have an artifact for this deck - deck_artifacts = [a for a in artifacts if a.target == Target("deck", deck.name, "fw")] + deck_artifacts = [a for a in artifacts if a.target == Target('deck', deck.name, 'fw')] if len(deck_artifacts) == 0: - print(f"Skipping {deck.name}, no artifact for it in the .zip") + print(f'Skipping {deck.name}, no artifact for it in the .zip') continue deck_artifact = deck_artifacts[0] - print(f"Handling {deck.name}") + if self.progress_cb: + self.progress_cb(f'Updating deck {deck.name}', int(50)) + print(f'Handling {deck.name}') # Test and wait for the deck to be started while not deck.is_started: - print("Deck not yet started ...") + print('Deck not yet started ...') time.sleep(500) deck = mgr.query_decks()[deck_index] # Run a brunch of sanity checks ... if not deck.supports_fw_upgrade: - print(f"Deck {deck.name} does not support firmware update, skipping!") + print(f'Deck {deck.name} does not support firmware update, skipping!') continue if not deck.is_fw_upgrade_required: - print(f"Deck {deck.name} firmware up to date, skipping") + print(f'Deck {deck.name} firmware up to date, skipping') continue if not deck.is_bootloader_active: - print(f"Error: Deck {deck.name} bootloader not active, skipping!") + print(f'Error: Deck {deck.name} bootloader not active, skipping!') continue # ToDo, white the correct file there ... result = deck.write_sync(0, deck_artifact.content) if result: - print("Wrote binary to the deck") + if self.progress_cb: + self.progress_cb(f'Deck {deck.name} updated succesfully!', int(75)) else: - print("Failed to write to the deck") - - def _read_manifest(self, manifest): - pass + if self.progress_cb: + self.progress_cb(f'Failed to update deck {deck.name}', int(0)) + raise Exception(f'Failed to update deck {deck.name}') + From 90ac62d345986094d19f9a708bdd9fb5fa074380 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 11 Mar 2021 13:57:54 +0100 Subject: [PATCH 146/861] #208: Add check to disable deck update on coldboot --- cflib/bootloader/__init__.py | 36 ++++++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index a2ba3999d..ebeb09641 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -145,28 +145,36 @@ def flash(self, filename, targets: List[Target]): self._flash_flash(flash_artifacts, flash_targets) # Flash the decks + deck_update_msg = 'Deck update skipped.' if len(targets) == 0 or len(deck_targets) > 0: - if self.progress_cb: - self.progress_cb('Restarting firmware to update decks.', int(0)) + # only in warm boot + if self.clink: + if self.progress_cb: + self.progress_cb('Restarting firmware to update decks.', int(0)) - # Reset to firmware mode - self.reset_to_firmware() - self.close() - time.sleep(3) + # Reset to firmware mode + self.reset_to_firmware() + self.close() + time.sleep(3) - self._flash_deck(deck_artifacts, deck_targets) + self._flash_deck(deck_artifacts, deck_targets) - if self.progress_cb: - self.progress_cb('Deck updated! Restarting firmware.', int(100)) + if self.progress_cb: + self.progress_cb('Deck updated! Restarting firmware.', int(100)) + + # Reset the firmware after flashing the decks + self.start_bootloader(warm_boot=True) + self.reset_to_firmware() + self.close() - # Reset the firmware after flashing the decks - self.start_bootloader(warm_boot=True) - self.reset_to_firmware() - self.close() + deck_update_msg = 'Deck update complete.' + else: + print('Skipping updating deck on coldboot') + deck_update_msg = 'Deck update skipped in ColdBoot mode.' if self.progress_cb: self.progress_cb( - '({}/{}) Flashing done!'.format(len(flash_artifacts), len(flash_artifacts)), + f'({len(flash_artifacts)}/{len(flash_artifacts)}) Flashing done! {deck_update_msg}', int(100)) else: print('') From 753e5cbc96dc55e7e051d68c31c1b9c524ca950e Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 11 Mar 2021 14:38:53 +0100 Subject: [PATCH 147/861] #208: Exit flash function in bootloader mode Also fix warm-boot detection logic --- cflib/bootloader/__init__.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index ebeb09641..a9113efa2 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -74,6 +74,8 @@ def __init__(self, clink=None): self.error_code = 0 self.protocol_version = 0 + self.warm_booted = False + # Outgoing callbacks for progress and flash termination self.progress_cb = None # type: Optional[Callable[[str, int], None]] self.error_cb = None # type: Optional[Callable[[str], None]] @@ -86,6 +88,8 @@ def __init__(self, clink=None): in_boot_cb=None) def start_bootloader(self, warm_boot=False): + self.warm_booted = warm_boot + if warm_boot: self._cload.open_bootloader_uri(self.clink) started = self._cload.reset_to_bootloader(TargetTypes.NRF51) @@ -148,7 +152,7 @@ def flash(self, filename, targets: List[Target]): deck_update_msg = 'Deck update skipped.' if len(targets) == 0 or len(deck_targets) > 0: # only in warm boot - if self.clink: + if self.warm_booted: if self.progress_cb: self.progress_cb('Restarting firmware to update decks.', int(0)) @@ -162,10 +166,8 @@ def flash(self, filename, targets: List[Target]): if self.progress_cb: self.progress_cb('Deck updated! Restarting firmware.', int(100)) - # Reset the firmware after flashing the decks + # Put the crazyflie back in Bootloader mode to exit the function in the same state we entered it self.start_bootloader(warm_boot=True) - self.reset_to_firmware() - self.close() deck_update_msg = 'Deck update complete.' else: From 6c4a1191f27c6986239c0d168e84469688c473ee Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 Mar 2021 15:08:28 +0100 Subject: [PATCH 148/861] #208 Styling --- cflib/bootloader/__init__.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index a9113efa2..31e0543d3 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -33,17 +33,17 @@ import time import zipfile from collections import namedtuple -from typing import List, Optional, Callable +from typing import Callable +from typing import List +from typing import Optional from .boottypes import BootVersion from .boottypes import TargetTypes from .cloader import Cloader - -from cflib.crazyflie.mem import deck_memory -import cflib.crtp from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.mem import deck_memory from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie logger = logging.getLogger(__name__) @@ -53,6 +53,7 @@ Target = namedtuple('Target', ['platform', 'target', 'type']) FlashArtifact = namedtuple('FlashArtifact', ['content', 'target']) + class Bootloader: """Bootloader utility for the Crazyflie""" @@ -139,7 +140,7 @@ def flash(self, filename, targets: List[Target]): artifacts = [FlashArtifact(content, targets[0])] else: raise(Exception('Cannot flash a .bin to more than one target!')) - + # Separate artifacts for flash and decks flash_artifacts = [a for a in artifacts if a.target.platform == platform] deck_artifacts = [a for a in artifacts if a.target.platform == 'deck'] @@ -173,7 +174,7 @@ def flash(self, filename, targets: List[Target]): else: print('Skipping updating deck on coldboot') deck_update_msg = 'Deck update skipped in ColdBoot mode.' - + if self.progress_cb: self.progress_cb( f'({len(flash_artifacts)}/{len(flash_artifacts)}) Flashing done! {deck_update_msg}', @@ -184,7 +185,7 @@ def flash(self, filename, targets: List[Target]): def _get_flash_artifacts_from_zip(self, filename): if not zipfile.is_zipfile(filename): return [] - + zf = zipfile.ZipFile(filename) manifest = zf.read('manifest.json').decode('utf8') @@ -198,7 +199,7 @@ def _get_flash_artifacts_from_zip(self, filename): content = zf.read(file) target = Target(metadata['platform'], metadata['target'], metadata['type']) flash_artifacts.append(FlashArtifact(content, target)) - + return flash_artifacts def _flash_flash(self, artifacts: List[FlashArtifact], targets: List[Target]): @@ -342,7 +343,7 @@ def _get_platform_id(self): identifier = 'cf2' return identifier - + def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): flash_all_targets = len(targets) == 0 @@ -389,11 +390,11 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): if not deck.supports_fw_upgrade: print(f'Deck {deck.name} does not support firmware update, skipping!') continue - + if not deck.is_fw_upgrade_required: print(f'Deck {deck.name} firmware up to date, skipping') continue - + if not deck.is_bootloader_active: print(f'Error: Deck {deck.name} bootloader not active, skipping!') continue @@ -407,4 +408,3 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): if self.progress_cb: self.progress_cb(f'Failed to update deck {deck.name}', int(0)) raise Exception(f'Failed to update deck {deck.name}') - From 32433ace0cf3e7147fd0249873eb3eeb3c3ec691 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 Mar 2021 20:45:58 +0100 Subject: [PATCH 149/861] Added system type to file management and upload to CF --- .../localization/lighthouse_config_manager.py | 25 +++++++++++++++---- test/localization/fixtures/system_config.yaml | 1 + .../test_lighthouse_config_manager.py | 9 ++++--- 3 files changed, 26 insertions(+), 9 deletions(-) diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index e0d0d89af..3e67f1354 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -36,9 +36,13 @@ class LighthouseConfigFileManager: VERSION = '1' GEOS_ID = 'geos' CALIBS_ID = 'calibs' + SYSTEM_TYPE_ID = 'systemType' + + SYSTEM_TYPE_V1 = 1 + SYSTEM_TYPE_V2 = 2 @staticmethod - def write(file_name, geos={}, calibs={}): + def write(file_name, geos={}, calibs={}, system_type=SYSTEM_TYPE_V2): file = open(file_name, 'w') with file: file_geos = {} @@ -54,6 +58,7 @@ def write(file_name, geos={}, calibs={}): data = { LighthouseConfigFileManager.TYPE_ID: LighthouseConfigFileManager.TYPE, LighthouseConfigFileManager.VERSION_ID: LighthouseConfigFileManager.VERSION, + LighthouseConfigFileManager.SYSTEM_TYPE_ID: system_type, LighthouseConfigFileManager.GEOS_ID: file_geos, LighthouseConfigFileManager.CALIBS_ID: file_calibs } @@ -78,6 +83,10 @@ def read(file_name): if data[LighthouseConfigFileManager.VERSION_ID] != LighthouseConfigFileManager.VERSION: raise Exception('Unsupported file version') + result_system_type = LighthouseConfigFileManager.SYSTEM_TYPE_V2 + if LighthouseConfigFileManager.SYSTEM_TYPE_ID in data: + result_system_type = data[LighthouseConfigFileManager.SYSTEM_TYPE_ID] + result_geos = {} result_calibs = {} @@ -89,7 +98,7 @@ def read(file_name): for id, calib in data[LighthouseConfigFileManager.CALIBS_ID].items(): result_calibs[id] = LighthouseBsCalibration.from_file_object(calib) - return result_geos, result_calibs + return result_geos, result_calibs, result_system_type class LighthouseConfigWriter: @@ -107,7 +116,7 @@ def __init__(self, cf, nr_of_base_stations=16): self._write_failed_for_one_or_more_objects = False self._nr_of_base_stations = nr_of_base_stations - def write_and_store_config(self, data_stored_cb, geos=None, calibs=None): + def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, system_type=2): """ Transfer geometry and calibration data to the Crazyflie and persist to permanent storage. The callback is called when done. @@ -134,6 +143,12 @@ def write_and_store_config(self, data_stored_cb, geos=None, calibs=None): self._write_failed_for_one_or_more_objects = False + # Change system type first as this will erase calib and geo data in the CF. + # Changing system type may trigger a lengthy operation if the persistant memory write takes a long time. + # Setting a param is an asynchronous operataion, and it is not possible to know if the system swich is finished + # before we continue. Is it possible that this will cause problems? + self._cf.param.set_value('lighthouse.systemType', system_type) + self._next() def write_and_store_config_from_file(self, data_stored_cb, file_name): @@ -141,8 +156,8 @@ def write_and_store_config_from_file(self, data_stored_cb, file_name): Read system configuration data from file and write/persist to the Crazyflie. Geometry and calibration data for base stations that are not in the config file will be invalidated. """ - geos, calibs = LighthouseConfigFileManager.read(file_name) - self.write_and_store_config(data_stored_cb, geos=geos, calibs=calibs) + geos, calibs, system_type = LighthouseConfigFileManager.read(file_name) + self.write_and_store_config(data_stored_cb, geos=geos, calibs=calibs, system_type=system_type) def _next(self): if self._geos_to_write is not None: diff --git a/test/localization/fixtures/system_config.yaml b/test/localization/fixtures/system_config.yaml index 3930516cd..3c18ad13b 100644 --- a/test/localization/fixtures/system_config.yaml +++ b/test/localization/fixtures/system_config.yaml @@ -64,5 +64,6 @@ geos: - - 30.0 - 31.0 - 32.0 +systemType: 1 type: lighthouse_system_configuration version: '1' diff --git a/test/localization/test_lighthouse_config_manager.py b/test/localization/test_lighthouse_config_manager.py index bff79d1de..1658715a1 100644 --- a/test/localization/test_lighthouse_config_manager.py +++ b/test/localization/test_lighthouse_config_manager.py @@ -98,17 +98,18 @@ def test_that_wrong_version_raises(self, mock_yaml_load): LighthouseConfigFileManager.read('some/name.yaml') @patch('yaml.safe_load') - def test_that_no_data_returns_empty_dictionaries(self, mock_yaml_load): + def test_that_no_data_returns_empty_default_data(self, mock_yaml_load): # Fixture mock_yaml_load.return_value = self.data # Test with patch('builtins.open', new_callable=mock_open()): - actual_geos, actual_calibs = LighthouseConfigFileManager.read('some/name.yaml') + actual_geos, actual_calibs, actual_system_type = LighthouseConfigFileManager.read('some/name.yaml') # Assert self.assertEqual(0, len(actual_geos)) self.assertEqual(0, len(actual_calibs)) + self.assertEqual(LighthouseConfigFileManager.SYSTEM_TYPE_V2, actual_system_type) @patch('yaml.dump') def test_file_end_to_end_write_read(self, mock_yaml_dump): @@ -120,9 +121,9 @@ def test_file_end_to_end_write_read(self, mock_yaml_dump): file.close() # Test - geos, calibs = LighthouseConfigFileManager.read(fixture_file) + geos, calibs, system_type = LighthouseConfigFileManager.read(fixture_file) with patch('builtins.open', new_callable=mock_open()): - LighthouseConfigFileManager.write('some/name.yaml', geos=geos, calibs=calibs) + LighthouseConfigFileManager.write('some/name.yaml', geos=geos, calibs=calibs, system_type=system_type) # Assert mock_yaml_dump.assert_called_with(expected, ANY) From c1f79d6ad148b2dd847d06d355dc52e22a993523 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 16 Mar 2021 06:06:14 +0100 Subject: [PATCH 150/861] Remove the debug driver The debug driver is a software (Python) endpoint for the CRTP protocol that can be used to develop against when there is no accessable hardware. It requires maintenance in order to stay up-to-date with the CRTP protocol and the Crazyflie and the benefit we receive from it does not outweigh the amount work needed to keep it relevant. --- cflib/crtp/__init__.py | 3 +- cflib/crtp/crtpstack.py | 1 - cflib/crtp/debugdriver.py | 907 ------------------ docs/user-guides/python_api.md | 23 +- docs/user-guides/sbs_connect_log_param.md | 22 +- docs/user-guides/sbs_motion_commander.md | 14 +- examples/autonomousSequence.py | 2 +- examples/autonomous_sequence_high_level.py | 2 +- examples/basicLedTiming.py | 4 +- examples/basicLedmemSync.py | 4 +- examples/basicLedparamSync.py | 4 +- examples/basiclog.py | 4 +- examples/basiclogSync.py | 4 +- examples/basicparam.py | 4 +- examples/cfbridge.py | 4 +- examples/erase-ow.py | 4 +- examples/flash-memory.py | 2 +- examples/flowsequenceSync.py | 4 +- examples/lighthouse/lighthouse_openvr_grab.py | 2 +- .../lighthouse_openvr_grab_color.py | 2 +- .../lighthouse/lighthouse_openvr_multigrab.py | 2 +- examples/lighthouse/read_lighthouse_mem.py | 4 +- examples/lighthouse/write_lighthouse_mem.py | 4 +- examples/lps_reboot_to_bootloader.py | 4 +- examples/motion_commander_demo.py | 4 +- examples/multiramp.py | 4 +- examples/multiranger_pointcloud.py | 2 +- examples/multiranger_push.py | 4 +- examples/position_commander_demo.py | 2 +- examples/positioning/initial_position.py | 2 +- examples/positioning/matrix_light_printer.py | 2 +- examples/qualisys/qualisys_hl_commander.py | 2 +- examples/ramp.py | 4 +- examples/read-eeprom.py | 4 +- examples/read-ow.py | 4 +- examples/read_deck_mem.py | 4 +- examples/scan.py | 2 +- .../step-by-step/sbs_connect_log_param.py | 4 +- examples/step-by-step/sbs_motion_commander.py | 2 +- examples/swarm/hl-commander-swarm.py | 2 +- examples/swarm/swarmSequence.py | 2 +- examples/swarm/swarmSequenceCircle.py | 2 +- examples/swarm/synchronizedSequence.py | 2 +- examples/tuning/PID_controller_tuner.py | 2 +- examples/write-eeprom.py | 4 +- examples/write-ow.py | 4 +- .../single_cf_grounded/single_cf_grounded.py | 2 +- sys_test/swarm_test_rig/test_connection.py | 2 +- sys_test/swarm_test_rig/test_logging.py | 2 +- sys_test/swarm_test_rig/test_memory_map.py | 2 +- sys_test/swarm_test_rig/test_response_time.py | 2 +- 51 files changed, 88 insertions(+), 1016 deletions(-) delete mode 100644 cflib/crtp/debugdriver.py diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index f45ee9f1d..c52bcf1a3 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -28,7 +28,6 @@ import logging import os -from .debugdriver import DebugDriver from .exceptions import WrongUriType from .prrtdriver import PrrtDriver from .radiodriver import RadioDriver @@ -56,7 +55,7 @@ def init_drivers(enable_debug_driver=False, enable_serial_driver=False): CLASSES.extend([RadioDriver, UsbDriver]) if enable_debug_driver: - CLASSES.append(DebugDriver) + logger.warn('The debug driver is no longer supported!') if enable_serial_driver: CLASSES.append(SerialDriver) diff --git a/cflib/crtp/crtpstack.py b/cflib/crtp/crtpstack.py index c640bed2c..929ee58a2 100644 --- a/cflib/crtp/crtpstack.py +++ b/cflib/crtp/crtpstack.py @@ -48,7 +48,6 @@ class CRTPPort: COMMANDER_GENERIC = 0x07 SETPOINT_HL = 0x08 PLATFORM = 0x0D - DEBUGDRIVER = 0x0E LINKCTRL = 0x0F ALL = 0xFF diff --git a/cflib/crtp/debugdriver.py b/cflib/crtp/debugdriver.py deleted file mode 100644 index 25ad41775..000000000 --- a/cflib/crtp/debugdriver.py +++ /dev/null @@ -1,907 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2011-2013 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Fake link driver used to debug the UI without using the Crazyflie. - -The operation of this driver can be controlled in two ways, either by -connecting to different URIs or by sending messages to the DebugDriver port -though CRTP once connected. - -For normal connections a console thread is also started that will send -generated console output via CRTP. -""" -import errno -import logging -import queue -import random -import re -import string -import struct -import time -from datetime import datetime -from threading import Thread - -from .crtpdriver import CRTPDriver -from .crtpstack import CRTPPacket -from .crtpstack import CRTPPort -from .exceptions import WrongUriType -from cflib.crazyflie.log import LogTocElement -from cflib.crazyflie.param import ParamTocElement - -__author__ = 'Bitcraze AB' -__all__ = ['DebugDriver'] - -logger = logging.getLogger(__name__) - -# This setup is used to debug raw memory logging -memlogging = {0x01: {'min': 0, 'max': 255, 'mod': 1, 'vartype': 1}, - 0x02: {'min': 0, 'max': 65000, 'mod': 100, 'vartype': 2}, - 0x03: {'min': 0, 'max': 100000, 'mod': 1000, 'vartype': 3}, - 0x04: {'min': -100, 'max': 100, 'mod': 1, 'vartype': 4}, - 0x05: {'min': -10000, 'max': 10000, 'mod': 2000, 'vartype': 5}, - 0x06: {'min': -50000, 'max': 50000, 'mod': 1000, 'vartype': 6}, - 0x07: {'min': 0, 'max': 255, 'mod': 1, 'vartype': 1}} - - -class FakeMemory: - TYPE_I2C = 0 - TYPE_1W = 1 - - def __init__(self, type, size, addr, data=None): - self.type = type - self.size = size - self.addr = addr - self.data = [0] * size - if data: - for i in range(len(data)): - self.data[i] = data[i] - - def erase(self): - self.data = [0] * self.size - - -class DebugDriver(CRTPDriver): - """ Debug driver used for debugging UI/communication without using a - Crazyflie""" - - def __init__(self): - self.fakeLoggingThreads = [] - self._fake_mems = [] - self.needs_resending = False - # Fill up the fake logging TOC with values and data - self.fakeLogToc = [] - self.fakeLogToc.append({'varid': 0, 'vartype': 5, 'vargroup': 'imu', - 'varname': 'gyro_x', 'min': -10000, - 'max': 10000, 'mod': 1000}) - self.fakeLogToc.append({'varid': 1, 'vartype': 5, 'vargroup': 'imu', - 'varname': 'gyro_y', 'min': -10000, - 'max': 10000, 'mod': 150}) - self.fakeLogToc.append({'varid': 2, 'vartype': 5, 'vargroup': 'imu', - 'varname': 'gyro_z', 'min': -10000, - 'max': 10000, 'mod': 200}) - self.fakeLogToc.append({'varid': 3, 'vartype': 5, 'vargroup': 'imu', - 'varname': 'acc_x', 'min': -1000, - 'max': 1000, 'mod': 15}) - self.fakeLogToc.append({'varid': 4, 'vartype': 5, 'vargroup': 'imu', - 'varname': 'acc_y', 'min': -1000, - 'max': 1000, 'mod': 10}) - self.fakeLogToc.append({'varid': 5, 'vartype': 5, 'vargroup': 'imu', - 'varname': 'acc_z', 'min': -1000, - 'max': 1000, 'mod': 20}) - self.fakeLogToc.append({'varid': 6, 'vartype': 7, - 'vargroup': 'stabilizer', 'varname': 'roll', - 'min': -90, 'max': 90, 'mod': 2}) - self.fakeLogToc.append({'varid': 7, 'vartype': 7, - 'vargroup': 'stabilizer', 'varname': 'pitch', - 'min': -90, 'max': 90, 'mod': 1.5}) - self.fakeLogToc.append({'varid': 8, 'vartype': 7, - 'vargroup': 'stabilizer', 'varname': 'yaw', - 'min': -90, 'max': 90, 'mod': 2.5}) - self.fakeLogToc.append({'varid': 9, 'vartype': 7, 'vargroup': 'pm', - 'varname': 'vbat', 'min': 3.0, - 'max': 4.2, 'mod': 0.1}) - self.fakeLogToc.append({'varid': 10, 'vartype': 6, 'vargroup': 'motor', - 'varname': 'm1', 'min': 0, 'max': 65000, - 'mod': 1000}) - self.fakeLogToc.append({'varid': 11, 'vartype': 6, 'vargroup': 'motor', - 'varname': 'm2', 'min': 0, 'max': 65000, - 'mod': 1000}) - self.fakeLogToc.append({'varid': 12, 'vartype': 6, 'vargroup': 'motor', - 'varname': 'm3', 'min': 0, 'max': 65000, - 'mod': 1000}) - self.fakeLogToc.append({'varid': 13, 'vartype': 6, 'vargroup': 'motor', - 'varname': 'm4', 'min': 0, 'max': 65000, - 'mod': 1000}) - self.fakeLogToc.append({'varid': 14, 'vartype': 2, - 'vargroup': 'stabilizer', 'varname': 'thrust', - 'min': 0, 'max': 65000, 'mod': 1000}) - self.fakeLogToc.append({'varid': 15, 'vartype': 7, - 'vargroup': 'baro', 'varname': 'asl', - 'min': 540, 'max': 545, 'mod': 0.5}) - self.fakeLogToc.append({'varid': 16, 'vartype': 7, - 'vargroup': 'baro', 'varname': 'aslRaw', - 'min': 540, 'max': 545, 'mod': 1.0}) - self.fakeLogToc.append({'varid': 17, 'vartype': 7, - 'vargroup': 'posEstimatorAlt', - 'varname': 'estimatedZ', - 'min': 540, 'max': 545, 'mod': 0.5}) - self.fakeLogToc.append({'varid': 18, 'vartype': 7, - 'vargroup': 'baro', 'varname': 'temp', - 'min': 26, 'max': 38, 'mod': 1.0}) - self.fakeLogToc.append({'varid': 19, 'vartype': 7, - 'vargroup': 'posCtlAlt', - 'varname': 'targetZ', - 'min': 542, 'max': 543, 'mod': 0.1}) - self.fakeLogToc.append({'varid': 20, 'vartype': 6, - 'vargroup': 'gps', 'varname': 'lat', - 'min': 556112190, 'max': 556112790, - 'mod': 10}) - self.fakeLogToc.append({'varid': 21, 'vartype': 6, - 'vargroup': 'gps', 'varname': 'lon', - 'min': 129945110, 'max': 129945710, - 'mod': 10}) - self.fakeLogToc.append({'varid': 22, 'vartype': 6, - 'vargroup': 'gps', 'varname': 'hMSL', - 'min': 0, 'max': 100000, - 'mod': 1000}) - self.fakeLogToc.append({'varid': 23, 'vartype': 6, - 'vargroup': 'gps', 'varname': 'heading', - 'min': -10000000, 'max': 10000000, - 'mod': 100000}) - self.fakeLogToc.append({'varid': 24, 'vartype': 6, - 'vargroup': 'gps', 'varname': 'gSpeed', - 'min': 0, 'max': 1000, - 'mod': 100}) - self.fakeLogToc.append({'varid': 25, 'vartype': 3, - 'vargroup': 'gps', 'varname': 'hAcc', - 'min': 0, 'max': 5000, - 'mod': 100}) - self.fakeLogToc.append({'varid': 26, 'vartype': 1, - 'vargroup': 'gps', 'varname': 'fixType', - 'min': 0, 'max': 5, - 'mod': 1}) - - # Fill up the fake logging TOC with values and data - self.fakeParamToc = [] - self.fakeParamToc.append({'varid': 0, 'vartype': 0x08, - 'vargroup': 'blah', 'varname': 'p', - 'writable': True, 'value': 100}) - self.fakeParamToc.append({'varid': 1, 'vartype': 0x0A, - 'vargroup': 'info', 'varname': 'cid', - 'writable': False, 'value': 1234}) - self.fakeParamToc.append({'varid': 2, 'vartype': 0x06, - 'vargroup': 'rpid', 'varname': 'prp', - 'writable': True, 'value': 1.5}) - self.fakeParamToc.append({'varid': 3, 'vartype': 0x06, - 'vargroup': 'rpid', 'varname': 'pyaw', - 'writable': True, 'value': 2.5}) - self.fakeParamToc.append({'varid': 4, 'vartype': 0x06, - 'vargroup': 'rpid', 'varname': 'irp', - 'writable': True, 'value': 3.5}) - self.fakeParamToc.append({'varid': 5, 'vartype': 0x06, - 'vargroup': 'rpid', 'varname': 'iyaw', - 'writable': True, 'value': 4.5}) - self.fakeParamToc.append({'varid': 6, 'vartype': 0x06, - 'vargroup': 'pid_attitude', - 'varname': 'pitch_kd', 'writable': True, - 'value': 5.5}) - self.fakeParamToc.append({'varid': 7, 'vartype': 0x06, - 'vargroup': 'rpid', 'varname': 'dyaw', - 'writable': True, 'value': 6.5}) - self.fakeParamToc.append({'varid': 8, 'vartype': 0x06, - 'vargroup': 'apid', 'varname': 'prp', - 'writable': True, 'value': 7.5}) - self.fakeParamToc.append({'varid': 9, 'vartype': 0x06, - 'vargroup': 'apid', 'varname': 'pyaw', - 'writable': True, 'value': 8.5}) - self.fakeParamToc.append({'varid': 10, 'vartype': 0x06, - 'vargroup': 'apid', 'varname': 'irp', - 'writable': True, 'value': 9.5}) - self.fakeParamToc.append({'varid': 11, 'vartype': 0x06, - 'vargroup': 'apid', 'varname': 'iyaw', - 'writable': True, 'value': 10.5}) - self.fakeParamToc.append({'varid': 12, 'vartype': 0x06, - 'vargroup': 'apid', 'varname': 'drp', - 'writable': True, 'value': 11.5}) - self.fakeParamToc.append({'varid': 13, 'vartype': 0x06, - 'vargroup': 'apid', 'varname': 'dyaw', - 'writable': True, 'value': 12.5}) - self.fakeParamToc.append({'varid': 14, 'vartype': 0x08, - 'vargroup': 'flightctrl', - 'varname': 'xmode', 'writable': True, - 'value': 1}) - self.fakeParamToc.append({'varid': 15, 'vartype': 0x08, - 'vargroup': 'flightctrl', - 'varname': 'ratepid', 'writable': True, - 'value': 1}) - self.fakeParamToc.append({'varid': 16, 'vartype': 0x08, - 'vargroup': 'imu_sensors', - 'varname': 'HMC5883L', 'writable': False, - 'value': 1}) - self.fakeParamToc.append({'varid': 17, 'vartype': 0x08, - 'vargroup': 'imu_sensors', - 'varname': 'MS5611', 'writable': False, - 'value': 1}) - self.fakeParamToc.append({'varid': 18, 'vartype': 0x0A, - 'vargroup': 'firmware', - 'varname': 'revision0', 'writable': False, - 'value': 0xdeb}) - self.fakeParamToc.append({'varid': 19, 'vartype': 0x09, - 'vargroup': 'firmware', - 'varname': 'revision1', 'writable': False, - 'value': 0x99}) - self.fakeParamToc.append({'varid': 20, 'vartype': 0x08, - 'vargroup': 'firmware', - 'varname': 'modified', 'writable': False, - 'value': 1}) - self.fakeParamToc.append({'varid': 21, 'vartype': 0x08, - 'vargroup': 'imu_tests', - 'varname': 'MPU6050', 'writable': False, - 'value': 1}) - self.fakeParamToc.append({'varid': 22, 'vartype': 0x08, - 'vargroup': 'imu_tests', - 'varname': 'HMC5883L', 'writable': False, - 'value': 1}) - self.fakeParamToc.append({'varid': 23, 'vartype': 0x08, - 'vargroup': 'imu_tests', - 'varname': 'MS5611', 'writable': False, - 'value': 1}) - - self.fakeflash = {} - self._random_answer_delay = True - self.queue = queue.Queue() - self._packet_handler = _PacketHandlingThread(self.queue, - self.fakeLogToc, - self.fakeParamToc, - self._fake_mems) - self._packet_handler.start() - - def scan_interface(self, address): - return [['debug://0/0', 'Normal connection'], - ['debug://0/1', 'Fail to connect'], - ['debug://0/2', 'Incomplete log TOC download'], - ['debug://0/3', 'Insert random delays on replies'], - ['debug://0/4', - 'Insert random delays on replies and random TOC CRCs'], - ['debug://0/5', 'Normal but random TOC CRCs'], - ['debug://0/6', 'Normal but empty I2C and OW mems']] - - def get_status(self): - return 'Ok' - - def get_name(self): - return 'debug' - - def connect(self, uri, linkQualityCallback, linkErrorCallback): - - if not re.search('^debug://', uri): - raise WrongUriType('Not a debug URI') - - self._packet_handler.linkErrorCallback = linkErrorCallback - self._packet_handler.linkQualityCallback = linkQualityCallback - - # Debug-options for this driver that - # is set by using different connection URIs - self._packet_handler.inhibitAnswers = False - self._packet_handler.doIncompleteLogTOC = False - self._packet_handler.bootloader = False - self._packet_handler._random_answer_delay = False - self._packet_handler._random_toc_crcs = False - - if (re.search('^debug://.*/1$', uri)): - self._packet_handler.inhibitAnswers = True - if (re.search('^debug://.*/110$', uri)): - self._packet_handler.bootloader = True - if (re.search('^debug://.*/2$', uri)): - self._packet_handler.doIncompleteLogTOC = True - if (re.search('^debug://.*/3$', uri)): - self._packet_handler._random_answer_delay = True - if (re.search('^debug://.*/4$', uri)): - self._packet_handler._random_answer_delay = True - self._packet_handler._random_toc_crcs = True - if (re.search('^debug://.*/5$', uri)): - self._packet_handler._random_toc_crcs = True - - if len(self._fake_mems) == 0: - # Add empty EEPROM - self._fake_mems.append(FakeMemory(type=0, size=100, addr=0)) - # Add EEPROM with settings - self._fake_mems.append( - FakeMemory(type=0, size=100, addr=0, - data=[48, 120, 66, 67, 1, 8, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 231, 8, 231, 231, 231, 218])) - # Add 1-wire memory with settings for LED-ring - self._fake_mems.append( - FakeMemory(type=1, size=112, addr=0x1234567890ABCDEF, - data=[0xeb, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, - 0x44, 0x00, 0x0e, - 0x01, 0x09, 0x62, 0x63, 0x4c, 0x65, 0x64, - 0x52, 0x69, 0x6e, - 0x67, 0x02, 0x01, 0x62, 0x55])) - # Add 1-wire memory with settings for LED-ring but bad CRC - self._fake_mems.append( - FakeMemory(type=1, size=112, addr=0x1234567890ABCDEF, - data=[0xeb, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, - 0x44, 0x00, 0x0e, - 0x01, 0x09, 0x62, 0x63, 0x4c, 0x65, 0x64, - 0x52, 0x69, 0x6e, - 0x67, 0x02, 0x01, 0x62, 0x56])) - # Add empty 1-wire memory - self._fake_mems.append( - FakeMemory(type=1, size=112, addr=0x1234567890ABCDEE, - data=[0x00 for a in range(112)])) - - if (re.search('^debug://.*/6$', uri)): - logger.info('------------->Erasing memories on connect') - for m in self._fake_mems: - m.erase() - - self.fakeConsoleThread = None - - if (not self._packet_handler.inhibitAnswers and - not self._packet_handler.bootloader): - self.fakeConsoleThread = FakeConsoleThread(self.queue) - self.fakeConsoleThread.start() - - if (self._packet_handler.linkQualityCallback is not None): - self._packet_handler.linkQualityCallback(0) - - def receive_packet(self, time=0): - if time == 0: - try: - return self.queue.get(False) - except queue.Empty: - return None - elif time < 0: - try: - return self.queue.get(True) - except queue.Empty: - return None - else: - try: - return self.queue.get(True, time) - except queue.Empty: - return None - - def send_packet(self, pk): - self._packet_handler.handle_packet(pk) - - def close(self): - logger.info('Closing debugdriver') - for f in self._packet_handler.fakeLoggingThreads: - f.stop() - if self.fakeConsoleThread: - self.fakeConsoleThread.stop() - - -class _PacketHandlingThread(Thread): - """Thread for handling packets asynchronously""" - - def __init__(self, out_queue, fake_log_toc, fake_param_toc, fake_mems): - Thread.__init__(self) - self.setDaemon(True) - self.queue = out_queue - self.fakeLogToc = fake_log_toc - self.fakeParamToc = fake_param_toc - self._fake_mems = fake_mems - self._in_queue = queue.Queue() - - self.inhibitAnswers = False - self.doIncompleteLogTOC = False - self.bootloader = False - self._random_answer_delay = False - self._random_toc_crcs = False - - self.linkErrorCallback = None - self.linkQualityCallback = None - random.seed(None) - self.fakeLoggingThreads = [] - - self._added_blocks = [] - - self.nowAnswerCounter = 4 - - def handle_packet(self, pk): - self._in_queue.put(pk) - - def run(self): - while (True): - pk = self._in_queue.get(True) - if (self.inhibitAnswers): - self.nowAnswerCounter = self.nowAnswerCounter - 1 - logger.debug( - 'Not answering with any data, will send link errori' - ' in %d retries', self.nowAnswerCounter) - if (self.nowAnswerCounter == 0): - self.linkErrorCallback('Nothing is answering, and it' - " shouldn't") - else: - if (pk.port == 0xFF): - self._handle_bootloader(pk) - elif (pk.port == CRTPPort.DEBUGDRIVER): - self._handle_debugmessage(pk) - elif (pk.port == CRTPPort.COMMANDER): - pass - elif (pk.port == CRTPPort.LOGGING): - self._handle_logging(pk) - elif (pk.port == CRTPPort.PARAM): - self.handleParam(pk) - elif (pk.port == CRTPPort.MEM): - self._handle_mem_access(pk) - elif (pk.port == CRTPPort.LINKCTRL): - self._handle_link_ctrl(pk) - else: - logger.warning( - 'Not handling incoming packets on port [%d]', - pk.port) - - def _handle_link_ctrl(self, pk): - """ Handle request for protocol version """ - if pk.channel == 1: # LINKSERVICE_SOURCE - if pk.data[0] == 0: # Request protocol version - p_out = CRTPPacket() - p_out.set_header(CRTPPort.LINKCTRL, 1) - # Send protcol version 3. - # We send a version < 4 because debug driver does not yet - # support > 255 TOC entries. - p_out.data = (0, 3) - self._send_packet(p_out) - - def _handle_mem_access(self, pk): - chan = pk.channel - cmd = pk.data[0] - payload = pk.data[1:] - - if chan == 0: # Info channel - p_out = CRTPPacket() - p_out.set_header(CRTPPort.MEM, 0) - if cmd == 1: # Request number of memories - p_out.data = (1, len(self._fake_mems)) - if cmd == 2: - id = payload[0] - logger.info('Getting mem {}'.format(id)) - m = self._fake_mems[id] - p_out.data = struct.pack( - ' 1): - varIndex = pk.data[1] - logger.debug('TOC[%d]: Requesting ID=%d', pk.port, - varIndex) - else: - logger.debug('TOC[%d]: Requesting first index..surprise,' - ' it 0 !', pk.port) - - if (pk.port == CRTPPort.LOGGING): - entry = self.fakeLogToc[varIndex] - if (pk.port == CRTPPort.PARAM): - entry = self.fakeParamToc[varIndex] - - vartype = entry['vartype'] - if (pk.port == CRTPPort.PARAM and entry['writable'] is True): - vartype = vartype | (0x10) - - p.data = struct.pack(' 5') - - if (cmd == 1): # TOC CRC32 request - fakecrc = 0 - if (pk.port == CRTPPort.LOGGING): - tocLen = len(self.fakeLogToc) - fakecrc = 0xAAAAAAAA - if (pk.port == CRTPPort.PARAM): - tocLen = len(self.fakeParamToc) - fakecrc = 0xBBBBBBBB - - if self._random_toc_crcs: - fakecrc = int(''.join( - random.choice('ABCDEF' + string.digits) for x in - range(8)), 16) - logger.debug('Generated random TOC CRC: 0x%x', fakecrc) - logger.info('TOC[%d]: Requesting TOC CRC, sending back fake' - ' stuff: %d', pk.port, len(self.fakeLogToc)) - p = CRTPPacket() - p.set_header(pk.port, 0) - p.data = struct.pack(' 1): - logger.warning('LOG: Uplink packets with channels > 1 not' - ' supported!') - - def _send_packet(self, pk): - # Do not delay log data - if (self._random_answer_delay and pk.port != 0x05 and - pk.channel != 0x02): - # Calculate a delay between 0ms and 250ms - delay = random.randint(0, 250) / 1000.0 - logger.debug('Delaying answer %.2fms', delay * 1000) - time.sleep(delay) - self.queue.put(pk) - - -class _FakeLoggingDataThread(Thread): - """Thread that will send back fake logging data via CRTP""" - - def __init__(self, outQueue, blockId, listofvars, fakeLogToc): - Thread.__init__(self) - self.starttime = datetime.now() - self.outQueue = outQueue - self.setDaemon(True) - self.mod = 0 - self.blockId = blockId - self.period = 0 - self.listofvars = listofvars - self.shouldLog = False - self.fakeLogToc = fakeLogToc - self.fakeLoggingData = [] - self.setName('Fakelog block=%d' % blockId) - self.shouldQuit = False - - logging.info('FakeDataLoggingThread created for blockid=%d', blockId) - i = 0 - while (i < len(listofvars)): - varType = listofvars[i] - var_stored_as = (varType >> 8) - var_fetch_as = (varType & 0xFF) - if (var_stored_as > 0): - addr = struct.unpack('> 8) & 0x0ff, - (timestamp >> 16) & 0x0ff) # Timestamp - - for d in self.fakeLoggingData: - # Set new value - d[1] = d[1] + d[0]['mod'] * d[2] - # Obej the limitations - if (d[1] > d[0]['max']): - d[1] = d[0]['max'] # Limit value - d[2] = -1 # Switch direction - if (d[1] < d[0]['min']): - d[1] = d[0]['min'] # Limit value - d[2] = 1 # Switch direction - # Pack value - formatStr = LogTocElement.types[d[0]['vartype']][1] - p.data += struct.pack(formatStr, d[1]) - self.outQueue.put(p) - time.sleep(self.period / 1000.0) # Period in ms here - - -class FakeConsoleThread(Thread): - """Thread that will send back fake console data via CRTP""" - - def __init__(self, outQueue): - Thread.__init__(self) - self.outQueue = outQueue - self.setDaemon(True) - self._should_run = True - - def stop(self): - self._shoud_run = False - - def run(self): - # Temporary hack to test GPS from firmware by sending NMEA string - # on console - long_val = 0 - lat_val = 0 - alt_val = 0 - - while (self._should_run): - long_val += 1 - lat_val += 1 - alt_val += 1.0 - - long_string = '5536.677%d' % (long_val % 99) - lat_string = '01259.645%d' % (lat_val % 99) - alt_string = '%.1f' % (alt_val % 100.0) - - # Copy of what is sent from the module, but note that only - # the GPGGA message is being simulated, the others are fixed... - self._send_text('Time is now %s\n' % datetime.now()) - self._send_text('$GPVTG,,T,,M,0.386,N,0.716,K,A*2E\n') - self._send_text('$GPGGA,135544.0') - self._send_text('0,%s,N,%s,E,1,04,2.62,3.6,M,%s,M,,*58\n' % ( - long_string, lat_string, alt_string)) - self._send_text( - '$GPGSA,A,3,31,20,23,07,,,,,,,,,3.02,2.62,1.52*05\n') - self._send_text('$GPGSV,2,1,07,07,09,181,15,13,63,219,26,16,02,' - '097,,17,05,233,20*7E\n') - self._send_text( - '$GPGSV,2,2,07,20,42,119,35,23,77,097,27,31,12,032,19*47\n') - self._send_text( - '$GPGLL,5536.67734,N,01259.64578,E,135544.00,A,A*68\n') - - time.sleep(2) - - def _send_text(self, message): - p = CRTPPacket() - p.set_header(0, 0) - - # This might be done prettier ;-) - p.data = message - - self.outQueue.put(p) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 609d723ed..2f40a58f4 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -123,33 +123,14 @@ following methods: call(*args) """ Call the callbacks registered with the arguments args """ ``` - -### Debug driver - -The library contains a special link driver, named `DebugDriver`. This -driver will emulate a Crazyflie and is used for testing of the UI and -library. Normally this will be hidden from the user except if explicitly -enabled in the configuration file. The driver supports the following: - -- Connecting a download param and log TOCs -- Setting up log configurations and sending back fake data -- Setting and reading parameters -- Bootloading - -There are a number of different URIs that will be returned from the -driver. These will have different functions, like always returning a -random TOC CRC to always trigger TOC downloading or failing during -connect. The driver also has support for sending back data with random -delays to trigger random re-sending by the library. - ## Initiating the link drivers Before the library can be used the link drivers have to he initialized. This will search for available drivers and instantiate them. ``` {.python} - init_drivers(enable_debug_driver=False) - """ Search for and initialize link drivers. If enable_debug_driver is True then the DebugDriver will also be used.""" + init_drivers() + """ Search for and initialize link drivers.""" ``` ### Serial driver diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index 341b9df6d..8b4090012 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -72,8 +72,8 @@ This is the radio uri of the crazyflie, and currently displaying the default. It Write the following in the script: ``` if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -137,8 +137,8 @@ def simple_connect(): print("Now I will disconnect :'(") if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -180,7 +180,7 @@ logging.basicConfig(level=logging.ERROR) Now we are going to define the logging configuration. So add `lg_stab` in the `__main__` function : ``` ... - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) lg_stab.add_variable('stabilizer.roll', 'float') @@ -265,8 +265,8 @@ def simple_log(scf, logconf): ... if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) lg_stab.add_variable('stabilizer.roll', 'float') @@ -362,8 +362,8 @@ def simple_log_async(scf, logconf): (...) if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) lg_stab.add_variable('stabilizer.roll', 'float') @@ -490,8 +490,8 @@ def simple_connect(): ... if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) lg_stab.add_variable('stabilizer.roll', 'float') diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 9b34ce9d6..5e7b91747 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -32,7 +32,7 @@ logging.basicConfig(level=logging.ERROR) if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: ``` @@ -109,7 +109,7 @@ def param_deck_flow(name, value_str): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -196,7 +196,7 @@ def param_deck_flow(name, value_str): ... if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -280,7 +280,7 @@ def param_deck_flow(name, value_str): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -374,7 +374,7 @@ def param_deck_flow(name, value_str): ... if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -485,7 +485,7 @@ def param_deck_flow(name, value_str): ... if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -608,7 +608,7 @@ def param_deck_flow(name, value_str): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: diff --git a/examples/autonomousSequence.py b/examples/autonomousSequence.py index f4d37fac4..337723fd8 100644 --- a/examples/autonomousSequence.py +++ b/examples/autonomousSequence.py @@ -143,7 +143,7 @@ def run_sequence(scf, sequence): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: reset_estimator(scf) diff --git a/examples/autonomous_sequence_high_level.py b/examples/autonomous_sequence_high_level.py index e59ca0e80..3f81225d4 100644 --- a/examples/autonomous_sequence_high_level.py +++ b/examples/autonomous_sequence_high_level.py @@ -182,7 +182,7 @@ def run_sequence(cf, trajectory_id, duration): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py index 620600e11..2cb45f4fd 100644 --- a/examples/basicLedTiming.py +++ b/examples/basicLedTiming.py @@ -45,8 +45,8 @@ if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf diff --git a/examples/basicLedmemSync.py b/examples/basicLedmemSync.py index e8155331b..a4ecfc082 100644 --- a/examples/basicLedmemSync.py +++ b/examples/basicLedmemSync.py @@ -45,8 +45,8 @@ if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf diff --git a/examples/basicLedparamSync.py b/examples/basicLedparamSync.py index 654d88fb7..d424bb296 100644 --- a/examples/basicLedparamSync.py +++ b/examples/basicLedparamSync.py @@ -43,8 +43,8 @@ if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() with SyncCrazyflie(URI) as scf: cf = scf.cf diff --git a/examples/basiclog.py b/examples/basiclog.py index b761270df..ebfa04c0d 100644 --- a/examples/basiclog.py +++ b/examples/basiclog.py @@ -122,8 +122,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/basiclogSync.py b/examples/basiclogSync.py index e24765ef6..a5db5c8c0 100644 --- a/examples/basiclogSync.py +++ b/examples/basiclogSync.py @@ -43,8 +43,8 @@ if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/basicparam.py b/examples/basicparam.py index d2b76bf67..c9960e1ee 100644 --- a/examples/basicparam.py +++ b/examples/basicparam.py @@ -149,8 +149,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/cfbridge.py b/examples/cfbridge.py index 5456bdee7..5ecd76e63 100755 --- a/examples/cfbridge.py +++ b/examples/cfbridge.py @@ -112,10 +112,10 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) + # Initialize the low-level drivers cflib.crtp.radiodriver.set_retries_before_disconnect(1500) cflib.crtp.radiodriver.set_retries(3) - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') if len(sys.argv) > 2: diff --git a/examples/erase-ow.py b/examples/erase-ow.py index d8a66d2d2..da4b80e98 100644 --- a/examples/erase-ow.py +++ b/examples/erase-ow.py @@ -124,8 +124,8 @@ def _disconnected(self, link_uri): print('This example will not work with the BLE version of the nRF51' ' firmware (flashed on production units). See https://github.com' '/bitcraze/crazyflie-clients-python/issues/166') - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/flash-memory.py b/examples/flash-memory.py index ff7551464..d43e7c721 100644 --- a/examples/flash-memory.py +++ b/examples/flash-memory.py @@ -135,7 +135,7 @@ def scan(): """ # Initiate the low level drivers - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() # Scan for Crazyflies print('Scanning interfaces for Crazyflies...') diff --git a/examples/flowsequenceSync.py b/examples/flowsequenceSync.py index fb88be857..1e83c74b7 100644 --- a/examples/flowsequenceSync.py +++ b/examples/flowsequenceSync.py @@ -44,8 +44,8 @@ if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py index bbba813c2..63ee8ec7e 100644 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ b/examples/lighthouse/lighthouse_openvr_grab.py @@ -167,7 +167,7 @@ def run_sequence(scf): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: reset_estimator(scf) diff --git a/examples/lighthouse/lighthouse_openvr_grab_color.py b/examples/lighthouse/lighthouse_openvr_grab_color.py index 9c5a4bfdf..765b06be0 100644 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ b/examples/lighthouse/lighthouse_openvr_grab_color.py @@ -198,7 +198,7 @@ def run_sequence(scf): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: reset_estimator(scf) diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index ec408cabe..a1fa69a05 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -193,7 +193,7 @@ def run_sequence(scf0, scf1): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(uri0, cf=Crazyflie(rw_cache='./cache')) as scf0: reset_estimator(scf0) diff --git a/examples/lighthouse/read_lighthouse_mem.py b/examples/lighthouse/read_lighthouse_mem.py index 20bd09b7d..23b881964 100644 --- a/examples/lighthouse/read_lighthouse_mem.py +++ b/examples/lighthouse/read_lighthouse_mem.py @@ -71,7 +71,7 @@ def _calib_read_ready(self, calib_data): # URI to the Crazyflie to connect to uri = 'radio://0/80' - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # 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 index b0fc5d12d..1db49669f 100644 --- a/examples/lighthouse/write_lighthouse_mem.py +++ b/examples/lighthouse/write_lighthouse_mem.py @@ -67,8 +67,8 @@ def _data_written(self, success): # URI to the Crazyflie to connect to uri = 'radio://0/80' - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() bs1geo = LighthouseBsGeometry() bs1geo.origin = [1.0, 2.0, 3.0] diff --git a/examples/lps_reboot_to_bootloader.py b/examples/lps_reboot_to_bootloader.py index 3dd4f17be..c0eb80fdc 100644 --- a/examples/lps_reboot_to_bootloader.py +++ b/examples/lps_reboot_to_bootloader.py @@ -93,8 +93,8 @@ def _reboot_thread(self): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/motion_commander_demo.py b/examples/motion_commander_demo.py index 6040d1ffc..d204d6f36 100644 --- a/examples/motion_commander_demo.py +++ b/examples/motion_commander_demo.py @@ -49,8 +49,8 @@ if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: # We take off when the commander is created diff --git a/examples/multiramp.py b/examples/multiramp.py index 076820c5e..c1f29a9e0 100644 --- a/examples/multiramp.py +++ b/examples/multiramp.py @@ -107,8 +107,8 @@ def _ramp_motors(self): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # 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') diff --git a/examples/multiranger_pointcloud.py b/examples/multiranger_pointcloud.py index a753a5118..b97940a6f 100644 --- a/examples/multiranger_pointcloud.py +++ b/examples/multiranger_pointcloud.py @@ -102,7 +102,7 @@ def __init__(self, URI): self.setCentralWidget(self.canvas.native) - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() self.cf = Crazyflie(ro_cache=None, rw_cache='cache') # Connect callbacks from the Crazyflie API diff --git a/examples/multiranger_push.py b/examples/multiranger_push.py index d52f774ac..a4ffb080a 100755 --- a/examples/multiranger_push.py +++ b/examples/multiranger_push.py @@ -70,8 +70,8 @@ def is_close(range): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: diff --git a/examples/position_commander_demo.py b/examples/position_commander_demo.py index d2e1adfaa..36a26003f 100644 --- a/examples/position_commander_demo.py +++ b/examples/position_commander_demo.py @@ -76,7 +76,7 @@ def simple_sequence(): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() simple_sequence() # slightly_more_complex_usage() diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index 29ed5bfa2..800f7dd01 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -137,7 +137,7 @@ def run_sequence(scf, sequence, base_x, base_y, base_z, yaw): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() # Set these to the position and yaw based on how your Crazyflie is placed # on the floor diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index b4fd15bca..77314dee5 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -106,7 +106,7 @@ def matrix_print(cf, pc, image_def): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() image_def = ImageDef('monalisa.png') diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 5d95e03cf..62c72090f 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -291,7 +291,7 @@ def run_sequence(cf, trajectory_id, duration): commander.stop() -cflib.crtp.init_drivers(enable_debug_driver=False) +cflib.crtp.init_drivers() # Connect to QTM qtm_wrapper = QtmWrapper(ridgid_body_name) diff --git a/examples/ramp.py b/examples/ramp.py index d5d9808da..11fd106dd 100644 --- a/examples/ramp.py +++ b/examples/ramp.py @@ -102,8 +102,8 @@ def _ramp_motors(self): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/read-eeprom.py b/examples/read-eeprom.py index 3800ecef2..c56ead011 100644 --- a/examples/read-eeprom.py +++ b/examples/read-eeprom.py @@ -115,8 +115,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/read-ow.py b/examples/read-ow.py index c38a733fd..79e9d409f 100644 --- a/examples/read-ow.py +++ b/examples/read-ow.py @@ -122,8 +122,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index 25f0588f4..f94a79c5f 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -93,7 +93,7 @@ def read_failed(self, addr): # URI to the Crazyflie to connect to uri = 'radio://0/80' - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() ReadMem(uri) diff --git a/examples/scan.py b/examples/scan.py index 06e0c8143..fe7220790 100644 --- a/examples/scan.py +++ b/examples/scan.py @@ -29,7 +29,7 @@ import cflib.crtp # Initiate the low level drivers -cflib.crtp.init_drivers(enable_debug_driver=False) +cflib.crtp.init_drivers() print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/step-by-step/sbs_connect_log_param.py b/examples/step-by-step/sbs_connect_log_param.py index ed888c93c..45536a018 100644 --- a/examples/step-by-step/sbs_connect_log_param.py +++ b/examples/step-by-step/sbs_connect_log_param.py @@ -92,8 +92,8 @@ def simple_connect(): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) lg_stab.add_variable('stabilizer.roll', 'float') diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index aa0f44efb..e35a86110 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -108,7 +108,7 @@ def param_deck_flow(name, value_str): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index d13c50846..c8493c5aa 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -137,7 +137,7 @@ def run_shared_sequence(scf): } if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(activate_high_level_commander) diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py index 2b442987a..a18916918 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -276,7 +276,7 @@ def run_sequence(scf, sequence): if __name__ == '__main__': # logging.basicConfig(level=logging.DEBUG) - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index 64509a85e..7c0522efe 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -142,7 +142,7 @@ def run_sequence(scf, params): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index fe97e13ad..c29e41a65 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -235,7 +235,7 @@ def control_thread(): if __name__ == '__main__': controlQueues = [Queue() for _ in range(len(uris))] - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(activate_high_level_commander) diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index e8ce3eb80..7d558c04f 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -386,7 +386,7 @@ def wait_for_position_estimator(scf): root = tk.Tk() pid_gui = TunerGUI(root) - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf) as scf: wait_for_position_estimator(scf) diff --git a/examples/write-eeprom.py b/examples/write-eeprom.py index 1c6efe249..ebfcb82fb 100644 --- a/examples/write-eeprom.py +++ b/examples/write-eeprom.py @@ -127,8 +127,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/examples/write-ow.py b/examples/write-ow.py index 63658dd18..f4462d200 100644 --- a/examples/write-ow.py +++ b/examples/write-ow.py @@ -137,8 +137,8 @@ def _disconnected(self, link_uri): ' firmware (flashed on production units). See https://github.com' '/bitcraze/crazyflie-clients-python/issues/166') - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + # Initialize the low-level drivers + cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() diff --git a/sys_test/single_cf_grounded/single_cf_grounded.py b/sys_test/single_cf_grounded/single_cf_grounded.py index aec105300..7ab9eae1b 100644 --- a/sys_test/single_cf_grounded/single_cf_grounded.py +++ b/sys_test/single_cf_grounded/single_cf_grounded.py @@ -30,7 +30,7 @@ class TestSingleCfGrounded(unittest.TestCase): def setUp(self): - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() self.radioUri = 'radio://0/80/2M/E7E7E7E7E7' self.usbUri = 'usb://0' diff --git a/sys_test/swarm_test_rig/test_connection.py b/sys_test/swarm_test_rig/test_connection.py index 14c4b2117..0df6dce98 100644 --- a/sys_test/swarm_test_rig/test_connection.py +++ b/sys_test/swarm_test_rig/test_connection.py @@ -34,7 +34,7 @@ class TestConnection(unittest.TestCase): def setUp(self): - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() self.test_rig_support = RigSupport() def test_that_connection_time_scales_with_more_devices_without_cache(self): diff --git a/sys_test/swarm_test_rig/test_logging.py b/sys_test/swarm_test_rig/test_logging.py index 839058cc7..2226f45b2 100644 --- a/sys_test/swarm_test_rig/test_logging.py +++ b/sys_test/swarm_test_rig/test_logging.py @@ -35,7 +35,7 @@ class TestLogging(unittest.TestCase): def setUp(self): - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() self.test_rig_support = RigSupport() def test_that_requested_logging_is_received_properly_from_one_cf(self): diff --git a/sys_test/swarm_test_rig/test_memory_map.py b/sys_test/swarm_test_rig/test_memory_map.py index 873b8bfcf..5150c76c5 100644 --- a/sys_test/swarm_test_rig/test_memory_map.py +++ b/sys_test/swarm_test_rig/test_memory_map.py @@ -37,7 +37,7 @@ class TestMemoryMapping(unittest.TestCase): def setUp(self): - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() self.test_rig_support = RigSupport() def test_memory_mapping_with_one_cf(self): diff --git a/sys_test/swarm_test_rig/test_response_time.py b/sys_test/swarm_test_rig/test_response_time.py index f00378464..ef9bdbe13 100644 --- a/sys_test/swarm_test_rig/test_response_time.py +++ b/sys_test/swarm_test_rig/test_response_time.py @@ -34,7 +34,7 @@ class TestResponseTime(unittest.TestCase): ECHO = 0 def setUp(self): - cflib.crtp.init_drivers(enable_debug_driver=False) + cflib.crtp.init_drivers() self.test_rig_support = RigSupport() self.links = [] From 100aea30ea966f4c5d3b9580b368c4f1373071d8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 16 Mar 2021 11:11:29 +0100 Subject: [PATCH 151/861] Add delay to system write process to reduce risk of race condition --- cflib/localization/lighthouse_config_manager.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index 3e67f1354..f0b4e1d39 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -22,6 +22,8 @@ """ Functionality to manage lighthouse system configuration (geometry and calibration data). """ +import time + import yaml from cflib.crazyflie.mem import LighthouseBsCalibration @@ -144,11 +146,16 @@ def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, system_ self._write_failed_for_one_or_more_objects = False # Change system type first as this will erase calib and geo data in the CF. - # Changing system type may trigger a lengthy operation if the persistant memory write takes a long time. + # Changing system type may trigger a lengthy operation (up to 0.5 s) if the persistant memory requires defrag. # Setting a param is an asynchronous operataion, and it is not possible to know if the system swich is finished - # before we continue. Is it possible that this will cause problems? + # before we continue. self._cf.param.set_value('lighthouse.systemType', system_type) + # We add a sleep here to make sure the change of system type is finished. It is dirty but will have to do for + # now. A more propper solution would be to add support for Remote Procedure Calls (RPC) with synchronous + # function calls. + time.sleep(0.8) + self._next() def write_and_store_config_from_file(self, data_stored_cb, file_name): From 01125cb315548f2959b60335af0a013ddc74416a Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 17 Mar 2021 14:07:10 +0100 Subject: [PATCH 152/861] Use constants instead of numbers for System type --- cflib/localization/lighthouse_config_manager.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index f0b4e1d39..b792e682a 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -118,7 +118,8 @@ def __init__(self, cf, nr_of_base_stations=16): self._write_failed_for_one_or_more_objects = False self._nr_of_base_stations = nr_of_base_stations - def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, system_type=2): + def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, + system_type=LighthouseConfigFileManager.SYSTEM_TYPE_V2): """ Transfer geometry and calibration data to the Crazyflie and persist to permanent storage. The callback is called when done. From 366593451c55f4a6380535d0797737a4dafe8543 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 17 Mar 2021 14:13:46 +0100 Subject: [PATCH 153/861] Update version to 0.1.14 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 532155e88..3bee383d1 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.13.1', + version='0.1.14', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 2c000a8837977fc8d12ae82c0202d0a27d8289e4 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 18 Mar 2021 16:14:35 +0100 Subject: [PATCH 154/861] Make system type in LighthouseConfigWriter.write_and_store_config() optional and None by default. This change will enable clients to chose which data to write to the CF: geo, calib and system type. If system_type==None the system type will not be updated --- .../localization/lighthouse_config_manager.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index b792e682a..64039b38e 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -118,8 +118,7 @@ def __init__(self, cf, nr_of_base_stations=16): self._write_failed_for_one_or_more_objects = False self._nr_of_base_stations = nr_of_base_stations - def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, - system_type=LighthouseConfigFileManager.SYSTEM_TYPE_V2): + def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, system_type=None): """ Transfer geometry and calibration data to the Crazyflie and persist to permanent storage. The callback is called when done. @@ -146,16 +145,17 @@ def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, self._write_failed_for_one_or_more_objects = False - # Change system type first as this will erase calib and geo data in the CF. - # Changing system type may trigger a lengthy operation (up to 0.5 s) if the persistant memory requires defrag. - # Setting a param is an asynchronous operataion, and it is not possible to know if the system swich is finished - # before we continue. - self._cf.param.set_value('lighthouse.systemType', system_type) - - # We add a sleep here to make sure the change of system type is finished. It is dirty but will have to do for - # now. A more propper solution would be to add support for Remote Procedure Calls (RPC) with synchronous - # function calls. - time.sleep(0.8) + if system_type is not None: + # Change system type first as this will erase calib and geo data in the CF. + # Changing system type may trigger a lengthy operation (up to 0.5 s) if the persistant memory requires defrag. + # Setting a param is an asynchronous operataion, and it is not possible to know if the system swich is finished + # before we continue. + self._cf.param.set_value('lighthouse.systemType', system_type) + + # We add a sleep here to make sure the change of system type is finished. It is dirty but will have to do for + # now. A more propper solution would be to add support for Remote Procedure Calls (RPC) with synchronous + # function calls. + time.sleep(0.8) self._next() From 33b4521575f45572f5acb346ee76a699808690c6 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 18 Mar 2021 16:25:10 +0100 Subject: [PATCH 155/861] Styling --- cflib/localization/lighthouse_config_manager.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index 64039b38e..d5204ea4b 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -147,14 +147,14 @@ def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, system_ if system_type is not None: # Change system type first as this will erase calib and geo data in the CF. - # Changing system type may trigger a lengthy operation (up to 0.5 s) if the persistant memory requires defrag. - # Setting a param is an asynchronous operataion, and it is not possible to know if the system swich is finished - # before we continue. + # Changing system type may trigger a lengthy operation (up to 0.5 s) if the persistant memory requires + # defrag. Setting a param is an asynchronous operataion, and it is not possible to know if the system + # swich is finished before we continue. self._cf.param.set_value('lighthouse.systemType', system_type) - # We add a sleep here to make sure the change of system type is finished. It is dirty but will have to do for - # now. A more propper solution would be to add support for Remote Procedure Calls (RPC) with synchronous - # function calls. + # We add a sleep here to make sure the change of system type is finished. It is dirty but will have to + # do for now. A more propper solution would be to add support for Remote Procedure Calls (RPC) with + # synchronous function calls. time.sleep(0.8) self._next() From 69b25ca78bb9054f99abc921c7cde85f2290e185 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 18 Mar 2021 16:36:27 +0100 Subject: [PATCH 156/861] Update version to 0.1.14.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 3bee383d1..5e1ff05f2 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.14', + version='0.1.14.1', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 3876f9b44be47b36824777bf0deaefc6dd8b793f Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 22 Mar 2021 16:25:30 +0100 Subject: [PATCH 157/861] Fix regression where power_switch does not work with Python backend This was introduced in 730aea39b321ddb78dfbfd6853563c1e7e134167, where the Python backend accepted the URI options. For the power switch, we still need an additional check to enable the old code path using the shared radio. --- cflib/utils/power_switch.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index cbb8a3fd4..671f3b3fa 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -39,13 +39,16 @@ def __init__(self, uri): self.uri = uri uri_augmented = uri+'?safelink=0&autoping=0&ackfilter=0' self.link = cflib.crtp.get_link_driver(uri_augmented) - # Switch to legacy mode, if uri options are not available - if not self.link: + # Switch to legacy mode, if uri options are not available or old Python backend is used + if not self.link or self.link.get_name() == 'radio': uri_parts = cflib.crtp.RadioDriver.parse_uri(uri) self.devid = uri_parts[0] self.channel = uri_parts[1] self.datarate = uri_parts[2] self.address = uri_parts[3] + if self.link: + self.link.close() + self.link = None def platform_power_down(self): """ Power down the platform, both NRF and STM MCUs. From 6cb0e0376446f9e98fd278ffeccef1d510c36669 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 23 Mar 2021 09:43:17 +0100 Subject: [PATCH 158/861] Add emergency stop and emergency stop watchdog support. Emergency stop packets were added to the firmware May 2017 (commit 1e9c08c0ffc942dd0999f8b8dbbecfdfec2a84c7, firmware issue 226), but they are not supported in the lib, yet. This change adds send_emergency_stop and send_emergency_stop_watchdog to the Localization class. --- cflib/crazyflie/localization.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 0b4c8ae3b..9d0ce8367 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -169,6 +169,28 @@ def send_short_lpp_packet(self, dest_id, data): pk.data = struct.pack(' Date: Tue, 23 Mar 2021 13:49:16 +0100 Subject: [PATCH 159/861] Update version to 0.1.14.2 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 5e1ff05f2..5869f327e 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.14.1', + version='0.1.14.2', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 887981df1e5e9f4c9f69ce938de40ebc22d12e30 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 23 Mar 2021 14:06:59 +0100 Subject: [PATCH 160/861] (#143) changed name of install_from_source.md to install.md --- docs/installation/{install_from_source.md => install.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename docs/installation/{install_from_source.md => install.md} (100%) diff --git a/docs/installation/install_from_source.md b/docs/installation/install.md similarity index 100% rename from docs/installation/install_from_source.md rename to docs/installation/install.md From 3dd443c5bceb86d7e84faddd61bb3716bc900cbd Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 23 Mar 2021 14:11:54 +0100 Subject: [PATCH 161/861] examples: basicLedTiming: Do not crash on missing LED ring --- examples/basicLedTiming.py | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py index 2cb45f4fd..47553b37a 100644 --- a/examples/basicLedTiming.py +++ b/examples/basicLedTiming.py @@ -53,16 +53,19 @@ # Get LED memory and write to it mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LEDTIMING) - 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) + 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') From 719b1655d7145a5de8dd13a64c2306eda814d12f Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 23 Mar 2021 14:29:30 +0100 Subject: [PATCH 162/861] (#143) copied content of readme into install.md and replace with link --- README.md | 108 ++--------------------------------- docs/_data/menu.yml | 2 +- docs/installation/install.md | 76 ++++++++++++++++++++++-- 3 files changed, 78 insertions(+), 108 deletions(-) diff --git a/README.md b/README.md index 216af2049..155b2de30 100644 --- a/README.md +++ b/README.md @@ -2,114 +2,18 @@ cflib is an API written in Python that is used to communicate with the Crazyflie and Crazyflie 2.0 quadcopters. It is intended to be used by client software to -communicate with and control a Crazyflie quadcopter. For instance the [cfclient][cfclient] Crazyflie PC client uses the cflib. +communicate with and control a Crazyflie quadcopter. For instance the [Crazyflie PC client](https://www.github.com/bitcraze/crazyflie-clients-python) uses the cflib. See [below](#platform-notes) for platform specific instruction. For more info see our [documentation](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/). +## Installation +See the [installation instructions](docs/installation/install.md) in the github docs folder. -## Development -### Developing for the cfclient -* [Fork the cflib](https://help.github.com/articles/fork-a-repo/) -* [Clone the cflib](https://help.github.com/articles/cloning-a-repository/), `git clone git@github.com:YOUR-USERNAME/crazyflie-lib-python.git` -* [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` - - -* [Uninstall the cflib if you don't want it any more](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib` - -Note: If you are developing for the [cfclient][cfclient] you must use python3. On Ubuntu (16.04, 18.08) use `pip3` instead of `pip`. - -### Linux, OSX, Windows - -The following should be executed in the root of the crazyflie-lib-python file tree. - -#### Virtualenv -This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) -with package dependencies. If you don't want to use virualenv and don't mind installing cflib dependencies system-wide -you can skip this section. - -* Install virtualenv: `pip install virtualenv` -* create an environment: `virtualenv venv` -* Activate the environment: `source venv/bin/activate` - - -* To deactivate the virtualenv when you are done using it `deactivate` - -Note: For systems that support [make](https://www.gnu.org/software/make/manual/html_node/Simple-Makefile.html), you can use `make venv` to -create an environment, activate it and install dependencies. - -#### Install cflib dependencies -Install dependencies required by the lib: `pip install -r requirements.txt`. If you are planning on developing on the lib you should also run: `pip install -r requirements-dev.txt`. - -To verify the installation, connect the crazyflie and run an example: `python examples/basiclog` - -### Pre commit hooks -If you want some extra help with keeping to the mandated python coding style you can install hooks that verify your style at commit time. This is done by running: -``` -$ pre-commit install -``` -This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes and alert you if you need to change anything. - -## Testing -### With docker and the toolbelt - -For information and installation of the -[toolbelt.](https://wiki.bitcraze.io/projects:dockerbuilderimage:index) - -* Check to see if you pass tests: `tb test` -* Check to see if you pass style guidelines: `tb verify` - -Note: Docker and the toolbelt is an optional way of running tests and reduces the -work needed to maintain your python environment. - -### Native python on Linux, OSX, Windows - [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` -* If test fails after installing tox with `pip install tox`, installing with `sudo apt-get install tox`result a successful test run - -* Test package in python3.4 `TOXENV=py34 tox` -* Test package in python3.6 `TOXENV=py36 tox` - -Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python 3.3, 3.4 and would require all python versions to be installed on the machine.) - - -## Platform notes - -### Linux - -#### Setting udev permissions - -The following steps make it possible to use the USB Radio without being root. - -``` -sudo groupadd plugdev -sudo usermod -a -G plugdev $USER -``` - -You will need to log out and log in again in order to be a member of the plugdev group. - -Create a file named ```/etc/udev/rules.d/99-crazyradio.rules``` and add the -following: -``` -# Crazyradio (normal operation) -SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="7777", MODE="0664", GROUP="plugdev" -# Bootloader -SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="0101", MODE="0664", GROUP="plugdev" -``` - -To connect Crazyflie 2.0 via usb, create a file name ```/etc/udev/rules.d/99-crazyflie.rules``` and add the following: -``` -SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE="0664", GROUP="plugdev" -``` - -You can reload the udev-rules using the following: -``` -sudo udevadm control --reload-rules -sudo udevadm trigger -``` - -[cfclient]: https://www.github.com/bitcraze/crazyflie-clients-python +## Official Documentation +Check out the [Bitcraze crazyflie-lib-python documentation](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/) on our website. ## Contribute -Everyone is encouraged to contribute to the CrazyFlie library by forking the Github repository and making a pull request or opening an issue. +Everyone is encouraged to contribute to the CrazyFlie library by forking the Github repository and making a pull request or opening an issue. \ No newline at end of file diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index a17ece329..465157d7f 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -1,7 +1,7 @@ - page_id: home - title: Installation subs: - - {page_id: install_from_source} + - {page_id: install} - {page_id: usd_permissions} - title: User guides subs: diff --git a/docs/installation/install.md b/docs/installation/install.md index 88c7ca297..4ceea710b 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -1,7 +1,9 @@ --- -title: Installing from source -page_id: install_from_source +title: Installation +page_id: install --- + +## Development ### Developing for the cfclient * [Fork the cflib](https://help.github.com/articles/fork-a-repo/) * [Clone the cflib](https://help.github.com/articles/cloning-a-repository/), `git clone git@github.com:YOUR-USERNAME/crazyflie-lib-python.git` @@ -32,10 +34,74 @@ Note: For systems that support [make](https://www.gnu.org/software/make/manual/h create an environment, activate it and install dependencies. #### Install cflib dependencies -Install dependencies required by the lib: `pip install -r requirements.txt` +Install dependencies required by the lib: `pip install -r requirements.txt`. If you are planning on developing on the lib you should also run: `pip install -r requirements-dev.txt`. To verify the installation, connect the crazyflie and run an example: `python examples/basiclog` -#### Install serial driver dependencies +### Pre commit hooks +If you want some extra help with keeping to the mandated python coding style you can install hooks that verify your style at commit time. This is done by running: +``` +$ pre-commit install +``` +This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes and alert you if you need to change anything. + +## Testing +### With docker and the toolbelt + +For information and installation of the +[toolbelt.](https://wiki.bitcraze.io/projects:dockerbuilderimage:index) + +* Check to see if you pass tests: `tb test` +* Check to see if you pass style guidelines: `tb verify` + +Note: Docker and the toolbelt is an optional way of running tests and reduces the +work needed to maintain your python environment. + +### Native python on Linux, OSX, Windows + [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` +* If test fails after installing tox with `pip install tox`, installing with `sudo apt-get install tox`result a successful test run + +* Test package in python3.4 `TOXENV=py34 tox` +* Test package in python3.6 `TOXENV=py36 tox` + +Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python 3.3, 3.4 and would require all python versions to be installed on the machine.) + + +## Platform notes + +### Linux + +#### Setting udev permissions + +The following steps make it possible to use the USB Radio without being root. + +``` +sudo groupadd plugdev +sudo usermod -a -G plugdev $USER +``` + +You will need to log out and log in again in order to be a member of the plugdev group. + +Create a file named ```/etc/udev/rules.d/99-crazyradio.rules``` and add the +following: +``` +# Crazyradio (normal operation) +SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="7777", MODE="0664", GROUP="plugdev" +# Bootloader +SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="0101", MODE="0664", GROUP="plugdev" +``` + +To connect Crazyflie 2.0 via usb, create a file name ```/etc/udev/rules.d/99-crazyflie.rules``` and add the following: +``` +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE="0664", GROUP="plugdev" +``` + +You can reload the udev-rules using the following: +``` +sudo udevadm control --reload-rules +sudo udevadm trigger +``` + +[cfclient]: https://www.github.com/bitcraze/crazyflie-clients-python + -To use the serial driver, pyserial must be installed: `pip install pyserial` From b56334315f89013755025782383ff154a8e51198 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 23 Mar 2021 14:30:29 +0100 Subject: [PATCH 163/861] (#143) removed old mentions of cfclient --- docs/installation/install.md | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 4ceea710b..c66fea499 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -4,7 +4,7 @@ page_id: install --- ## Development -### Developing for the cfclient +### Developing for the cflib * [Fork the cflib](https://help.github.com/articles/fork-a-repo/) * [Clone the cflib](https://help.github.com/articles/cloning-a-repository/), `git clone git@github.com:YOUR-USERNAME/crazyflie-lib-python.git` * [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` @@ -12,7 +12,7 @@ page_id: install * [Uninstall the cflib if you don't want it any more](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib` -Note: If you are developing for the [cfclient][cfclient] you must use python3. On Ubuntu (16.04, 18.08) use `pip3` instead of `pip`. +Note: If you are developing for the cflib you must use python3. On Ubuntu (16.04, 18.08) use `pip3` instead of `pip`. ### Linux, OSX, Windows @@ -102,6 +102,5 @@ sudo udevadm control --reload-rules sudo udevadm trigger ``` -[cfclient]: https://www.github.com/bitcraze/crazyflie-clients-python From df6297230baa144a19208299fb0774da8363ee89 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 23 Mar 2021 19:23:09 +0100 Subject: [PATCH 164/861] (#143) fixed CI problem --- README.md | 2 +- docs/installation/install.md | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/README.md b/README.md index 155b2de30..80dd87be6 100644 --- a/README.md +++ b/README.md @@ -16,4 +16,4 @@ Check out the [Bitcraze crazyflie-lib-python documentation](https://www.bitcraze ## Contribute -Everyone is encouraged to contribute to the CrazyFlie library by forking the Github repository and making a pull request or opening an issue. \ No newline at end of file +Everyone is encouraged to contribute to the CrazyFlie library by forking the Github repository and making a pull request or opening an issue. diff --git a/docs/installation/install.md b/docs/installation/install.md index c66fea499..e9f0003b7 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -101,6 +101,3 @@ You can reload the udev-rules using the following: sudo udevadm control --reload-rules sudo udevadm trigger ``` - - - From 3be9a1a3ce64b6c2a15b68b43bf0577289e0a0c9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 24 Mar 2021 10:40:16 +0100 Subject: [PATCH 165/861] System tests: improve documentation and reporting * Add Readme that explains how to run the system tests. * Update firmware from release zip, rather than separate bin files. * Enable verbose output for sys-test script. --- sys_test/swarm_test_rig/README.md | 29 ++++++++++++++++++++++++ sys_test/swarm_test_rig/cload_all.sh | 14 ++++++++++++ sys_test/swarm_test_rig/cload_all_nrf.sh | 14 ------------ sys_test/swarm_test_rig/cload_all_stm.sh | 14 ------------ tools/build/sys-test | 2 +- 5 files changed, 44 insertions(+), 29 deletions(-) create mode 100644 sys_test/swarm_test_rig/README.md create mode 100755 sys_test/swarm_test_rig/cload_all.sh delete mode 100755 sys_test/swarm_test_rig/cload_all_nrf.sh delete mode 100755 sys_test/swarm_test_rig/cload_all_stm.sh diff --git a/sys_test/swarm_test_rig/README.md b/sys_test/swarm_test_rig/README.md new file mode 100644 index 000000000..8849a0bd3 --- /dev/null +++ b/sys_test/swarm_test_rig/README.md @@ -0,0 +1,29 @@ +# Tests using the Roadrunner Testrig + +## Preparation + +* Power Roadrunner Testrig +* attach a Crazyradio (PA) to PC +* Flash the firmware using `./cload_all.sh .zip>` + +## Execute Tests + +All tests: + +``` +python3 -m unittest discover . -v +``` + +There is also a script in `tools/build/sys-test` that essentially does that. + +A single test file, e.g.: + +``` +python3 test_connection.py +``` + +A concrete test case, e.g.: + +``` +python3 test_connection.py TestConnection.test_that_connection_time_scales_with_more_devices_without_cache +``` diff --git a/sys_test/swarm_test_rig/cload_all.sh b/sys_test/swarm_test_rig/cload_all.sh new file mode 100755 index 000000000..f5515e705 --- /dev/null +++ b/sys_test/swarm_test_rig/cload_all.sh @@ -0,0 +1,14 @@ +#!/usr/bin/env bash + +file=$1 + +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74201 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74202 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74203 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74204 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74205 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74206 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74207 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74208 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E74209 +python3 -m cfloader flash $file nrf51-fw stm32-fw -w radio://0/42/2M/E7E7E7420A diff --git a/sys_test/swarm_test_rig/cload_all_nrf.sh b/sys_test/swarm_test_rig/cload_all_nrf.sh deleted file mode 100755 index 7feabb0e8..000000000 --- a/sys_test/swarm_test_rig/cload_all_nrf.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env bash - -file=$1 - -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74201 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74202 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74203 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74204 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74205 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74206 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74207 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74208 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E74209 -python3 -m cfloader flash $file nrf51-fw -w radio://0/42/2M/E7E7E7420A diff --git a/sys_test/swarm_test_rig/cload_all_stm.sh b/sys_test/swarm_test_rig/cload_all_stm.sh deleted file mode 100755 index 01f837f04..000000000 --- a/sys_test/swarm_test_rig/cload_all_stm.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env bash - -file=$1 - -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74201 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74202 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74203 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74204 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74205 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74206 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74207 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74208 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E74209 -python3 -m cfloader flash $file stm32-fw -w radio://0/42/2M/E7E7E7420A diff --git a/tools/build/sys-test b/tools/build/sys-test index fd61ee99e..5ba60f83d 100755 --- a/tools/build/sys-test +++ b/tools/build/sys-test @@ -8,7 +8,7 @@ try: root = _path.normpath(_path.join(script_dir, '../../sys_test')) print('**** Running sys tests ****') - subprocess.check_call(['python3', '-m', 'unittest', 'discover', root]) + subprocess.check_call(['python3', '-m', 'unittest', 'discover', root, '-v']) print('Unit tests pass') except subprocess.CalledProcessError as e: From 228b7d19342dd80ffee2c3a14421c81f7474ba78 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 23 Mar 2021 14:28:15 +0100 Subject: [PATCH 166/861] Qualisys example: add support to only send position estimate. --- examples/qualisys/qualisys_hl_commander.py | 50 ++++++++++++---------- 1 file changed, 27 insertions(+), 23 deletions(-) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 62c72090f..a2057263b 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -25,10 +25,7 @@ figure 8. Set the uri to the radio settings of the Crazyflie and modify the -ridgid_body_name to match the name of the Crazyflie in QTM. - -Limitations: This script does not support full pose and the Crazyflie -must be started facing positive X. +rigid_body_name to match the name of the Crazyflie in QTM. """ import asyncio import math @@ -51,7 +48,10 @@ uri = 'radio://0/80/2M' # The name of the rigid body in QTM that represents the Crazyflie -ridgid_body_name = 'cf' +rigid_body_name = 'cf' + +# True: send position and orientation; False: send position only +send_full_pose = False # The trajectory to fly # See https://github.com/whoenig/uav_trajectories for a tool to generate @@ -231,7 +231,10 @@ def send_extpose_rot_matrix(cf, x, y, z, rot): # Normalize the quaternion ql = math.sqrt(qx ** 2 + qy ** 2 + qz ** 2 + qw ** 2) - cf.extpos.send_extpose(x, y, z, qx / ql, qy / ql, qz / ql, qw / ql) + if send_full_pose: + cf.extpos.send_extpose(x, y, z, qx / ql, qy / ql, qz / ql, qw / ql) + else: + cf.extpos.send_extpos(x, y, z) def reset_estimator(cf): @@ -291,25 +294,26 @@ def run_sequence(cf, trajectory_id, duration): commander.stop() -cflib.crtp.init_drivers() +if __name__ == '__main__': + cflib.crtp.init_drivers() -# Connect to QTM -qtm_wrapper = QtmWrapper(ridgid_body_name) + # 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 + 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]) + # 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]) - activate_kalman_estimator(cf) - activate_high_level_commander(cf) - # activate_mellinger_controller(cf) - duration = upload_trajectory(cf, trajectory_id, figure8) - print('The sequence is {:.1f} seconds long'.format(duration)) - reset_estimator(cf) - run_sequence(cf, trajectory_id, duration) + activate_kalman_estimator(cf) + activate_high_level_commander(cf) + # activate_mellinger_controller(cf) + duration = upload_trajectory(cf, trajectory_id, figure8) + print('The sequence is {:.1f} seconds long'.format(duration)) + reset_estimator(cf) + run_sequence(cf, trajectory_id, duration) -qtm_wrapper.close() + qtm_wrapper.close() From dfa553ed21fbae91b228d9ecb672d1c156aec75e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 25 Mar 2021 16:43:07 +0100 Subject: [PATCH 167/861] (#143) small fixes in python api and fixed python highlights --- docs/user-guides/python_api.md | 54 +++++++++++------------ docs/user-guides/sbs_connect_log_param.md | 52 +++++++++++----------- docs/user-guides/sbs_motion_commander.md | 44 +++++++++--------- docs/user-guides/uart_communication.md | 2 +- 4 files changed, 76 insertions(+), 76 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 2f40a58f4..d1302ca40 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -35,9 +35,9 @@ Currently only *radio* and *debug* interfaces are used but there\'s ideas for more like *udp*, *serial*, *usb*, etc\...Here are some examples: -- \%%Radio interface, USB dongle number 0, radio channel 10 and radio - speed 250 Kbit/s: radio://0/10/250K %% -- Debug interface, id 0, channel 1: debug://0/1 +- _radio://0/10/250K_ : Radio interface, USB dongle number 0, radio channel 10 and radio + speed 250 Kbit/s: radio://0/10/250K +- _debug://0/1_ : Debug interface, id 0, channel 1 ### Variables and logging @@ -113,7 +113,7 @@ There\'s a limit of 28 chars in total and here are some examples: All callbacks are handled using the `Caller` class that contains the following methods: -``` {.python} +``` python add_callback(cb) """ Register cb as a new callback. Will not register duplicates. """ @@ -128,7 +128,7 @@ following methods: Before the library can be used the link drivers have to he initialized. This will search for available drivers and instantiate them. -``` {.python} +``` python init_drivers() """ Search for and initialize link drivers.""" ``` @@ -138,7 +138,7 @@ This will search for available drivers and instantiate them. The serial driver is disabled by default and has to be enabled to be used. Enable it in the call to `init_drivers()` -``` {.python} +``` python init_drivers(enable_serial_driver=True) ``` @@ -147,7 +147,7 @@ be used. Enable it in the call to `init_drivers()` Operations on the link and connection will return directly and will call the following callbacks when events occur: -``` {.python} +``` python # Called on disconnect, no matter the reason disconnected = Caller() # Called on unintentional disconnect only @@ -171,7 +171,7 @@ the following callbacks when events occur: To register for callbacks the following is used: -``` {.python} +``` python crazyflie = Crazyflie() crazyflie.connected.add_callback(crazyflie_connected) ``` @@ -182,7 +182,7 @@ The first thing to do is to find a Crazyflie quadcopter that we can connect to. This is done by queuing the library that will scan all the available interfaces (currently the debug and radio interface). -``` {.python} +``` python cflib.crtp.init_drivers() available = cflib.crtp.scan_interfaces() for i in available: @@ -192,7 +192,7 @@ available interfaces (currently the debug and radio interface). Opening and closing a communication link is doing by using the Crazyflie object: -``` {.python} +``` python crazyflie = Crazyflie() crazyflie.connected.add_callback(crazyflie_connected) crazyflie.open_link("radio://0/10/250K") @@ -200,7 +200,7 @@ object: Then you can use the following to close the link again: -``` {.python} +``` python crazyflie.close_link() ``` @@ -209,7 +209,7 @@ Then you can use the following to close the link again: The control setpoints are not implemented as parameters, instead they have a special API. -``` {.python} +``` python send_setpoint(roll, pitch, yaw, thrust): """ Send a new control set-point for roll/pitch/yaw/thust to the copter @@ -221,7 +221,7 @@ have a special API. To send a new control set-point use the following: -``` {.python} +``` python roll = 0.0 pitch = 0.0 yawrate = 0 @@ -261,7 +261,7 @@ instead. To set a parameter you have to the connected to the Crazyflie. A parameter is set using: -``` {.python} +``` python param_name = "group.name" param_value = 3 crazyflie.param.set_value(param_name, param_value) @@ -273,7 +273,7 @@ back by the library and this will trigger the callbacks. Parameter callbacks can be added at any time (you don\'t have to be connected to a Crazyflie). -``` {.python} +``` python add_update_callback(group, name=None, cb=None) """ Add a callback for a specific parameter name or group. If not name is specified then @@ -290,7 +290,7 @@ Crazyflie). Here\'s an example of how to use the calls. -``` {.python} +``` python crazyflie.param.add_update_callback(group="group", name="name", param_updated_callback) def param_updated_callback(name, value): @@ -312,7 +312,7 @@ instead. The API to create and get information from LogConfig: -``` {.python} +``` python # Called when new logging data arrives data_received_cb = Caller() # Called when there's an error @@ -345,7 +345,7 @@ The API to create and get information from LogConfig: The API for the log in the Crazyflie: -``` {.python} +``` python add_config(logconf) """Add a log configuration to the logging framework. @@ -359,7 +359,7 @@ The API for the log in the Crazyflie: To create a logging configuration the following can be used: -``` {.python} +``` python logconf = LogConfig(name="Logging", period_in_ms=100) logconf.add_variable("group1.name1", "float") logconf.add_variable("group1.name2", "uint8_t") @@ -378,7 +378,7 @@ internal type to transferred type before transfers: The logging cannot be started until your are connected to a Crazyflie: -``` {.python} +``` python # Callback called when the connection is established to the Crazyflie def connected(link_uri): crazyflie.log.add_config(logconf) @@ -405,7 +405,7 @@ can be used in one logging configuration. If the desired log variables do not fit in one logging configuration, a second cofiguration may be added. -``` {.python} +``` python crazyflie.log.add_config([logconf1, logconfig2]) ``` @@ -425,7 +425,7 @@ exiting it, for instance a connection or take off/landing of a Crazyflie. The SyncCrazyflie class wrapps a Crazyflie instance and mainly simplifies connect/disconnect. Basic usage -``` {.python} +``` python with SyncCrazyflie(uri) as scf: # A Crazyflie instance is created and is now connected. If the connection failes, # an exception is raised. @@ -440,7 +440,7 @@ Basic usage If some special properties are required for the underlying Crazyflie object, a Crazyflie instance can be passed in to the SyncCrazyflie instance. -``` {.python} +``` python my_cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(uri, cf=my_cf) as scf: # The my_cf is now connected @@ -453,7 +453,7 @@ a Crazyflie instance can be passed in to the SyncCrazyflie instance. The SyncLogger class wraps setting up, as well as starting/stopping logging. It works both for Crazyflie and SyncCrazyflie instances. To get the log values, iterate the instance. -``` {.python} +``` python # Connect to a Crazyflie with SyncCrazyflie(uri) as scf: # Create a log configuration @@ -484,7 +484,7 @@ movements that are blocking until the motion is finished. The MotionCommander is using velocity set points and does not have a global coordinate system, all positions are relative. It is mainly intended to be used with a Flow deck. -``` {.python} +``` python with SyncCrazyflie(URI) as scf: # We take off when the commander is created with MotionCommander(scf) as mc: @@ -506,7 +506,7 @@ The PositionHlCommander uses the high level commander in the Crazyflie and is based on a global coordinate system and absolute positoinins. It is inteneded to be used with a positioning system such as LPS, the lighthouse or a mocap system. -``` {.python} +``` python with SyncCrazyflie(URI) as scf: with PositionHlCommander(scf) as pc: # Go to the coordinate (0, 0, 1) @@ -517,4 +517,4 @@ to be used with a positioning system such as LPS, the lighthouse or a mocap syst ## Examples -The see the example folder of the repository. +The see the [example folder of the repository](https://github.com/bitcraze/crazyflie-lib-python/tree/master/examples). diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index 8b4090012..097279f49 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -36,14 +36,14 @@ You can call it `connect_log_param.py` (that is what we are using in this tutori Then you would need to start with the following standard python libraries. -``` +```python import logging import time ``` then you need to import the libraries for cflib: -``` +```python import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie @@ -60,7 +60,7 @@ into blocking function. After these imports, start the script with: -``` +```python # URI to the Crazyflie to connect to uri = 'radio://0/80/2M/E7E7E7E7E7' ``` @@ -70,7 +70,7 @@ This is the radio uri of the crazyflie, and currently displaying the default. It ## Main Write the following in the script: -``` +```python if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() @@ -86,7 +86,7 @@ The `syncCrazyflie` will create a synchronous Crazyflie instance with the specif Start a function above the main function (but below the URI) which you call simple connect: -``` +```python def simple_connect(): print("Yeah, I'm connected! :D") @@ -104,7 +104,7 @@ Now run the script in your terminal: `python3 connect_log_param.py` You are supposed to see the following in the terminal: -``` +```python Connecting to radio://0/80/2M/E7E7E7E7E7 Connected to radio://0/80/2M/E7E7E7E7E7 Yeah, I'm connected! :D @@ -119,7 +119,7 @@ Not super exciting stuff yet but it is a great start! It is also a good test if If you are getting an error, retrace your steps or check if your code matches the entire code underneath here. Also make sure your Crazyflie is on and your crazyradio PA connected to you computer, and that the Crazyflie is not connected to anything else (like the cfclient). If everything is peachy, please continue to the next part! -``` +```python import logging import time @@ -157,7 +157,7 @@ Alright, now taking a step up. We will now add to the script means to read out l Now we need to add several imports on the top of the script connect_log_param.py - ``` + ```python ... from cflib.crazyflie.syncCrazyflie import SyncCrazyflie @@ -170,7 +170,7 @@ from cflib.crazyflie.syncLogger import SyncLogger Also add the following underneath URI -``` +```python # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) ``` @@ -178,7 +178,7 @@ logging.basicConfig(level=logging.ERROR) ## Add logging config Now we are going to define the logging configuration. So add `lg_stab` in the `__main__` function : - ``` + ```python ... cflib.crtp.init_drivers() @@ -198,14 +198,14 @@ Here you will add the logs variables you would like to read out. If you are unsu ## Make the logging function Use the same connect_log_param.py script, and add the following function above `simple_connect()` and underneath URI: - ``` + ```python def simple_log(scf, logconf): ``` Notice that now you will need to include the SyncCrazyflie instance (`scf`) and the logging configuration. Now the logging instances will be inserted by adding the following after you configured the lg_stab: - ``` + ```python with SyncLogger(scf, lg_stab) as logger: for log_entry in logger: @@ -232,7 +232,7 @@ If everything is fine it should continuously print the logging variables, like t If you want to continuously receive the messages in the for loop, remove the `break`. You can stop the script with _ctrl+c_ If you are getting errors, check if your script corresponds with the full code: - ``` + ```python import logging import time @@ -291,7 +291,7 @@ Here we will explain how this asynchronous logging can be set up in the script. Above `def simple_log(..)`, begin a new function: -``` +```python def simple_log_async(scf, logconf): cf = scf.cf cf.log.add_config(logconf) @@ -304,20 +304,20 @@ Here you add the logging configuration to to the logging framework of the Crazyf ## Add a callback function First we will make the callback function like follows: -``` +```python def log_stab_callback(timestamp, data, logconf): print('[%d][%s]: %s' % (timestamp, logconf.name, data)) ``` This callback will be called once the log variables have received it and prints the contents. The callback function added to the logging framework by adding it to the log config in `simple_log_async(..)`: -``` +```python logconf.data_received_cb.add_callback(log_stab_callback) ``` Then the log configuration would need to be started manually, and then stopped after a few seconds: -``` +```python logconf.start() time.sleep(5) logconf.stop() @@ -331,7 +331,7 @@ Make sure to replace the `simple_log(...)` to `simple_log_async(...)` in the `__ If something went wrong, check if your script corresponds to the this: -``` +```python import logging import time @@ -381,7 +381,7 @@ Next to logging variables, it is possible to read and set parameters settings. T First add the group parameter name just above `with SyncCrazyflie(...` in `__main__`. -``` +```python group = "stabilizer" name = "estimator" ``` @@ -390,7 +390,7 @@ First add the group parameter name just above `with SyncCrazyflie(...` in `__mai Start the following function above `def log_stab_callback(...)`: -``` +```python def simple_param_async(scf, groupstr, namestr): cf = scf.cf full_name = groupstr+ "." +namestr @@ -400,13 +400,13 @@ def simple_param_async(scf, groupstr, namestr): In a similar way as in the previous section for the Async logging, we are going to make a callback function for the parameters. Add the callback function above `def simple_param_async`: -``` +```python def param_stab_est_callback(name, value): print('The crazyflie has parameter ' + name + ' set at number: ' + value) ``` Then add the following to the `def simple_param_async(...)` function: -``` +```python cf.param.add_update_callback(group=groupstr, name=namestr, cb=param_stab_est_callback) time.sleep(1) @@ -422,7 +422,7 @@ If you would like to test out the script now already, replace `simple_log_async( Now we will set a variable in a parameter. Add the following to the `simple_param_async(...)` function: -``` +```python cf.param.set_value(full_name,2) ``` @@ -440,13 +440,13 @@ What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) para It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physcially restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. So finish the `simple_param_async(...)` function by adding the next few lines: -``` +```python cf.param.set_value(full_name,1) time.sleep(1) ``` Make sure the right function is in `__main__`. Check if your script corresponds with the code: -``` +```python import logging import time @@ -509,7 +509,7 @@ if __name__ == '__main__': Run the script with `python3 connect_log_param.py` in a terminal and you should see the following: -``` +```python Connecting to radio://0/80/2M/E7E7E7E7E7 Connected to radio://0/80/2M/E7E7E7E7E7 The crazyflie has parameter stabilizer.estimator set at number: 1 diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 5e7b91747..d206938dd 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -17,7 +17,7 @@ We will assume that you already know this before you start with the tutorial: Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up an new python script called `motion_flying.py`. First you will start by adding the following import to the script: -``` +```python import logging import time @@ -47,7 +47,7 @@ This imports the motion commander, which is pretty much a wrapper around the pos Since this tutorial won't be a table top tutorial like last time, but an actual flying one, we need to put some securities in place. The flowdeck or any other positioning deck that you are using, should be correctly attached to the crazyflie. If it is not, it will try to fly anyway without a good position estimate and for sure is going to crash. We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...SyncCrazyflie(...)` in `__main__` -``` +```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: scf.cf.param.add_update_callback(group="deck", name="bcFlow2", @@ -57,7 +57,7 @@ We want to know if the deck is correctly attached before flying, therefore we wi ``` Above `__main__`, start a parameter callback function: -``` +```python def param_deck_flow(name, value_str): value = int(value_str) print(value) @@ -72,7 +72,7 @@ def param_deck_flow(name, value_str): The `is_deck_attached` is a global variable which should be defined under `URI`. Note that the value type that the `param_deck_flow()` is a string type, so you will need to convert it to a number first before you can do any operations with it. -``` +```python ... URI = 'radio://0/80/2M/E7E7E7E7E7' is_deck_attached = False @@ -81,7 +81,7 @@ is_deck_attached = False Try to run the script now, and try to see if it is able to detect that the flowdeck (or any other positioning deck), is correctly attached. Also try to remove it to see if it can detect it missing as well. This is the full script as we are: -``` +```python import logging import time @@ -123,7 +123,7 @@ if __name__ == '__main__': So now we are going to start up the SyncCrazyflie and start a function in the `__main__` function: -``` +```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: if is_deck_attached: @@ -133,7 +133,7 @@ See that we are now using `is_deck_attached`? If this is false, the function wil Now make the function `take_off_simple(..)` above `__main__`, which will contain the motion commander instance. -``` +```python def take_off_simple(scf): with MotionCommander(scf) as mc: time.sleep(3) @@ -148,7 +148,7 @@ The reason for the crazyflie to immediately take off, is that the motion command Currently the motion commander had 0.3 meters height as default but this can ofcourse be changed. Change the following line in `take_off_simple(...)`: -``` +```python with MotionCommander(scf) as mc: mc.up(0.3) time.sleep(3) @@ -157,12 +157,12 @@ Change the following line in `take_off_simple(...)`: Run the script again. The crazyflie will first take off to 0.3 meters and then goes up for another 0.3 meters. The same can be achieved by adjusting the default_height of the motion_commander, which is what we will do for now on in this tutorial. Remove the `mc.up(0.3)` and replace the motion commander line with -``` +```python with MotionCommander(scf, default_height = DEFAULT_HEIGHT) as mc: ``` Add the variable underneath `URI`: -``` +```python DEFAULT_HEIGHT = 0.5 ``` @@ -170,7 +170,7 @@ DEFAULT_HEIGHT = 0.5 Double check if your script is the same as beneath and run it again to check -``` +```python import logging import time @@ -213,7 +213,7 @@ if __name__ == '__main__': So now we know how to take off, so the second step is to move in a direction! Start a new function above `def take_off_simple(scf)`: -``` +```python def move_linear_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(1) @@ -229,7 +229,7 @@ You will see the crazyflie take off, fly 0.5 m forward, fly backwards and land a Now we are going to add a turn into it. Replace the content under motion commander in `move_linear_simple(..)` with the following: -``` +```python time.sleep(1) mc.forward(0.5) time.sleep(1) @@ -243,7 +243,7 @@ Try to run the script again. Now you can see the crazyflie take off, go forward, Double check if your code code is still correct: -``` +```python import logging import time @@ -299,7 +299,7 @@ When the motion commander commands have been executed, the script stops and the Let's integrate some logging to this as well. Add the following log config right into `__main__` under `SyncCrazyflie` -``` +```python lg_stab = LogConfig(name='Position', period_in_ms=10) lg_stab.add_variable('stateEstimate.x', 'float') lg_stab.add_variable('stateEstimate.y', 'float') @@ -319,7 +319,7 @@ Don't forget to add `from cflib.crazyflie.log import LogConfig` at the imports ( -``` +```python def log_pos_callback(timestamp, data, logconf): print(data) ``` @@ -329,7 +329,7 @@ NOW: Make global variable which is a list called `position_estimate` and fill th Just double check that everything has been implemented correctly and then run the script. You will see the same behavior as with the previous step but then with the position estimated printed at the same time. *You can replace the print function in the callback with a plotter if you would like to try that out, like with the python lib matplotlib :)* -``` +```python import logging import time @@ -404,7 +404,7 @@ There is a reason why we put the position_estimate to catch the positions from t ## Back and forth with limits Lets start with a new function above `move_in_box_limit(scf)`: -``` +```python def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: @@ -418,7 +418,7 @@ If you would run this (don't forget to replace `move_linear_simple()` in `__main Now we will add some behavior in the while loop: -``` +```python def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: start_forward() @@ -438,7 +438,7 @@ Run the script and you will see that the crazyflie will start moving back and fo You probably also noticed that we are using `mc.start_back()` and `mc.start_forward()` instead of the `mc.forward(0.5)` and `mc.back(0.5)` used in the previous steps. The main difference is that the *mc.forward* and *mc.back* are **blocking** functions that won't continue the code until the distance has been reached. The *mc.start_...()* will start the crazyflie in a direction and will not stop until the `mc.stop()` is given, which is given automatically when the motion commander instance is exited. That is why this is nice functions to use in reactive scenarios like these. -``` +```python import logging import time @@ -508,7 +508,7 @@ if __name__ == '__main__': ## Bouncing in a bounding box Let's take it up a notch! Replace the content in the while loop with the following: -``` +```python body_x_cmd = 0.2; body_y_cmd = 0.1; max_vel = 0.2; @@ -534,7 +534,7 @@ This will now start a linear motion into a certain direction, and makes the Craz Check out if your code still matches the full code and run the script! -``` +```python import logging import time diff --git a/docs/user-guides/uart_communication.md b/docs/user-guides/uart_communication.md index eb2a956e4..f02bc945b 100644 --- a/docs/user-guides/uart_communication.md +++ b/docs/user-guides/uart_communication.md @@ -53,7 +53,7 @@ The serial URI has the form `serial://` (e.g. `serial://ttyAMA0`, `serial: The following script might give an idea on how a first test of the setup might look like. -```{.python} +```python #!/usr/bin/env python3 import logging From 4b4069b6c2e73e9078e0d400ff0c812b4abf0807 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 25 Mar 2021 16:47:37 +0100 Subject: [PATCH 168/861] (#143) moved uart_communication to development --- docs/_data/menu.yml | 2 +- docs/{user-guides => development}/uart_communication.md | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename docs/{user-guides => development}/uart_communication.md (100%) diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index 465157d7f..437a861cb 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -6,7 +6,6 @@ - title: User guides subs: - {page_id: python_api} - - {page_id: uart_communication} - {page_id: sbs_connect_log_param} - {page_id: sbs_motion_commander} - title: Functional areas @@ -17,3 +16,4 @@ - {page_id: testing} - {page_id: matlab} - {page_id: eeprom} + - {page_id: uart_communication} diff --git a/docs/user-guides/uart_communication.md b/docs/development/uart_communication.md similarity index 100% rename from docs/user-guides/uart_communication.md rename to docs/development/uart_communication.md From 4910639c533f47970083b6465418f2e3abfdb102 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 25 Mar 2021 16:48:59 +0100 Subject: [PATCH 169/861] (#143) removed testing duplicate docs) --- docs/development/testing.md | 25 ------------------------- 1 file changed, 25 deletions(-) delete mode 100644 docs/development/testing.md diff --git a/docs/development/testing.md b/docs/development/testing.md deleted file mode 100644 index f94860452..000000000 --- a/docs/development/testing.md +++ /dev/null @@ -1,25 +0,0 @@ ---- -title: Testing -page_id: testing ---- - -## Testing -### With docker and the toolbelt - -For information and installation of the -[toolbelt.](https://wiki.bitcraze.io/projects:dockerbuilderimage:index) - -* Check to see if you pass tests: `tb test` -* Check to see if you pass style guidelines: `tb verify` - -Note: Docker and the toolbelt is an optional way of running tests and reduces the -work needed to maintain your python environment. - -### Native python on Linux, OSX, Windows - [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` -* If test fails after installing tox with `pip install tox`, installing with `sudo apt-get install tox`result a successful test run - -* Test package in python3.4 `TOXENV=py34 tox` -* Test package in python3.6 `TOXENV=py36 tox` - -Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python 3.3, 3.4 and would require all python versions to be installed on the machine.) From 357bf466b9e008145835d7d3eb17ce100afc66a6 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 25 Mar 2021 16:51:35 +0100 Subject: [PATCH 170/861] (#143) added one more small syntax highlight --- docs/functonal-areas/crazyradio_lib.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/functonal-areas/crazyradio_lib.md b/docs/functonal-areas/crazyradio_lib.md index ac12472f6..5de3cf450 100644 --- a/docs/functonal-areas/crazyradio_lib.md +++ b/docs/functonal-areas/crazyradio_lib.md @@ -27,7 +27,7 @@ contains data. This is an example on how to use the lib: -``` {.python} +``` .python import crazyradio cr = crazyradio.Crazyradio() From 5a618b745e047d4877b81a02bf99aa022bdcf94c Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 24 Mar 2021 11:04:57 +0100 Subject: [PATCH 171/861] cflib: utils: Add module to set uri from env --- cflib/utils/uri_helper.py | 49 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 cflib/utils/uri_helper.py diff --git a/cflib/utils/uri_helper.py b/cflib/utils/uri_helper.py new file mode 100644 index 000000000..280be06f7 --- /dev/null +++ b/cflib/utils/uri_helper.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2021 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 +import os +import sys + + +def uri_from_env(env='CFLIB_URI', default='radio://0/80/2M/E7E7E7E7E7') -> str: + try: + return os.environ[env] + except KeyError: + return default + + +def address_from_env(env='CFLIB_URI', default=0xE7E7E7E7E7) -> int: + try: + uri = os.environ[env] + except KeyError: + return default + + # Get the address part of the uri + address = uri.rsplit('/', 1)[-1] + try: + return int(address, 16) + except ValueError: + print('address is not hexadecimal! (%s)' % address, file=sys.stderr) + return None From d1b267d778265ff17fbc7c343e2991a2ac93062d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 24 Mar 2021 14:08:03 +0100 Subject: [PATCH 172/861] Use uri_helper to handle address and URI Instead of having different ways to set address in examples and test we can make sure all of them checks the `CFLIB_URI` environment variable and derives uri or address from that. If no environment variable is set then we fall back to default (radio://0/80/2M/E7E7E7E7E7) --- examples/autonomousSequence.py | 3 ++- examples/autonomous_sequence_high_level.py | 3 ++- examples/basicLedTiming.py | 4 +++- examples/basicLedmemSync.py | 3 ++- examples/basicLedparamSync.py | 4 +++- examples/basiclog.py | 5 ++++- examples/basiclogSync.py | 5 ++++- examples/basicparam.py | 5 ++++- examples/erase-ow.py | 5 ++++- examples/flash-memory.py | 5 ++++- examples/flowsequenceSync.py | 3 ++- examples/lps_reboot_to_bootloader.py | 5 ++++- examples/motion_commander_demo.py | 3 ++- examples/multiranger_pointcloud.py | 3 ++- examples/multiranger_push.py | 3 ++- examples/position_commander_demo.py | 3 ++- examples/positioning/matrix_light_printer.py | 3 ++- examples/qualisys/qualisys_hl_commander.py | 4 ++-- examples/ramp.py | 5 ++++- examples/read-eeprom.py | 5 ++++- examples/read-ow.py | 5 ++++- examples/step-by-step/sbs_motion_commander.py | 3 ++- examples/tuning/PID_controller_tuner.py | 6 ++++-- examples/write-eeprom.py | 9 ++++++--- examples/write-ow.py | 5 ++++- sys_test/single_cf_grounded/single_cf_grounded.py | 3 ++- test/crazyflie/test_syncCrazyflie.py | 5 +++-- test/crazyflie/test_syncLogger.py | 2 +- 28 files changed, 84 insertions(+), 33 deletions(-) diff --git a/examples/autonomousSequence.py b/examples/autonomousSequence.py index 337723fd8..2c9423575 100644 --- a/examples/autonomousSequence.py +++ b/examples/autonomousSequence.py @@ -39,9 +39,10 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Change the sequence according to your setup # x y z YAW diff --git a/examples/autonomous_sequence_high_level.py b/examples/autonomous_sequence_high_level.py index 3f81225d4..2f3331923 100644 --- a/examples/autonomous_sequence_high_level.py +++ b/examples/autonomous_sequence_high_level.py @@ -40,9 +40,10 @@ from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' +uri = uri_helper.from_env(default='radio://0/80/2M/E7E7E7E7E7') # The trajectory to fly # See https://github.com/whoenig/uav_trajectories for a tool to generate diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py index 47553b37a..144f5aad2 100644 --- a/examples/basicLedTiming.py +++ b/examples/basicLedTiming.py @@ -37,8 +37,10 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper -URI = 'radio://0/80/2M/E7E7E7E7E7' +# 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) diff --git a/examples/basicLedmemSync.py b/examples/basicLedmemSync.py index a4ecfc082..5676ad897 100644 --- a/examples/basicLedmemSync.py +++ b/examples/basicLedmemSync.py @@ -37,8 +37,9 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper -URI = 'radio://0/80/250K' +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) diff --git a/examples/basicLedparamSync.py b/examples/basicLedparamSync.py index d424bb296..15b078652 100644 --- a/examples/basicLedparamSync.py +++ b/examples/basicLedparamSync.py @@ -35,8 +35,10 @@ import cflib.crtp from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper -URI = 'radio://0/80/250K' +# 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) diff --git a/examples/basiclog.py b/examples/basiclog.py index ebfa04c0d..a08569571 100644 --- a/examples/basiclog.py +++ b/examples/basiclog.py @@ -34,6 +34,9 @@ import cflib.crtp # noqa from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -126,7 +129,7 @@ def _disconnected(self, link_uri): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/basiclogSync.py b/examples/basiclogSync.py index a5db5c8c0..7b5291c8a 100644 --- a/examples/basiclogSync.py +++ b/examples/basiclogSync.py @@ -37,6 +37,9 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -47,7 +50,7 @@ cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/basicparam.py b/examples/basicparam.py index c9960e1ee..abc2f03ac 100644 --- a/examples/basicparam.py +++ b/examples/basicparam.py @@ -34,6 +34,9 @@ import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -153,7 +156,7 @@ def _disconnected(self, link_uri): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/erase-ow.py b/examples/erase-ow.py index da4b80e98..c8d0fde47 100644 --- a/examples/erase-ow.py +++ b/examples/erase-ow.py @@ -34,6 +34,9 @@ import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.INFO) @@ -128,7 +131,7 @@ def _disconnected(self, link_uri): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/flash-memory.py b/examples/flash-memory.py index d43e7c721..f362bf168 100644 --- a/examples/flash-memory.py +++ b/examples/flash-memory.py @@ -26,6 +26,9 @@ import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) class NotConnected(RuntimeError): @@ -139,7 +142,7 @@ def scan(): # Scan for Crazyflies print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) interfaces = [uri for uri, _ in available] if not interfaces: diff --git a/examples/flowsequenceSync.py b/examples/flowsequenceSync.py index 1e83c74b7..00d8a7449 100644 --- a/examples/flowsequenceSync.py +++ b/examples/flowsequenceSync.py @@ -36,8 +36,9 @@ import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper -URI = 'radio://0/80/250K' +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) diff --git a/examples/lps_reboot_to_bootloader.py b/examples/lps_reboot_to_bootloader.py index c0eb80fdc..415fc966f 100644 --- a/examples/lps_reboot_to_bootloader.py +++ b/examples/lps_reboot_to_bootloader.py @@ -34,8 +34,11 @@ import cflib from cflib.crazyflie import Crazyflie +from cflib.utils import uri_helper from lpslib.lopoanchor import LoPoAnchor +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) + logging.basicConfig(level=logging.ERROR) @@ -97,7 +100,7 @@ def _reboot_thread(self): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/motion_commander_demo.py b/examples/motion_commander_demo.py index d204d6f36..ccc0e90d1 100644 --- a/examples/motion_commander_demo.py +++ b/examples/motion_commander_demo.py @@ -41,8 +41,9 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper -URI = 'radio://0/70/2M' +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) diff --git a/examples/multiranger_pointcloud.py b/examples/multiranger_pointcloud.py index b97940a6f..3592999a0 100644 --- a/examples/multiranger_pointcloud.py +++ b/examples/multiranger_pointcloud.py @@ -61,6 +61,7 @@ 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 @@ -73,7 +74,7 @@ logging.basicConfig(level=logging.INFO) -URI = 'radio://0/80/2M' +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') if len(sys.argv) > 1: URI = sys.argv[1] diff --git a/examples/multiranger_push.py b/examples/multiranger_push.py index a4ffb080a..66f4671de 100755 --- a/examples/multiranger_push.py +++ b/examples/multiranger_push.py @@ -49,9 +49,10 @@ 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 = 'radio://0/80/2M' +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') if len(sys.argv) > 1: URI = sys.argv[1] diff --git a/examples/position_commander_demo.py b/examples/position_commander_demo.py index 36a26003f..9210f55d5 100644 --- a/examples/position_commander_demo.py +++ b/examples/position_commander_demo.py @@ -35,9 +35,10 @@ 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 = 'radio://0/80/2M/E7E7E7E7E7' +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') def slightly_more_complex_usage(): diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index 77314dee5..a53563c1f 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -40,9 +40,10 @@ 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 = 'radio://0/80' +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') class ImageDef: diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 62c72090f..d5de69dbe 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -45,10 +45,10 @@ from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger - +from cflib.utils import uri_helper # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # The name of the rigid body in QTM that represents the Crazyflie ridgid_body_name = 'cf' diff --git a/examples/ramp.py b/examples/ramp.py index 11fd106dd..2da0d426e 100644 --- a/examples/ramp.py +++ b/examples/ramp.py @@ -33,6 +33,9 @@ import cflib from cflib.crazyflie import Crazyflie +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) logging.basicConfig(level=logging.ERROR) @@ -106,7 +109,7 @@ def _ramp_motors(self): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/read-eeprom.py b/examples/read-eeprom.py index c56ead011..7dde7bc9e 100644 --- a/examples/read-eeprom.py +++ b/examples/read-eeprom.py @@ -34,6 +34,9 @@ import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -119,7 +122,7 @@ def _disconnected(self, link_uri): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/read-ow.py b/examples/read-ow.py index 79e9d409f..9fcd0ec74 100644 --- a/examples/read-ow.py +++ b/examples/read-ow.py @@ -34,6 +34,9 @@ import cflib.crtp # noqa from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -126,7 +129,7 @@ def _disconnected(self, link_uri): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) 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 e35a86110..63a51e245 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -31,9 +31,10 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 BOX_LIMIT = 0.5 diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 7d558c04f..84862b7e0 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -31,9 +31,9 @@ import logging import sys import time +import tkinter as tk import matplotlib.pyplot as plt -import tkinter as tk from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg import cflib.crtp @@ -41,8 +41,10 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper -URI = 'radio://0/30/2M/E7E7E7E702' +# URI to the Crazyflie to connect to +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') STANDARD_HEIGHT = 0.8 STEP_RESPONSE_TIME = 3.0 STEP_SIZE = -0.2 # meters diff --git a/examples/write-eeprom.py b/examples/write-eeprom.py index ebfcb82fb..ca806cd00 100644 --- a/examples/write-eeprom.py +++ b/examples/write-eeprom.py @@ -34,6 +34,9 @@ import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -81,8 +84,8 @@ def _connected(self, link_uri): elems['pitch_trim'] = 0.0 elems['roll_trim'] = 0.0 elems['radio_channel'] = 80 - elems['radio_speed'] = 0 - elems['radio_address'] = 0xE7E7E7E7E7 + elems['radio_speed'] = 2 + elems['radio_address'] = 0xDEADBEEF mems[0].write_data(self._data_written) @@ -131,7 +134,7 @@ def _disconnected(self, link_uri): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/examples/write-ow.py b/examples/write-ow.py index f4462d200..75e290b65 100644 --- a/examples/write-ow.py +++ b/examples/write-ow.py @@ -39,6 +39,9 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.mem import OWElement +from cflib.utils import uri_helper + +address = uri_helper.address_from_env(default=0xE7E7E7E7E7) # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -141,7 +144,7 @@ def _disconnected(self, link_uri): cflib.crtp.init_drivers() # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() + available = cflib.crtp.scan_interfaces(address) print('Crazyflies found:') for i in available: print(i[0]) diff --git a/sys_test/single_cf_grounded/single_cf_grounded.py b/sys_test/single_cf_grounded/single_cf_grounded.py index 7ab9eae1b..622117993 100644 --- a/sys_test/single_cf_grounded/single_cf_grounded.py +++ b/sys_test/single_cf_grounded/single_cf_grounded.py @@ -26,12 +26,13 @@ import cflib.crtp from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort +from cflib.utils import uri_helper class TestSingleCfGrounded(unittest.TestCase): def setUp(self): cflib.crtp.init_drivers() - self.radioUri = 'radio://0/80/2M/E7E7E7E7E7' + self.radioUri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') self.usbUri = 'usb://0' def is_stm_connected(self): diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index 977963b96..643b5d26c 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -24,18 +24,19 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import unittest -from test.support.asyncCallbackCaller import AsyncCallbackCaller from unittest.mock import MagicMock from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper from cflib.utils.callbacks import Caller +from test.support.asyncCallbackCaller import AsyncCallbackCaller class SyncCrazyflieTest(unittest.TestCase): def setUp(self): - self.uri = 'radio://0/60/2M' + self.uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') self.cf_mock = MagicMock(spec=Crazyflie) self.cf_mock.connected = Caller() diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index 3417fe397..f00f7e0e6 100644 --- a/test/crazyflie/test_syncLogger.py +++ b/test/crazyflie/test_syncLogger.py @@ -24,7 +24,6 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import unittest -from test.support.asyncCallbackCaller import AsyncCallbackCaller from unittest.mock import call from unittest.mock import MagicMock @@ -34,6 +33,7 @@ from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils.callbacks import Caller +from test.support.asyncCallbackCaller import AsyncCallbackCaller class SyncLoggerTest(unittest.TestCase): From 918254c0c686802994d104c5c57fb65440fcf78f Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 26 Mar 2021 06:13:03 +0100 Subject: [PATCH 173/861] examples: Do not use scan_interfaces --- examples/basiclog.py | 15 ++------- examples/basiclogSync.py | 48 ++++++++++++---------------- examples/cfbridge.py | 24 ++++---------- examples/erase-ow.py | 15 ++------- examples/flash-memory.py | 30 +++-------------- examples/lps_reboot_to_bootloader.py | 15 ++------- examples/ramp.py | 15 ++------- examples/read-eeprom.py | 15 ++------- examples/read-ow.py | 15 ++------- examples/write-eeprom.py | 15 ++------- examples/write-ow.py | 15 ++------- 11 files changed, 55 insertions(+), 167 deletions(-) diff --git a/examples/basiclog.py b/examples/basiclog.py index a08569571..85bde76e3 100644 --- a/examples/basiclog.py +++ b/examples/basiclog.py @@ -36,7 +36,7 @@ from cflib.crazyflie.log import LogConfig from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -127,17 +127,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = LoggingExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + le = LoggingExample(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 diff --git a/examples/basiclogSync.py b/examples/basiclogSync.py index 7b5291c8a..9ad3d199c 100644 --- a/examples/basiclogSync.py +++ b/examples/basiclogSync.py @@ -39,7 +39,8 @@ from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) + +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -48,35 +49,26 @@ if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - if len(available) == 0: - print('No Crazyflies found, cannot run example') - else: - lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) - lg_stab.add_variable('stabilizer.roll', 'float') - lg_stab.add_variable('stabilizer.pitch', 'float') - lg_stab.add_variable('stabilizer.yaw', 'float') + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') - cf = Crazyflie(rw_cache='./cache') - with SyncCrazyflie(available[0][0], cf=cf) as scf: - # Note: it is possible to add more than one log config using an - # array. - # with SyncLogger(scf, [lg_stab, other_conf]) as logger: - with SyncLogger(scf, lg_stab) as logger: - endTime = time.time() + 10 + cf = Crazyflie(rw_cache='./cache') + with SyncCrazyflie(uri, cf=cf) as scf: + # Note: it is possible to add more than one log config using an + # array. + # with SyncLogger(scf, [lg_stab, other_conf]) as logger: + with SyncLogger(scf, lg_stab) as logger: + endTime = time.time() + 10 - for log_entry in logger: - timestamp = log_entry[0] - data = log_entry[1] - logconf_name = log_entry[2] + for log_entry in logger: + timestamp = log_entry[0] + data = log_entry[1] + logconf_name = log_entry[2] - print('[%d][%s]: %s' % (timestamp, logconf_name, data)) + print('[%d][%s]: %s' % (timestamp, logconf_name, data)) - if time.time() > endTime: - break + if time.time() > endTime: + break diff --git a/examples/cfbridge.py b/examples/cfbridge.py index 5ecd76e63..ca02b2e56 100755 --- a/examples/cfbridge.py +++ b/examples/cfbridge.py @@ -116,26 +116,14 @@ def _disconnected(self, link_uri): cflib.crtp.radiodriver.set_retries_before_disconnect(1500) cflib.crtp.radiodriver.set_retries(3) cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - if len(sys.argv) > 2: - address = int(sys.argv[2], 16) # address=0xE7E7E7E7E7 - else: - address = None - - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - if len(available) > 0: - if len(sys.argv) > 1: - channel = str(sys.argv[1]) - else: - channel = 80 + if len(sys.argv) > 1: + channel = str(sys.argv[1]) + else: + channel = 80 - link_uri = 'radio://0/' + str(channel) + '/2M' - le = RadioBridge(link_uri) # (available[0][0]) + 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 diff --git a/examples/erase-ow.py b/examples/erase-ow.py index c8d0fde47..d672491a5 100644 --- a/examples/erase-ow.py +++ b/examples/erase-ow.py @@ -36,7 +36,7 @@ from cflib.crazyflie.mem import MemoryElement from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.INFO) @@ -129,17 +129,8 @@ def _disconnected(self, link_uri): '/bitcraze/crazyflie-clients-python/issues/166') # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = EEPROMExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + 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 diff --git a/examples/flash-memory.py b/examples/flash-memory.py index f362bf168..f0b761d8e 100644 --- a/examples/flash-memory.py +++ b/examples/flash-memory.py @@ -28,7 +28,7 @@ from cflib.crazyflie.mem import MemoryElement from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') class NotConnected(RuntimeError): @@ -132,30 +132,7 @@ def choose(items, title_text, question_text): return items[index - 1] -def scan(): - """ - Scan for Crazyflie and return its URI. - """ - - # Initiate the low level drivers - cflib.crtp.init_drivers() - - # Scan for Crazyflies - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - interfaces = [uri for uri, _ in available] - - if not interfaces: - return None - return choose(interfaces, 'Crazyflies found:', 'Select interface: ') - - if __name__ == '__main__': - radio_uri = scan() - if radio_uri is None: - print('None found.') - sys.exit(1) - # Show info about bug 166 print('\n###\n' 'Please make sure that your NRF firmware is compiled without\n' @@ -164,8 +141,11 @@ def scan(): 'https://github.com/bitcraze/crazyflie-clients-python/issues/166\n' '###\n') + # Initialize the low-level drivers + cflib.crtp.init_drivers() + # Initialize flasher - flasher = Flasher(radio_uri) + flasher = Flasher(uri) def abort(): flasher.disconnect() diff --git a/examples/lps_reboot_to_bootloader.py b/examples/lps_reboot_to_bootloader.py index 415fc966f..5a0c66339 100644 --- a/examples/lps_reboot_to_bootloader.py +++ b/examples/lps_reboot_to_bootloader.py @@ -37,7 +37,7 @@ from cflib.utils import uri_helper from lpslib.lopoanchor import LoPoAnchor -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') logging.basicConfig(level=logging.ERROR) @@ -98,14 +98,5 @@ def _reboot_thread(self): if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = LpsRebootToBootloader(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + le = LpsRebootToBootloader(uri) diff --git a/examples/ramp.py b/examples/ramp.py index 2da0d426e..d216755ed 100644 --- a/examples/ramp.py +++ b/examples/ramp.py @@ -35,7 +35,7 @@ from cflib.crazyflie import Crazyflie from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') logging.basicConfig(level=logging.ERROR) @@ -107,14 +107,5 @@ def _ramp_motors(self): if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = MotorRampExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + le = MotorRampExample(uri) diff --git a/examples/read-eeprom.py b/examples/read-eeprom.py index 7dde7bc9e..75eb5b944 100644 --- a/examples/read-eeprom.py +++ b/examples/read-eeprom.py @@ -36,7 +36,7 @@ from cflib.crazyflie.mem import MemoryElement from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -120,17 +120,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = EEPROMExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + 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 diff --git a/examples/read-ow.py b/examples/read-ow.py index 9fcd0ec74..38132677f 100644 --- a/examples/read-ow.py +++ b/examples/read-ow.py @@ -36,7 +36,7 @@ from cflib.crazyflie.mem import MemoryElement from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -127,17 +127,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = OWExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + le = OWExample(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 diff --git a/examples/write-eeprom.py b/examples/write-eeprom.py index ca806cd00..2313cac8c 100644 --- a/examples/write-eeprom.py +++ b/examples/write-eeprom.py @@ -36,7 +36,7 @@ from cflib.crazyflie.mem import MemoryElement from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -132,17 +132,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = EEPROMExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + 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 diff --git a/examples/write-ow.py b/examples/write-ow.py index 75e290b65..301f7c18e 100644 --- a/examples/write-ow.py +++ b/examples/write-ow.py @@ -41,7 +41,7 @@ from cflib.crazyflie.mem import OWElement from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -142,17 +142,8 @@ def _disconnected(self, link_uri): # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = WriteOwExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + + 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 From 5cee303844f66350a4e3f61859bdd5f6709d4f5c Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 7 Apr 2021 11:18:13 +0200 Subject: [PATCH 174/861] Reorder imports --- examples/tuning/PID_controller_tuner.py | 2 +- test/crazyflie/test_syncCrazyflie.py | 2 +- test/crazyflie/test_syncLogger.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 84862b7e0..3d70bc101 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -31,9 +31,9 @@ import logging import sys import time -import tkinter as tk import matplotlib.pyplot as plt +import tkinter as tk from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg import cflib.crtp diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index 643b5d26c..957b1e274 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -24,13 +24,13 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import unittest +from test.support.asyncCallbackCaller import AsyncCallbackCaller from unittest.mock import MagicMock from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper from cflib.utils.callbacks import Caller -from test.support.asyncCallbackCaller import AsyncCallbackCaller class SyncCrazyflieTest(unittest.TestCase): diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index f00f7e0e6..3417fe397 100644 --- a/test/crazyflie/test_syncLogger.py +++ b/test/crazyflie/test_syncLogger.py @@ -24,6 +24,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import unittest +from test.support.asyncCallbackCaller import AsyncCallbackCaller from unittest.mock import call from unittest.mock import MagicMock @@ -33,7 +34,6 @@ from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils.callbacks import Caller -from test.support.asyncCallbackCaller import AsyncCallbackCaller class SyncLoggerTest(unittest.TestCase): From 2fa0bde7a36d5b077989bf7c89d42226c3090f10 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 14 Apr 2021 16:02:46 +0200 Subject: [PATCH 175/861] Fixes #226: Skip corrupted deck mem entries --- cflib/crazyflie/mem/deck_memory.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 8a619f0d7..b602c756f 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -117,8 +117,12 @@ def is_bootloader_active(self): def _parse(self, data): self._bit_field = struct.unpack(' Date: Wed, 14 Apr 2021 16:21:29 +0200 Subject: [PATCH 176/861] Fix single-quote --- cflib/crazyflie/mem/deck_memory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index b602c756f..93febc393 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -121,7 +121,7 @@ def _parse(self, data): self.required_hash, self.required_length, self._base_address, _name = struct.unpack(' Date: Wed, 14 Apr 2021 16:33:10 +0200 Subject: [PATCH 177/861] Update version to 0.1.15 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 5869f327e..6db7a48b1 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.14.2', + version='0.1.15', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 09ebb40f41d1d830d5b16cc3a05ea1aca9740f97 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Fri, 16 Apr 2021 11:29:04 +0200 Subject: [PATCH 178/861] Fixes #227: Decode FP16 in log blocks --- cflib/crazyflie/log.py | 2 +- examples/basiclog.py | 12 ++++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 6f40ea58b..fafbb978b 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -340,7 +340,7 @@ class LogTocElement: 0x04: ('int8_t', ' Date: Thu, 22 Apr 2021 16:14:22 +0200 Subject: [PATCH 179/861] Bootloader: add flash_full() method This handles warm/cold boot and has callbacks for progress, termination and info. This simplifies the flashing API and allow us to test something that we know is very similar to what is used by lib and client. --- cflib/bootloader/__init__.py | 33 +++++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 31e0543d3..adfab40d4 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -35,7 +35,9 @@ from collections import namedtuple from typing import Callable from typing import List +from typing import NoReturn from typing import Optional +from typing import Tuple from .boottypes import BootVersion from .boottypes import TargetTypes @@ -126,7 +128,7 @@ def start_bootloader(self, warm_boot=False): def get_target(self, target_id): return self._cload.request_info_update(target_id) - def flash(self, filename, targets: List[Target]): + def flash(self, filename: str, targets: List[Target]): # Separate flash targets from decks platform = self._get_platform_id() flash_targets = [t for t in targets if t.platform == platform] @@ -182,6 +184,33 @@ def flash(self, filename, targets: List[Target]): else: print('') + def flash_full(self, filename: Optional[str], warm: bool = True, + targets: Optional[Tuple[str, ...]] = None, + info_cb: Optional[Callable[[int, TargetTypes], NoReturn]] = None, + progress_cb: Optional[Callable[[str, int], NoReturn]] = None, + terminate_flash_cb: Optional[Callable[[], bool]] = None): + """ + Flash .zip or bin .file to list of targets. + Reset to firmware when done. + """ + if progress_cb is not None: + self.progress_cb = progress_cb + if terminate_flash_cb is not None: + self.terminate_flashing_cb = terminate_flash_cb + + if not self.start_bootloader(warm_boot=warm): + raise Exception('Could not connect to bootloader') + + if info_cb is not None: + connected = (self.get_target(TargetTypes.STM32),) + if self.protocol_version == BootVersion.CF2_PROTO_VER: + connected += (self.get_target(TargetTypes.NRF51),) + info_cb(self.protocol_version, connected) + + if filename is not None: + self.flash(filename, targets) + self.reset_to_firmware() + def _get_flash_artifacts_from_zip(self, filename): if not zipfile.is_zipfile(filename): return [] @@ -246,7 +275,7 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ self.progress_cb('Error: Not enough space to flash the image file.', int(progress)) else: print('Error: Not enough space to flash the image file.') - raise Exception() + raise Exception('Not enough space to flash the image file') if not self.progress_cb: logger.info(('%d bytes (%d pages) ' % ( From 64b591be2e4715d7feebb06002ef14bbdbcb83da Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 23 Apr 2021 07:16:19 +0200 Subject: [PATCH 180/861] syncCrazyFlie: remove prints SyncCrazyFlie is used in system operations like flashing decks, the prints are polluting terminals of people everywhere. --- cflib/crazyflie/syncCrazyflie.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index fd5ff727c..3e763e547 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -57,8 +57,6 @@ def open_link(self): self._add_callbacks() - print('Connecting to %s' % self._link_uri) - self._connect_event = Event() self.cf.open_link(self._link_uri) self._connect_event.wait() @@ -88,7 +86,6 @@ def is_link_open(self): 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._is_link_open = True if self._connect_event: self._connect_event.set() @@ -96,7 +93,6 @@ def _connected(self, link_uri): def _connection_failed(self, link_uri, msg): """Callback when initial connection fails (i.e no Crazyflie at the specified address)""" - print('Connection to %s failed: %s' % (link_uri, msg)) self._is_link_open = False self._error_message = msg if self._connect_event: From 85cb2ad66e3354a5b5e90a6b4c626a3fcc4bdb82 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 23 Apr 2021 15:54:53 +0200 Subject: [PATCH 181/861] log: Fix check for log config size We need to check if the size of the log elements are larger than 26 rather than 30, since some bytes are taken up by timestamp and block id. This mirrors the check done on firmware side. Without this a user will get hard to track down errors when having LogConfig sizes between 27 and 30. --- cflib/crazyflie/log.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index fafbb978b..3401f71ae 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -143,6 +143,9 @@ class LogConfig(object): """Representation of one log configuration that enables logging from the Crazyflie""" + # Maximum log payload length (4 bytes are used for block id and timestamp) + MAX_LEN = 26 + def __init__(self, name, period_in_ms): """Initialize the entry""" self.data_received_cb = Caller() @@ -474,7 +477,7 @@ def add_config(self, logconf): logconf.valid = False raise KeyError('Variable {} not in TOC'.format(var.name)) - if (size <= CRTPPacket.MAX_DATA_SIZE and + if (size <= LogConfig.MAX_LEN and (logconf.period > 0 and logconf.period < 0xFF)): logconf.valid = True logconf.cf = self.cf From a42ac9883385120128f6d16080f38ebdda5dd5ae Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 23 Apr 2021 16:52:10 +0200 Subject: [PATCH 182/861] log: Check if more than max log blocks are added --- cflib/crazyflie/log.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 3401f71ae..6a38abd4a 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -401,6 +401,8 @@ def __init__(self, ident=0, data=None): class Log(): """Create log configuration""" + MAX_BLOCKS = 16 + # These codes can be decoded using os.stderror, but # some of the text messages will look very strange # in the UI, so they are redefined here @@ -447,6 +449,11 @@ def add_config(self, logconf): 'Crazyflie!') return + if len(self.log_blocks) == self.MAX_BLOCKS: + raise AttributeError( + 'Configuration has max number of blocks (%d)' % self.MAX_BLOCKS + ) + # If the log configuration contains variables that we added without # type (i.e we want the stored as type for fetching as well) then # resolve this now and add them to the block again. From b2ede82622905c582d073a9b9cb2b1bd403401ff Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 26 Apr 2021 10:59:48 +0200 Subject: [PATCH 183/861] log: Check against max number of variables --- cflib/crazyflie/log.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 6a38abd4a..6ca2e5d1e 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -402,6 +402,7 @@ class Log(): """Create log configuration""" MAX_BLOCKS = 16 + MAX_VARIABLES = 128 # These codes can be decoded using os.stderror, but # some of the text messages will look very strange @@ -454,6 +455,19 @@ def add_config(self, logconf): 'Configuration has max number of blocks (%d)' % self.MAX_BLOCKS ) + # + # The Crazyflie firmware can only handle 128 variables before erroring + # out with ENOMEM. + # + num_variables = 0 + for block in self.log_blocks: + num_variables += len(block.variables) + if num_variables + len(logconf.variables) > self.MAX_VARIABLES: + raise AttributeError( + ('Adding this configuration would exceed max number ' + 'of variables (%d)' % self.MAX_VARIABLES) + ) + # If the log configuration contains variables that we added without # type (i.e we want the stored as type for fetching as well) then # resolve this now and add them to the block again. From 3aa59dff855c2d91e0b867220f2624d90200b067 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 26 Apr 2021 13:24:25 +0200 Subject: [PATCH 184/861] syncCrazyflie: Re-add prints as debug logging --- cflib/crazyflie/syncCrazyflie.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index 3e763e547..fd00712e3 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -29,10 +29,13 @@ into blocking function. It is useful for simple scripts that performs tasks as a sequence of events. """ +import logging from threading import Event from cflib.crazyflie import Crazyflie +logger = logging.getLogger(__name__) + class SyncCrazyflie: @@ -57,6 +60,8 @@ def open_link(self): self._add_callbacks() + logger.debug('Connecting to %s' % self._link_uri) + self._connect_event = Event() self.cf.open_link(self._link_uri) self._connect_event.wait() @@ -86,6 +91,7 @@ def is_link_open(self): 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.""" + logger.debug('Connected to %s' % link_uri) self._is_link_open = True if self._connect_event: self._connect_event.set() @@ -93,6 +99,7 @@ def _connected(self, link_uri): def _connection_failed(self, link_uri, msg): """Callback when initial connection fails (i.e no Crazyflie at the specified address)""" + logger.debug('Connection to %s failed: %s' % (link_uri, msg)) self._is_link_open = False self._error_message = msg if self._connect_event: From 7064cd7ea63de9606888228dabbc6299ce931116 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 29 Apr 2021 11:51:57 +0200 Subject: [PATCH 185/861] user-guides: Remove reference to connection prints --- docs/user-guides/sbs_connect_log_param.md | 4 ---- 1 file changed, 4 deletions(-) diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index 097279f49..243fca9bf 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -105,8 +105,6 @@ Now run the script in your terminal: You are supposed to see the following in the terminal: ```python -Connecting to radio://0/80/2M/E7E7E7E7E7 -Connected to radio://0/80/2M/E7E7E7E7E7 Yeah, I'm connected! :D Now I will disconnect :'( ``` @@ -510,8 +508,6 @@ if __name__ == '__main__': Run the script with `python3 connect_log_param.py` in a terminal and you should see the following: ```python -Connecting to radio://0/80/2M/E7E7E7E7E7 -Connected to radio://0/80/2M/E7E7E7E7E7 The crazyflie has parameter stabilizer.estimator set at number: 1 The crazyflie has parameter stabilizer.estimator set at number: 2 The crazyflie has parameter stabilizer.estimator set at number: 1 From e72d6a0fd72f63e0039ad91d9849db8acf01f325 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 30 Apr 2021 08:35:15 +0200 Subject: [PATCH 186/861] Log: Move size checks to create() It is allowed to add a bunch of logconfigs that exceed the variable and block limits as long as you do not add/start them. Fixes: #231 --- cflib/crazyflie/log.py | 43 ++++++++++++++++++++++++------------------ 1 file changed, 25 insertions(+), 18 deletions(-) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 6ca2e5d1e..402bd6684 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -163,6 +163,7 @@ def __init__(self, name, period_in_ms): self.period_in_ms = period_in_ms self._added = False self._started = False + self.pending = False self.valid = False self.variables = [] self.default_fetch_as = [] @@ -264,6 +265,29 @@ def create(self): command = self._cmd_create_block() next_to_add = 0 is_done = False + + num_variables = 0 + pending = 0 + for block in self.cf.log.log_blocks: + if block.pending or block.added or block.started: + pending += 1 + num_variables += len(block.variables) + + if pending < Log.MAX_BLOCKS: + # + # The Crazyflie firmware can only handle 128 variables before + # erroring out with ENOMEM. + # + if num_variables + len(self.variables) > Log.MAX_VARIABLES: + raise AttributeError( + ('Adding this configuration would exceed max number ' + 'of variables (%d)' % Log.MAX_VARIABLES) + ) + else: + raise AttributeError( + 'Configuration has max number of blocks (%d)' % Log.MAX_BLOCKS + ) + self.pending += 1 while not is_done: pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) @@ -450,24 +474,6 @@ def add_config(self, logconf): 'Crazyflie!') return - if len(self.log_blocks) == self.MAX_BLOCKS: - raise AttributeError( - 'Configuration has max number of blocks (%d)' % self.MAX_BLOCKS - ) - - # - # The Crazyflie firmware can only handle 128 variables before erroring - # out with ENOMEM. - # - num_variables = 0 - for block in self.log_blocks: - num_variables += len(block.variables) - if num_variables + len(logconf.variables) > self.MAX_VARIABLES: - raise AttributeError( - ('Adding this configuration would exceed max number ' - 'of variables (%d)' % self.MAX_VARIABLES) - ) - # If the log configuration contains variables that we added without # type (i.e we want the stored as type for fetching as well) then # resolve this now and add them to the block again. @@ -555,6 +561,7 @@ def _new_packet_cb(self, packet): self.cf.send_packet(pk, expected_reply=( CMD_START_LOGGING, id)) block.added = True + block.pending = False else: msg = self._err_codes[error_status] logger.warning('Error %d when adding id=%d (%s)', From 6a2430d793dee6df617f385b6d10694a58bc8273 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 6 May 2021 12:13:59 +0200 Subject: [PATCH 187/861] Radiodriver: Set cflinkcpp as default on x86_64 --- cflib/crtp/__init__.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index c52bcf1a3..a23733121 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -26,7 +26,7 @@ # MA 02110-1301, USA. """Scans and creates communication interfaces.""" import logging -import os +import platform from .exceptions import WrongUriType from .prrtdriver import PrrtDriver @@ -47,8 +47,11 @@ def init_drivers(enable_debug_driver=False, enable_serial_driver=False): """Initialize all the drivers.""" - env = os.getenv('USE_CFLINK') - if env is not None and env == 'cpp': + # + # Right now cflinkcpp only supports x86_64, see setup.py for more info: + # + mach = platform.machine() + if mach in ['x86_64']: from .cflinkcppdriver import CfLinkCppDriver CLASSES.append(CfLinkCppDriver) else: From d62dea8a3856ab19eb49be5342d9d43d5e346fbf Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 6 May 2021 12:15:01 +0200 Subject: [PATCH 188/861] setup.py: Require cflinkcpp on x86_64 --- setup.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/setup.py b/setup.py index 6db7a48b1..3f6e1e059 100644 --- a/setup.py +++ b/setup.py @@ -1,7 +1,18 @@ #!/usr/bin/env python3 +import platform + from setuptools import find_packages from setuptools import setup +extra_required = [] +# +# For now we only require the CPP native radio link driver on x86_64 +# since we only build and test on that platform. This will probably change. +# And probably faster if you request it. +# +if platform.machine() == 'x86_64': + extra_required.extend(['cflinkcpp>=1.0a3']) + setup( name='cflib', version='0.1.15', @@ -26,8 +37,8 @@ install_requires=[ 'pyusb>=1.0.0b2', - 'opencv-python-headless==4.5.1.48' - ], + 'opencv-python-headless==4.5.1.48', + ] + extra_required, # $ pip install -e .[dev] extras_require={ From 2eed3ce87b0b729385255661d210b15dd3da449d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 6 May 2021 14:44:09 +0200 Subject: [PATCH 189/861] Radiodriver: Keep and enfoce the USE_CFLINK env --- cflib/crtp/__init__.py | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index a23733121..0a801c214 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -26,6 +26,7 @@ # MA 02110-1301, USA. """Scans and creates communication interfaces.""" import logging +import os import platform from .exceptions import WrongUriType @@ -47,16 +48,29 @@ def init_drivers(enable_debug_driver=False, enable_serial_driver=False): """Initialize all the drivers.""" - # - # Right now cflinkcpp only supports x86_64, see setup.py for more info: - # - mach = platform.machine() - if mach in ['x86_64']: + def append_cpp(): from .cflinkcppdriver import CfLinkCppDriver CLASSES.append(CfLinkCppDriver) - else: + + def append_python(): CLASSES.extend([RadioDriver, UsbDriver]) + env = os.getenv('USE_CFLINK') + if env is None: # this is default behavior + mach = platform.machine() # cflinkcpp only supports x86_64 + if mach in ['x86_64']: + append_cpp() + else: # on non-x86_64 machines, fall-back to python + append_python() + + else: # if USE_CFLINK override is used, enforce it. + if env == 'cpp': + append_cpp() + elif env == 'python': + append_python() + else: + raise Exception('The cflink "{}" is not supported'.format(env)) + if enable_debug_driver: logger.warn('The debug driver is no longer supported!') From 1067752aa3af3c9c8c72310efbf7e2fda21b928a Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 7 May 2021 11:15:49 +0200 Subject: [PATCH 190/861] Flashing: make sure there is only one connection We need to close any outstanding link to the cf under flash. And we should take care not start a link to bootloader if one exists already. --- cflib/bootloader/__init__.py | 34 ++++++++++++++++++++-------------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index adfab40d4..6a8271e0b 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -90,26 +90,30 @@ def __init__(self, clink=None): info_cb=None, in_boot_cb=None) - def start_bootloader(self, warm_boot=False): + def start_bootloader(self, warm_boot=False, cf=None): self.warm_booted = warm_boot if warm_boot: + if cf is not None and cf.link: + cf.close_link() self._cload.open_bootloader_uri(self.clink) started = self._cload.reset_to_bootloader(TargetTypes.NRF51) if started: started = self._cload.check_link_and_get_info() else: - uri = self._cload.scan_for_bootloader() + if not self._cload.link: + uri = self._cload.scan_for_bootloader() - # Workaround for libusb on Windows (open/close too fast) - time.sleep(1) + # Workaround for libusb on Windows (open/close too fast) + time.sleep(1) - if uri: - self._cload.open_bootloader_uri(uri) - started = self._cload.check_link_and_get_info() + if uri: + self._cload.open_bootloader_uri(uri) + started = self._cload.check_link_and_get_info() + else: + started = False else: - started = False - + started = True if started: self.protocol_version = self._cload.protocol_version @@ -128,7 +132,7 @@ def start_bootloader(self, warm_boot=False): def get_target(self, target_id): return self._cload.request_info_update(target_id) - def flash(self, filename: str, targets: List[Target]): + def flash(self, filename: str, targets: List[Target], cf=None): # Separate flash targets from decks platform = self._get_platform_id() flash_targets = [t for t in targets if t.platform == platform] @@ -170,7 +174,7 @@ def flash(self, filename: str, targets: List[Target]): self.progress_cb('Deck updated! Restarting firmware.', int(100)) # Put the crazyflie back in Bootloader mode to exit the function in the same state we entered it - self.start_bootloader(warm_boot=True) + self.start_bootloader(warm_boot=True, cf=cf) deck_update_msg = 'Deck update complete.' else: @@ -184,7 +188,9 @@ def flash(self, filename: str, targets: List[Target]): else: print('') - def flash_full(self, filename: Optional[str], warm: bool = True, + def flash_full(self, cf: Optional[Crazyflie] = None, + filename: Optional[str] = None, + warm: bool = True, targets: Optional[Tuple[str, ...]] = None, info_cb: Optional[Callable[[int, TargetTypes], NoReturn]] = None, progress_cb: Optional[Callable[[str, int], NoReturn]] = None, @@ -198,7 +204,7 @@ def flash_full(self, filename: Optional[str], warm: bool = True, if terminate_flash_cb is not None: self.terminate_flashing_cb = terminate_flash_cb - if not self.start_bootloader(warm_boot=warm): + if not self.start_bootloader(warm_boot=warm, cf=cf): raise Exception('Could not connect to bootloader') if info_cb is not None: @@ -208,7 +214,7 @@ def flash_full(self, filename: Optional[str], warm: bool = True, info_cb(self.protocol_version, connected) if filename is not None: - self.flash(filename, targets) + self.flash(filename, targets, cf) self.reset_to_firmware() def _get_flash_artifacts_from_zip(self, filename): From d2d518027c20244256b29dc2fd668ea0077a7678 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 10 May 2021 15:58:03 +0200 Subject: [PATCH 191/861] #236 Corrected call to non existing function. Changed from_env() to uri_from_env() --- examples/autonomous_sequence_high_level.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/autonomous_sequence_high_level.py b/examples/autonomous_sequence_high_level.py index 2f3331923..8b3c719b4 100644 --- a/examples/autonomous_sequence_high_level.py +++ b/examples/autonomous_sequence_high_level.py @@ -43,7 +43,7 @@ from cflib.utils import uri_helper # URI to the Crazyflie to connect to -uri = uri_helper.from_env(default='radio://0/80/2M/E7E7E7E7E7') +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # The trajectory to fly # See https://github.com/whoenig/uav_trajectories for a tool to generate From 4ac16d533f7f24f9e9009cbdfd4914230fa9faa6 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 11 May 2021 14:17:04 +0200 Subject: [PATCH 192/861] cflinkcppdriver: do not check for acks on USB This will always give error since there is no ack_count when using USB. Fixes: #235 --- cflib/crtp/cflinkcppdriver.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/crtp/cflinkcppdriver.py b/cflib/crtp/cflinkcppdriver.py index 756278a26..ff8cccb1d 100644 --- a/cflib/crtp/cflinkcppdriver.py +++ b/cflib/crtp/cflinkcppdriver.py @@ -70,7 +70,8 @@ def connect(self, uri, link_quality_callback, link_error_callback): self.uri = uri self._link_quality_callback = link_quality_callback self._link_error_callback = link_error_callback - if link_quality_callback is not None or link_error_callback is not None: + + if uri.startswith('radio://') and link_quality_callback is not None: self._last_connection_stats = self._connection.statistics self._recompute_link_quality_timer() From b56d4fa997b754e372b6603f7ff67183526bde8c Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 18 May 2021 10:00:33 +0200 Subject: [PATCH 193/861] Revert "Merge pull request #233 from jonasdn/jonasdn/196" This reverts commit ea0e16abb83e4d09d162dc4f334bd4c991ea1cfd, reversing changes made to 50bb606cfefac1b9b25ab503c4c787cef786668b. Temporarly disable nativeLink as default until we fix discovered bugs. --- cflib/crtp/__init__.py | 23 +++-------------------- setup.py | 15 ++------------- 2 files changed, 5 insertions(+), 33 deletions(-) diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 0a801c214..c52bcf1a3 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -27,7 +27,6 @@ """Scans and creates communication interfaces.""" import logging import os -import platform from .exceptions import WrongUriType from .prrtdriver import PrrtDriver @@ -48,29 +47,13 @@ def init_drivers(enable_debug_driver=False, enable_serial_driver=False): """Initialize all the drivers.""" - def append_cpp(): + env = os.getenv('USE_CFLINK') + if env is not None and env == 'cpp': from .cflinkcppdriver import CfLinkCppDriver CLASSES.append(CfLinkCppDriver) - - def append_python(): + else: CLASSES.extend([RadioDriver, UsbDriver]) - env = os.getenv('USE_CFLINK') - if env is None: # this is default behavior - mach = platform.machine() # cflinkcpp only supports x86_64 - if mach in ['x86_64']: - append_cpp() - else: # on non-x86_64 machines, fall-back to python - append_python() - - else: # if USE_CFLINK override is used, enforce it. - if env == 'cpp': - append_cpp() - elif env == 'python': - append_python() - else: - raise Exception('The cflink "{}" is not supported'.format(env)) - if enable_debug_driver: logger.warn('The debug driver is no longer supported!') diff --git a/setup.py b/setup.py index 3f6e1e059..6db7a48b1 100644 --- a/setup.py +++ b/setup.py @@ -1,18 +1,7 @@ #!/usr/bin/env python3 -import platform - from setuptools import find_packages from setuptools import setup -extra_required = [] -# -# For now we only require the CPP native radio link driver on x86_64 -# since we only build and test on that platform. This will probably change. -# And probably faster if you request it. -# -if platform.machine() == 'x86_64': - extra_required.extend(['cflinkcpp>=1.0a3']) - setup( name='cflib', version='0.1.15', @@ -37,8 +26,8 @@ install_requires=[ 'pyusb>=1.0.0b2', - 'opencv-python-headless==4.5.1.48', - ] + extra_required, + 'opencv-python-headless==4.5.1.48' + ], # $ pip install -e .[dev] extras_require={ From 313a344072267b0a2396888a9c066cbb403d334a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 18 May 2021 10:58:12 +0200 Subject: [PATCH 194/861] Made possible to give a velocity command and rate input at the same time for motion commander --- cflib/positioning/motion_commander.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index 8053ae18a..b977dee2a 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -399,6 +399,23 @@ def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m): self._set_vel_setpoint( velocity_x_m, velocity_y_m, velocity_z_m, 0.0) + def start_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate): + """ + Start a velocity motion with yaw rate input. This function returns immediately. + + positive X is forward + positive Y is left + positive Z is up + + :param velocity_x_m: The velocity along the X-axis (meters/second) + :param velocity_y_m: The velocity along the Y-axis (meters/second) + :param velocity_z_m: The velocity along the Z-axis (meters/second) + :param rate: The angular rate (degrees/second) + :return: + """ + self._set_vel_setpoint( + velocity_x_m, velocity_y_m, velocity_z_m, rate) + def _set_vel_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw): if not self._is_flying: raise Exception('Can not move on the ground. Take off first!') From 62d1e23b243cf173717222fc40b32603e18551dd Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 18 May 2021 13:30:58 +0200 Subject: [PATCH 195/861] added yaw_rate to linear_motion --- cflib/positioning/motion_commander.py | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index b977dee2a..f48ded07b 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -383,7 +383,7 @@ def start_circle_right(self, radius_m, velocity=VELOCITY): self._set_vel_setpoint(velocity, 0.0, 0.0, rate) - def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m): + def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw = 0.0): """ Start a linear motion. This function returns immediately. @@ -397,24 +397,7 @@ def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m): :return: """ self._set_vel_setpoint( - velocity_x_m, velocity_y_m, velocity_z_m, 0.0) - - def start_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate): - """ - Start a velocity motion with yaw rate input. This function returns immediately. - - positive X is forward - positive Y is left - positive Z is up - - :param velocity_x_m: The velocity along the X-axis (meters/second) - :param velocity_y_m: The velocity along the Y-axis (meters/second) - :param velocity_z_m: The velocity along the Z-axis (meters/second) - :param rate: The angular rate (degrees/second) - :return: - """ - self._set_vel_setpoint( - velocity_x_m, velocity_y_m, velocity_z_m, rate) + velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw) def _set_vel_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw): if not self._is_flying: From 3958bc68f7bd18e94b4f2b07932e74e07bb89d4e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 18 May 2021 13:41:54 +0200 Subject: [PATCH 196/861] fix pep8 problem --- cflib/positioning/motion_commander.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index f48ded07b..da788bd5d 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -383,7 +383,7 @@ def start_circle_right(self, radius_m, velocity=VELOCITY): self._set_vel_setpoint(velocity, 0.0, 0.0, rate) - def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw = 0.0): + def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw=0.0): """ Start a linear motion. This function returns immediately. From f6732d067a6d437a953ce0b6aae8ca466bb318e7 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 18 May 2021 13:46:10 +0200 Subject: [PATCH 197/861] add some more doc --- cflib/positioning/motion_commander.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index da788bd5d..e9fa1ec1b 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -385,7 +385,7 @@ def start_circle_right(self, radius_m, velocity=VELOCITY): def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw=0.0): """ - Start a linear motion. This function returns immediately. + Start a linear motion with an optional yaw rate input. This function returns immediately. positive X is forward positive Y is left @@ -394,6 +394,7 @@ def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw :param velocity_x_m: The velocity along the X-axis (meters/second) :param velocity_y_m: The velocity along the Y-axis (meters/second) :param velocity_z_m: The velocity along the Z-axis (meters/second) + :param rate: The angular rate (degrees/second) :return: """ self._set_vel_setpoint( From 61dbf900eefcdc9d0d900a38c354f3a93165f600 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 2 Jun 2021 12:46:04 +0200 Subject: [PATCH 198/861] Return a bool to indicate if a radio pk was sent --- cflib/crtp/radiodriver.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 974c09e70..9f6b40d99 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -357,14 +357,16 @@ def receive_packet(self, wait=0): except queue.Empty: return None - def send_packet(self, pk): + def send_packet(self, pk) -> bool: """ Send the packet pk though the link """ try: self.out_queue.put(pk, True, 2) + return True except queue.Full: if self.link_error_callback: self.link_error_callback('RadioDriver: Could not send packet' ' to copter') + return False def pause(self): self._thread.stop() From ac18b88b21f6a96d33e5d1028033405a663b69a2 Mon Sep 17 00:00:00 2001 From: Stan Khaykin Date: Wed, 2 Jun 2021 17:23:40 -0700 Subject: [PATCH 199/861] Allow nonzero landing height (PositionHlCommander) --- cflib/positioning/position_hl_commander.py | 23 ++++++++++++++----- examples/position_commander_demo.py | 10 ++++++++ .../positioning/test_position_hl_commander.py | 14 +++++++++++ 3 files changed, 41 insertions(+), 6 deletions(-) diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index 0b4807cab..ac41dd48a 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -52,7 +52,8 @@ def __init__(self, crazyflie, x=0.0, y=0.0, z=0.0, default_velocity=0.5, default_height=0.5, - controller=CONTROLLER_PID): + controller=CONTROLLER_PID, + default_landing_height=0.0): """ Construct an instance of a PositionHlCommander @@ -63,6 +64,7 @@ def __init__(self, crazyflie, :param default_velocity: the default velocity to use :param default_height: the default height to fly at :param controller: Which underlying controller to use + :param default_landing_height: If taking off/landing on objects higher than ground, set this parameter to ensure proper landing """ if isinstance(crazyflie, SyncCrazyflie): self._cf = crazyflie.cf @@ -85,6 +87,8 @@ def __init__(self, crazyflie, self._init_time = time.time() + self._default_landing_height = default_landing_height + def take_off(self, height=DEFAULT, velocity=DEFAULT): """ Takes off, that is starts the motors, goes straight up and hovers. @@ -117,7 +121,7 @@ def take_off(self, height=DEFAULT, velocity=DEFAULT): time.sleep(duration_s) self._z = height - def land(self, velocity=DEFAULT): + def land(self, velocity=DEFAULT, landing_height=0.0): """ Go straight down and turn off the motors. @@ -128,10 +132,10 @@ def land(self, velocity=DEFAULT): :return: """ if self._is_flying: - duration_s = self._z / self._velocity(velocity) - self._hl_commander.land(0, duration_s) + duration_s = (self._z - landing_height) / self._velocity(velocity) + self._hl_commander.land(landing_height, duration_s) time.sleep(duration_s) - self._z = 0.0 + self._z = landing_height self._hl_commander.stop() self._is_flying = False @@ -141,7 +145,7 @@ def __enter__(self): return self def __exit__(self, exc_type, exc_val, exc_tb): - self.land() + self.land(landing_height=self._default_landing_height) def left(self, distance_m, velocity=DEFAULT): """ @@ -291,3 +295,10 @@ def _height(self, height): if height is self.DEFAULT: return self._default_height return height + + def set_landing_height(self, landing_height): + """ + Set the landing height to a specific value + Use this function to land on objects that are at non-zero height + """ + self._default_landing_height = landing_height diff --git a/examples/position_commander_demo.py b/examples/position_commander_demo.py index 9210f55d5..59e87aee9 100644 --- a/examples/position_commander_demo.py +++ b/examples/position_commander_demo.py @@ -67,6 +67,15 @@ def slightly_more_complex_usage(): pc.go_to(0.0, 0.0) +def land_on_elevated_surface(): + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2, default_landing_height=0.35) as pc: + #fly onto a landing platform at non-zero height (ex: from floor to desk, etc) + pc.forward(1.0) + pc.left(1.0) + # land() will be called on context exit, gradually lowering to the default_lanidng_height, then stoppig motors + + def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf) as pc: @@ -81,3 +90,4 @@ def simple_sequence(): simple_sequence() # slightly_more_complex_usage() + # land_on_elevated_surface() \ No newline at end of file diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index 003d86b54..3cfec41f2 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -128,6 +128,20 @@ def test_that_it_goes_down_on_landing( self.commander_mock.land.assert_called_with(0.0, duration) sleep_mock.assert_called_with(duration) + def test_that_it_goes_down_to_set_landing_height_on_landing( + self, sleep_mock): + # Fixture + self.sut.take_off(height=1.0) + self.sut.set_landing_height(0.4) + + # Test + self.sut.land(velocity=0.6, landing_height=0.4) + + # Assert + duration = (1.0 - 0.4) / 0.6 + self.commander_mock.land.assert_called_with(0.4, duration) + sleep_mock.assert_called_with(duration) + def test_that_it_takes_off_and_lands_as_context_manager( self, sleep_mock): # Fixture From d29a0d6eeafad6c97aede3e5a722f992a4549253 Mon Sep 17 00:00:00 2001 From: Stan Khaykin Date: Fri, 4 Jun 2021 02:14:32 -0700 Subject: [PATCH 200/861] FIX: Flake8 CI fail styling --- cflib/positioning/position_hl_commander.py | 4 ++-- examples/position_commander_demo.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index ac41dd48a..0dee82e70 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -64,7 +64,7 @@ def __init__(self, crazyflie, :param default_velocity: the default velocity to use :param default_height: the default height to fly at :param controller: Which underlying controller to use - :param default_landing_height: If taking off/landing on objects higher than ground, set this parameter to ensure proper landing + :param default_landing_height: Landing height (zero if not specified); for landing on objects off the ground """ if isinstance(crazyflie, SyncCrazyflie): self._cf = crazyflie.cf @@ -299,6 +299,6 @@ def _height(self, height): def set_landing_height(self, landing_height): """ Set the landing height to a specific value - Use this function to land on objects that are at non-zero height + Use this function to land on objects that are at non-zero height """ self._default_landing_height = landing_height diff --git a/examples/position_commander_demo.py b/examples/position_commander_demo.py index 59e87aee9..ba539477e 100644 --- a/examples/position_commander_demo.py +++ b/examples/position_commander_demo.py @@ -73,7 +73,7 @@ def land_on_elevated_surface(): #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 the default_lanidng_height, then stoppig motors + # land() will be called on context exit, gradually lowering to default_lanidng_height, then stoppig motors def simple_sequence(): From b3aa25668e02ef850693e8f0fe527ef73366798b Mon Sep 17 00:00:00 2001 From: Stan Khaykin Date: Fri, 4 Jun 2021 02:47:40 -0700 Subject: [PATCH 201/861] FIX: landing_height now set to DEFAULT in land() --- cflib/positioning/position_hl_commander.py | 10 ++++++++-- test/positioning/test_position_hl_commander.py | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index 0dee82e70..4e7a0ade7 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -121,7 +121,7 @@ def take_off(self, height=DEFAULT, velocity=DEFAULT): time.sleep(duration_s) self._z = height - def land(self, velocity=DEFAULT, landing_height=0.0): + def land(self, velocity=DEFAULT, landing_height=DEFAULT): """ Go straight down and turn off the motors. @@ -132,6 +132,7 @@ def land(self, velocity=DEFAULT, landing_height=0.0): :return: """ if self._is_flying: + landing_height = self._landing_height(landing_height) duration_s = (self._z - landing_height) / self._velocity(velocity) self._hl_commander.land(landing_height, duration_s) time.sleep(duration_s) @@ -145,7 +146,7 @@ def __enter__(self): return self def __exit__(self, exc_type, exc_val, exc_tb): - self.land(landing_height=self._default_landing_height) + self.land() def left(self, distance_m, velocity=DEFAULT): """ @@ -295,6 +296,11 @@ def _height(self, height): if height is self.DEFAULT: return self._default_height return height + + def _landing_height(self, landing_height): + if landing_height is self.DEFAULT: + return self._default_landing_height + return landing_height def set_landing_height(self, landing_height): """ diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index 3cfec41f2..545f193e1 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -135,7 +135,7 @@ def test_that_it_goes_down_to_set_landing_height_on_landing( self.sut.set_landing_height(0.4) # Test - self.sut.land(velocity=0.6, landing_height=0.4) + self.sut.land(velocity=0.6) # Assert duration = (1.0 - 0.4) / 0.6 From 2116a7f1df62e85104bfd05ff263497816119818 Mon Sep 17 00:00:00 2001 From: Dominik Natter Date: Sun, 6 Jun 2021 17:50:23 +0200 Subject: [PATCH 202/861] Here follow multiple minor updates to this step-by-step guide that increase user friendliness. In "Step 2: Take off function", section "Changing the height": In the first code snippet 'mc.stop()' was not added. However, in the full script one can use to double check against 'mc.stop()' is added. As this function is called automatically when the motion commander instance is exited, I'd drop it for clarity. In "Step 4: Logging while flying": If one follows the snippets for a step-wise instruction, the code will not compile. This is because 'lg_stab' should actually be called 'logconf'. In "Step 5: Combine logging and motion commander", section "Back and forth with limits": In the first snippet a function 'move_box_limit()' is introduced and the text suggests to place it above a non-existing function 'move_in_box_limit()'. I updated it with 'move_linear_simple()' in order to be consistent with the full ready-to-copy example. The second snippet will not compile, because start_forward() is a function of the motion commander instance. Furthermore, the three variables 'body_x_cmd', 'body_y_cmd', and 'max_vel' are neither introduced nor needed in this section (only in the following one). Hence I'd drop them here for clarity. --- docs/user-guides/sbs_motion_commander.md | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index d206938dd..75ddcebf7 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -190,7 +190,6 @@ logging.basicConfig(level=logging.ERROR) def take_off_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) - mc.stop() def param_deck_flow(name, value_str): ... @@ -300,9 +299,9 @@ Let's integrate some logging to this as well. Add the following log config right ```python - lg_stab = LogConfig(name='Position', period_in_ms=10) - lg_stab.add_variable('stateEstimate.x', 'float') - lg_stab.add_variable('stateEstimate.y', 'float') + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') cf = scf.cf cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) @@ -402,7 +401,7 @@ if __name__ == '__main__': There is a reason why we put the position_estimate to catch the positions from the log, since we would like to now do something with it! ## Back and forth with limits -Lets start with a new function above `move_in_box_limit(scf)`: +Lets start with a new function above `move_linear_simple(scf)`: ```python def move_box_limit(scf): @@ -421,7 +420,7 @@ Now we will add some behavior in the while loop: ```python def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: - start_forward() + mc.start_forward() while (1): if position_estimate[0] > BOX_LIMIT: @@ -462,10 +461,6 @@ position_estimate = [0, 0] def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: - body_x_cmd = 0.2 - body_y_cmd = 0.1 - max_vel = 0.2 - while (1): if position_estimate[0] > BOX_LIMIT: mc.start_back() From fa544451ccf5ea2a4692822478faf19a375e8703 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 8 Jun 2021 08:50:06 +0200 Subject: [PATCH 203/861] udpdriver: Handle socket import better --- cflib/crtp/udpdriver.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cflib/crtp/udpdriver.py b/cflib/crtp/udpdriver.py index bd2a1c352..d82e846a0 100644 --- a/cflib/crtp/udpdriver.py +++ b/cflib/crtp/udpdriver.py @@ -28,8 +28,10 @@ See udpserver.py for the protocol""" import queue import re +import socket import struct -from socket import socket + +from urllib.parse import urlparse from .crtpdriver import CRTPDriver from .crtpstack import CRTPPacket @@ -50,7 +52,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): raise WrongUriType('Not an UDP URI') self.queue = queue.Queue() - self.socket = socket(socket.AF_INET, socket.SOCK_DGRAM) + self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.addr = ('localhost', 7777) self.socket.connect(self.addr) From eeda0ba4a9c5e240a99e7f619ae382f7fd5e87e6 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 8 Jun 2021 08:50:47 +0200 Subject: [PATCH 204/861] udpdriver: Parse uri instead of hard coding --- cflib/crtp/udpdriver.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/crtp/udpdriver.py b/cflib/crtp/udpdriver.py index d82e846a0..00e37f7c0 100644 --- a/cflib/crtp/udpdriver.py +++ b/cflib/crtp/udpdriver.py @@ -30,7 +30,6 @@ import re import socket import struct - from urllib.parse import urlparse from .crtpdriver import CRTPDriver @@ -47,13 +46,14 @@ def __init__(self): None def connect(self, uri, linkQualityCallback, linkErrorCallback): - # check if the URI is a radio URI if not re.search('^udp://', uri): raise WrongUriType('Not an UDP URI') + parse = urlparse(uri) + self.queue = queue.Queue() self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.addr = ('localhost', 7777) + self.addr = (parse.hostname, parse.port) self.socket.connect(self.addr) # Add this to the server clients list From 59f541cb8c63dfeebf822f98b78278058102729e Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 8 Jun 2021 08:51:49 +0200 Subject: [PATCH 205/861] udpdriver: Encode bytes before sending This is needed for python3 to make sure we get bytes and not string. --- cflib/crtp/udpdriver.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/cflib/crtp/udpdriver.py b/cflib/crtp/udpdriver.py index 00e37f7c0..726764c67 100644 --- a/cflib/crtp/udpdriver.py +++ b/cflib/crtp/udpdriver.py @@ -56,8 +56,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): self.addr = (parse.hostname, parse.port) self.socket.connect(self.addr) - # Add this to the server clients list - self.socket.sendto('\xFF\x01\x01\x01', self.addr) + self.socket.sendto('\xFF\x01\x01\x01'.encode(), self.addr) def receive_packet(self, time=0): data, addr = self.socket.recvfrom(1024) @@ -92,11 +91,11 @@ def send_packet(self, pk): data = ''.join(chr(v) for v in (raw + (cksum,))) # print tuple(data) - self.socket.sendto(data, self.addr) + self.socket.sendto(data.encode(), self.addr) def close(self): # Remove this from the server clients list - self.socket.sendto('\xFF\x01\x02\x02', self.addr) + self.socket.sendto('\xFF\x01\x02\x02'.encode(), self.addr) def get_name(self): return 'udp' From a97800f95b69236cb1eab7ec9e34983a161b3d4d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 8 Jun 2021 08:52:47 +0200 Subject: [PATCH 206/861] udpdriver: Make sure we interpret data as uint Otherwise the CRTP stack will choke on data not being in the range of 0 - 255. Fixes errors seen as: ValueError: byte must be in range(0, 256) --- cflib/crtp/udpdriver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crtp/udpdriver.py b/cflib/crtp/udpdriver.py index 726764c67..17b66f6b5 100644 --- a/cflib/crtp/udpdriver.py +++ b/cflib/crtp/udpdriver.py @@ -62,7 +62,7 @@ def receive_packet(self, time=0): data, addr = self.socket.recvfrom(1024) if data: - data = struct.unpack('b' * (len(data) - 1), data[0:len(data) - 1]) + data = struct.unpack('B' * (len(data) - 1), data[0:len(data) - 1]) pk = CRTPPacket() pk.port = data[0] pk.data = data[1:] From ed4972febdac5837267c6875e0982cf8597ff5a0 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 9 Jun 2021 07:29:39 +0200 Subject: [PATCH 207/861] styling fixes --- cflib/positioning/position_hl_commander.py | 2 +- examples/position_commander_demo.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index 4e7a0ade7..17702d4c7 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -301,7 +301,7 @@ def _landing_height(self, landing_height): if landing_height is self.DEFAULT: return self._default_landing_height return landing_height - + def set_landing_height(self, landing_height): """ Set the landing height to a specific value diff --git a/examples/position_commander_demo.py b/examples/position_commander_demo.py index ba539477e..fb31cd47d 100644 --- a/examples/position_commander_demo.py +++ b/examples/position_commander_demo.py @@ -70,7 +70,7 @@ def slightly_more_complex_usage(): def land_on_elevated_surface(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2, default_landing_height=0.35) as pc: - #fly onto a landing platform at non-zero height (ex: from floor to desk, etc) + # 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_lanidng_height, then stoppig motors @@ -90,4 +90,4 @@ def simple_sequence(): simple_sequence() # slightly_more_complex_usage() - # land_on_elevated_surface() \ No newline at end of file + # land_on_elevated_surface() From d3a5ec714459e131f022533efbab1d6f81470da0 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 9 Jun 2021 06:59:49 +0200 Subject: [PATCH 208/861] cloader: Remove unused reset_to_bootloader1 method --- cflib/bootloader/cloader.py | 49 ------------------------------------- 1 file changed, 49 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 69db51506..ecb934602 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -134,55 +134,6 @@ def reset_to_bootloader(self, target_id): else: return False - def reset_to_bootloader1(self, cpu_id): - """ Reset to the bootloader - The parameter cpuid shall correspond to the device to reset. - - Return true if the reset has been done and the contact with the - bootloader is established. - """ - # Send an echo request and wait for the answer - # Mainly aim to bypass a bug of the crazyflie firmware that prevents - # reset before normal CRTP communication - pk = CRTPPacket() - pk.port = CRTPPort.LINKCTRL - pk.data = (1, 2, 3) + cpu_id - self.link.send_packet(pk) - - pk = None - while True: - pk = self.link.receive_packet(2) - if not pk: - return False - - if pk.port == CRTPPort.LINKCTRL: - break - - # Send the reset to bootloader request - pk = CRTPPacket() - pk.set_header(0xFF, 0xFF) - pk.data = (0xFF, 0xFE) + cpu_id - self.link.send_packet(pk) - - # Wait to ack the reset ... - pk = None - while True: - pk = self.link.receive_packet(2) - if not pk: - return False - - if pk.port == 0xFF and tuple(pk.data) == (0xFF, 0xFE) + cpu_id: - pk.data = (0xFF, 0xF0) + cpu_id - self.link.send_packet(pk) - break - - time.sleep(0.1) - self.link.close() - self.link = cflib.crtp.get_link_driver(self.clink_address) - # time.sleep(0.1) - - return self._update_info() - def reset_to_firmware(self, target_id): """ Reset to firmware The parameter cpuid shall correspond to the device to reset. From df4248079398c001e516ed7aed03ed4bce211bb3 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 9 Jun 2021 07:02:02 +0200 Subject: [PATCH 209/861] cloader: Improve reset_to_[bootloader|firmware] - Add resend to `reset_to_firmware`, packets can get lost - Make sure we return True / False from `reset_to_firmware` - Add timeout in seconds, instead of amount of retries - Remove legacy CF1 code to make flow more clear --- cflib/bootloader/cloader.py | 105 ++++++++++++++---------------------- 1 file changed, 40 insertions(+), 65 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index ecb934602..2f0435f20 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -37,7 +37,6 @@ from .boottypes import Target from .boottypes import TargetTypes from cflib.crtp.crtpstack import CRTPPacket -from cflib.crtp.crtpstack import CRTPPort __author__ = 'Bitcraze AB' __all__ = ['Cloader'] @@ -92,84 +91,60 @@ def scan_for_bootloader(self): return res[0] return None - def reset_to_bootloader(self, target_id): - retry_counter = 5 - pk = CRTPPacket() - pk.set_header(0xFF, 0xFF) - pk.data = (target_id, 0xFF) + def reset_to_bootloader(self, target_id: int) -> bool: + pk = CRTPPacket(0xFF, [target_id, 0xFF]) self.link.send_packet(pk) + address = None - got_answer = False - while(not got_answer and retry_counter >= 0): - pk = self.link.receive_packet(1) - if pk and pk.header == 0xFF: - try: - data = struct.unpack(' 3: + if struct.unpack(' bool: """ Reset to firmware - The parameter cpuid shall correspond to the device to reset. + The parameter target_id corresponds to the device to reset. - Return true if the reset has been done + Return True if the reset has been done, False on timeout """ - # The fake CPU ID is legacy from the Crazyflie 1.0 - # In order to reset the CPU id had to be sent, but this - # was removed before launching it. But the length check is - # still in the bootloader. So to work around this bug so - # some extra data needs to be sent. - fake_cpu_id = (1, 2, 4, 5, 6, 7, 8, 9, 10, 11, 12) - # Send the reset to bootloader request - pk = CRTPPacket() - pk.set_header(0xFF, 0xFF) - pk.data = (target_id, 0xFF) + fake_cpu_id + pk = CRTPPacket(0xFF, [target_id, 0xFF]) self.link.send_packet(pk) - # Wait to ack the reset ... - pk = None - while True: - pk = self.link.receive_packet(2) - if not pk: - return False - - if (pk.header == 0xFF and struct.unpack( - 'B' * len(pk.data), pk.data)[:2] == (target_id, 0xFF)): - # Difference in CF1 and CF2 (CPU ID) - if target_id == 0xFE: - pk.data = (target_id, 0xF0, 0x01) - else: - pk.data = (target_id, 0xF0) + fake_cpu_id + timeout = 5 # seconds + ts = time.time() + while time.time() - ts < timeout: + answer = self.link.receive_packet(2) + if answer is None: self.link.send_packet(pk) - break + continue + if answer.port == 15 and answer.channel == 3 and len(answer.data) > 2: + if struct.unpack(' Date: Wed, 9 Jun 2021 07:07:07 +0200 Subject: [PATCH 210/861] bootloader: Make reset_to_firmware return a bool --- cflib/bootloader/__init__.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 6a8271e0b..8f7b0d988 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -241,11 +241,11 @@ def _flash_flash(self, artifacts: List[FlashArtifact], targets: List[Target]): for (i, artifact) in enumerate(artifacts): self._internal_flash(artifact, i + 1, len(artifacts)) - def reset_to_firmware(self): + def reset_to_firmware(self) -> bool: if self._cload.protocol_version == BootVersion.CF2_PROTO_VER: - self._cload.reset_to_firmware(TargetTypes.NRF51) + return self._cload.reset_to_firmware(TargetTypes.NRF51) else: - self._cload.reset_to_firmware(TargetTypes.STM32) + return self._cload.reset_to_firmware(TargetTypes.STM32) def close(self): if self._cload: From 7dcacd148bbb0ccc170c46aca43a6f086a8eddf3 Mon Sep 17 00:00:00 2001 From: Dominik Natter Date: Sun, 13 Jun 2021 18:26:54 +0200 Subject: [PATCH 211/861] update the example to fit the tutorial in the docs --- examples/step-by-step/sbs_motion_commander.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index 63a51e245..d6b033829 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -86,7 +86,6 @@ def move_linear_simple(scf): def take_off_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) - mc.stop() def log_pos_callback(timestamp, data, logconf): From 5ddb4c5f515c0630f9ca80b476c8f11ed80e5883 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Jun 2021 16:30:34 +0200 Subject: [PATCH 212/861] fixed build issue --- examples/step-by-step/sbs_motion_commander.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index d6b033829..63a51e245 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -86,6 +86,7 @@ def move_linear_simple(scf): def take_off_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) + mc.stop() def log_pos_callback(timestamp, data, logconf): From 46df53a71f4919026989ac6c6de34672ebf5ce1f Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 17 Jun 2021 07:38:09 +0200 Subject: [PATCH 213/861] cloader: Add retry timeout to update_info We have seen that this can fail in some paths, and we might need a retry since this is talking to bootloader. --- cflib/bootloader/cloader.py | 72 +++++++++++++++++++------------------ 1 file changed, 38 insertions(+), 34 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 2f0435f20..e2ee99629 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -156,16 +156,15 @@ def open_bootloader_uri(self, uri=None): self.clink_address + '?safelink=0') def check_link_and_get_info(self, target_id=0xFF): - """Try to get a connection with the bootloader by requesting info - 5 times. This let roughly 10 seconds to boot the copter ...""" - for _ in range(0, 5): - if self._update_info(target_id): - if self._in_boot_cb: - self._in_boot_cb.call(True, self.targets[ - target_id].protocol_version) - if self._info_cb: - self._info_cb.call(self.targets[target_id]) - return True + """Try to get a connection with the bootloader ... + update_info has a timeout of 10 seconds """ + if self._update_info(target_id): + if self._in_boot_cb: + self._in_boot_cb.call(True, self.targets[ + target_id].protocol_version) + if self._info_cb: + self._info_cb.call(self.targets[target_id]) + return True return False def request_info_update(self, target_id): @@ -186,32 +185,37 @@ def _update_info(self, target_id): pk.data = (target_id, 0x10) self.link.send_packet(pk) - # Wait for the answer - pk = self.link.receive_packet(2) + timeout = 10 # seconds + ts = time.time() + while time.time() - ts < timeout: + # Wait for the answer + answer = self.link.receive_packet(2) + if answer is None: + self.link.send_packet(pk) - if (pk and pk.header == 0xFF and struct.unpack(' 22: - self.targets[target_id].protocol_version = pk.datat[22] - self.protocol_version = pk.datat[22] - self.targets[target_id].page_size = tab[2] - self.targets[target_id].buffer_pages = tab[3] - self.targets[target_id].flash_pages = tab[4] - self.targets[target_id].start_page = tab[5] - self.targets[target_id].cpuid = '%02X' % cpuid[0] - for i in cpuid[1:]: - self.targets[target_id].cpuid += ':%02X' % i - - if (self.protocol_version == 0x10 and - target_id == TargetTypes.STM32): - self._update_mapping(target_id) + if (answer and answer.header == 0xFF and struct.unpack(' 22: + self.targets[target_id].protocol_version = answer.datat[22] + self.protocol_version = answer.datat[22] + self.targets[target_id].page_size = tab[2] + self.targets[target_id].buffer_pages = tab[3] + self.targets[target_id].flash_pages = tab[4] + self.targets[target_id].start_page = tab[5] + self.targets[target_id].cpuid = '%02X' % cpuid[0] + for i in cpuid[1:]: + self.targets[target_id].cpuid += ':%02X' % i + + if (self.protocol_version == 0x10 and + target_id == TargetTypes.STM32): + self._update_mapping(target_id) - return True + return True return False From 67171f683a74defe6ea8b3e6e65536513f713419 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 23 Jun 2021 13:27:28 +0200 Subject: [PATCH 214/861] examples: read-ow: Add timeout and check for success --- examples/read-ow.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/examples/read-ow.py b/examples/read-ow.py index 38132677f..ae6c10f3e 100644 --- a/examples/read-ow.py +++ b/examples/read-ow.py @@ -53,6 +53,9 @@ def __init__(self, link_uri): # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() + # Keep track on if ow memory was detected + self.read_ow = False + # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) @@ -76,6 +79,11 @@ def _connected(self, link_uri): mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) self._mems_to_update = len(mems) print('Found {} 1-wire memories'.format(len(mems))) + + if len(mems) == 0: + self.read_ow = True + self._cf.close_link() + for m in mems: print('Updating id={}'.format(m.id)) m.update(self._data_updated) @@ -97,6 +105,7 @@ def _data_updated(self, mem): self._mems_to_update -= 1 if self._mems_to_update == 0: + self.read_ow = True self._cf.close_link() def _stab_log_error(self, logconf, msg): @@ -132,9 +141,14 @@ def _disconnected(self, 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. + # are just waiting until we are disconnected, or timeout. + timeout = 5 # seconds + ts = time.time() try: - while le.is_connected: + while le.is_connected and (time.time() - ts < timeout): time.sleep(1) except KeyboardInterrupt: sys.exit(1) + + if not le.read_ow: + sys.exit(1) From a40bc28754ca4f4db36b06f871662837b8e9b461 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 23 Jun 2021 14:03:58 +0200 Subject: [PATCH 215/861] read_deck_mem: use uri_helper --- examples/read_deck_mem.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index f94a79c5f..4561e3cff 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -31,6 +31,7 @@ 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) @@ -91,7 +92,7 @@ def read_failed(self, addr): if __name__ == '__main__': # URI to the Crazyflie to connect to - uri = 'radio://0/80' + uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Initialize the low-level drivers cflib.crtp.init_drivers() From e19da0c6d6ac8a15e2c963eed818c5b7ee9f99d9 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 23 Jun 2021 14:11:34 +0200 Subject: [PATCH 216/861] read_mem: Check for success --- examples/read_deck_mem.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index 4561e3cff..7c7e5f542 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -25,6 +25,7 @@ Example of how to read the memory from a deck """ import logging +import sys from threading import Event import cflib.crtp # noqa @@ -40,8 +41,10 @@ class ReadMem: def __init__(self, uri): self._event = Event() + self._cf = Crazyflie(rw_cache='./cache') + self.read_mem = False - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + with SyncCrazyflie(uri, cf=cf) as scf: mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) count = len(mems) @@ -83,6 +86,7 @@ def query_complete_cb(self, deck_memories): def read_complete(self, addr, data): print(data) + self.read_mem = True self._event.set() def read_failed(self, addr): @@ -97,4 +101,6 @@ def read_failed(self, addr): # Initialize the low-level drivers cflib.crtp.init_drivers() - ReadMem(uri) + rm = ReadMem(uri) + if not rm.read_mem: + sys.exit(1) From 0bc073df5c3c6e9e3f5e33844f44d9206eaa153e Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 23 Jun 2021 14:14:12 +0200 Subject: [PATCH 217/861] read_mem: fix typo --- examples/read_deck_mem.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index 7c7e5f542..7e9670b02 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -44,7 +44,7 @@ def __init__(self, uri): self._cf = Crazyflie(rw_cache='./cache') self.read_mem = False - with SyncCrazyflie(uri, cf=cf) as scf: + with SyncCrazyflie(uri, cf=self._cf) as scf: mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) count = len(mems) From 982b89a4a1fb77db40c7c30a11a8a480e3f5676c Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 23 Jun 2021 14:22:44 +0200 Subject: [PATCH 218/861] read_deck_mem: Assume success if no deck memories --- examples/read_deck_mem.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index 7e9670b02..380ba0e24 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -55,6 +55,10 @@ def __init__(self, uri): mem.query_decks(self.query_complete_cb) self._event.wait() + if len(mem.deck_memories.items()) == 0: + print('No memories to read') + self.read_mem = True + for id, deck_mem in mem.deck_memories.items(): print('-----') print('Deck id:', id) From 6172bb2bb5eaa79d400b5e1a6cc3a898d3397d28 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 24 Jun 2021 05:57:33 +0200 Subject: [PATCH 219/861] read_deck_mem: Do not report success or fail --- examples/read_deck_mem.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index 380ba0e24..f550f9944 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -42,7 +42,6 @@ class ReadMem: def __init__(self, uri): self._event = Event() self._cf = Crazyflie(rw_cache='./cache') - self.read_mem = False with SyncCrazyflie(uri, cf=self._cf) as scf: mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) @@ -57,7 +56,6 @@ def __init__(self, uri): if len(mem.deck_memories.items()) == 0: print('No memories to read') - self.read_mem = True for id, deck_mem in mem.deck_memories.items(): print('-----') @@ -90,7 +88,6 @@ def query_complete_cb(self, deck_memories): def read_complete(self, addr, data): print(data) - self.read_mem = True self._event.set() def read_failed(self, addr): @@ -106,5 +103,3 @@ def read_failed(self, addr): cflib.crtp.init_drivers() rm = ReadMem(uri) - if not rm.read_mem: - sys.exit(1) From 15b31f1caf9e420285f1c9385434199f130ee1b9 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 24 Jun 2021 06:00:10 +0200 Subject: [PATCH 220/861] sbs_connect_log_param: Use uri_helper --- docs/user-guides/sbs_connect_log_param.md | 5 +++-- examples/step-by-step/sbs_connect_log_param.py | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index 243fca9bf..08b380179 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -47,6 +47,7 @@ then you need to import the libraries for cflib: import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper ``` * The cflib.crtp module is for scanning for Crazyflies instances. @@ -62,10 +63,10 @@ After these imports, start the script with: ```python # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M/E7E7E7E7E7' +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') ``` -This is the radio uri of the crazyflie, and currently displaying the default. It should be probably fine but if you do not know what the uri of your Crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) +This is the radio uri of the crazyflie, it can be set by setting the environment variable `CFLIB_URI`, if not set it uses the default. It should be probably fine but if you do not know what the uri of your Crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) ## Main diff --git a/examples/step-by-step/sbs_connect_log_param.py b/examples/step-by-step/sbs_connect_log_param.py index 45536a018..1bba06e89 100644 --- a/examples/step-by-step/sbs_connect_log_param.py +++ b/examples/step-by-step/sbs_connect_log_param.py @@ -31,9 +31,10 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M/E7E7E7E7E7' +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) From d2127e726cbda225892066d648c2edb9e4e36623 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 28 Jun 2021 09:16:34 +0200 Subject: [PATCH 221/861] read_deck_mem: Remove unused import --- examples/read_deck_mem.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/read_deck_mem.py b/examples/read_deck_mem.py index f550f9944..35dda99a8 100644 --- a/examples/read_deck_mem.py +++ b/examples/read_deck_mem.py @@ -25,7 +25,6 @@ Example of how to read the memory from a deck """ import logging -import sys from threading import Event import cflib.crtp # noqa From a978b1c484982d3d5ea6ea608594c2d098bb728b Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 28 Jun 2021 13:42:02 +0200 Subject: [PATCH 222/861] Crazyflie: Wait for parameters to update --- cflib/crazyflie/__init__.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 24ef331f9..05a043402 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -165,11 +165,15 @@ def _start_connection_setup(self): def _platform_info_fetched(self): self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache) + def _param_values_updated_cb(self): + logger.info('Param values finished updating') + self.connected_ts = datetime.datetime.now() + self.connected.call(self.link_uri) + def _param_toc_updated_cb(self): """Called when the param TOC has been fully updated""" logger.info('Param TOC finished updating') - self.connected_ts = datetime.datetime.now() - self.connected.call(self.link_uri) + self.param.all_updated.add_callback(self._param_values_updated_cb) # Trigger the update for all the parameters self.param.request_update_of_all_params() From abb15fb9896e7dd4d2acd36e9cabf45db7fb8a24 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 28 Jun 2021 13:42:37 +0200 Subject: [PATCH 223/861] param.py: Add get_value to parameter class --- cflib/crazyflie/param.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 238735fbb..74239dac8 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -278,6 +278,17 @@ def set_value(self, complete_name, value): pk.data += struct.pack(element.pytype, value_nr) self.param_updater.request_param_setvalue(pk) + def get_value(self, complete_name): + """ + Read a value for the supplied parameter. If None then the value has + not been updated yet, retry. + """ + try: + [group, name] = complete_name.split('.') + return self.values[group][name] + except KeyError: + return None + class _ParamUpdater(Thread): """This thread will update params through a queue to make sure that we From 74f15dce948d8cfbaa8a29f3cc23438ea6ed1e90 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 29 Jun 2021 08:04:48 +0200 Subject: [PATCH 224/861] bootloader: Close link after reset_to_firmware When we have successfully reset to firmware we should not keep the link to the bootloader anymore. It confuses the state and causes bugs. It will have start_bootloader() return True because it thinks the link is already active. Fixes: bitcraze/crazyflie-clients-python#520 --- cflib/bootloader/__init__.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 8f7b0d988..7cd791da2 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -242,14 +242,21 @@ def _flash_flash(self, artifacts: List[FlashArtifact], targets: List[Target]): self._internal_flash(artifact, i + 1, len(artifacts)) def reset_to_firmware(self) -> bool: + status = False if self._cload.protocol_version == BootVersion.CF2_PROTO_VER: - return self._cload.reset_to_firmware(TargetTypes.NRF51) + status = self._cload.reset_to_firmware(TargetTypes.NRF51) else: - return self._cload.reset_to_firmware(TargetTypes.STM32) + status = self._cload.reset_to_firmware(TargetTypes.STM32) + + if status: + self.close() + + return status def close(self): if self._cload: self._cload.close() + self._cload.link = None def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_files=1): From 3e52395e638501ab660e583561a30a32cbfc7103 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 5 Jul 2021 07:18:56 +0200 Subject: [PATCH 225/861] Update version to 0.1.16 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 6db7a48b1..ffbfd28d8 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ setup( name='cflib', - version='0.1.15', + version='0.1.16', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From e655a794ce41ffbf4cade7b4b48179ddae124bc3 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 9 Aug 2021 05:54:34 +0200 Subject: [PATCH 226/861] Revert "Crazyflie: Wait for parameters to update" This reverts commit a978b1c484982d3d5ea6ea608594c2d098bb728b. --- cflib/crazyflie/__init__.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 05a043402..24ef331f9 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -165,15 +165,11 @@ def _start_connection_setup(self): def _platform_info_fetched(self): self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache) - def _param_values_updated_cb(self): - logger.info('Param values finished updating') - self.connected_ts = datetime.datetime.now() - self.connected.call(self.link_uri) - def _param_toc_updated_cb(self): """Called when the param TOC has been fully updated""" logger.info('Param TOC finished updating') - self.param.all_updated.add_callback(self._param_values_updated_cb) + self.connected_ts = datetime.datetime.now() + self.connected.call(self.link_uri) # Trigger the update for all the parameters self.param.request_update_of_all_params() From bc5dfdf81bb00678803993252da3bb7091aea10b Mon Sep 17 00:00:00 2001 From: Ricky <32981439+chengliu-LR@users.noreply.github.com> Date: Thu, 2 Sep 2021 16:26:58 +0200 Subject: [PATCH 227/861] Update __init__.py Propose to fix a little grammar error from 'know' to 'known' --- cflib/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/__init__.py b/cflib/__init__.py index 516ffc088..17d9c3d50 100644 --- a/cflib/__init__.py +++ b/cflib/__init__.py @@ -45,7 +45,7 @@ print "Found Crazyflie on URI [%s] with comment [%s]" % (available[0], available[1]) -Example of connecting to a Crazyflie with know URI (radio dongle 0 and +Example of connecting to a Crazyflie with known URI (radio dongle 0 and radio channel 125): cf = Crazyflie() cf.open_link("radio://0/125") From aa484bba2d3b27a6bfe136b7665146abbfc675e1 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Sep 2021 06:57:09 +0200 Subject: [PATCH 228/861] Add additional decription for package Use our README.md to provide a long description for the cflib package on pypi.org. Closes: #155 --- setup.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/setup.py b/setup.py index ffbfd28d8..244ea8e16 100644 --- a/setup.py +++ b/setup.py @@ -1,6 +1,11 @@ #!/usr/bin/env python3 +from pathlib import Path + from setuptools import find_packages from setuptools import setup +# read the contents of README.md file fo use in pypi description +directory = Path(__file__).parent +long_description = (directory / 'README.md').read_text() setup( name='cflib', @@ -10,6 +15,9 @@ description='Crazyflie python driver', url='https://github.com/bitcraze/crazyflie-lib-python', + long_description=long_description, + long_description_content_type='text/markdown', + author='Bitcraze and contributors', author_email='contact@bitcraze.io', license='GPLv3', From aa76cb873892a9db10454beb040afd2198fb6e06 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Sep 2021 07:12:51 +0200 Subject: [PATCH 229/861] Reorganize examples into subdirectories --- examples/{ => loco_nodes}/lps_reboot_to_bootloader.py | 0 examples/{ => logging}/basiclog.py | 0 examples/{ => logging}/basiclogSync.py | 0 examples/{ => memory}/flash-memory.py | 0 examples/{ => memory}/read-eeprom.py | 0 examples/{ => memory}/read_deck_mem.py | 0 examples/{ => memory}/write-eeprom.py | 0 examples/{ => motors}/multiramp.py | 0 examples/{ => motors}/ramp.py | 0 examples/{ => multiranger}/multiranger_pointcloud.py | 0 examples/{ => multiranger}/multiranger_push.py | 0 examples/{ => onewire}/erase-ow.py | 0 examples/{ => onewire}/read-ow.py | 0 examples/{ => onewire}/write-ow.py | 0 examples/{ => parameters}/basicparam.py | 0 examples/{ => positioning}/autonomousSequence.py | 0 examples/{ => positioning}/autonomous_sequence_high_level.py | 0 examples/{ => positioning}/flowsequenceSync.py | 0 examples/{ => positioning}/motion_commander_demo.py | 0 examples/{ => positioning}/position_commander_demo.py | 0 examples/{ => radio}/radio-test.py | 0 examples/{ => radio}/scan.py | 0 22 files changed, 0 insertions(+), 0 deletions(-) rename examples/{ => loco_nodes}/lps_reboot_to_bootloader.py (100%) rename examples/{ => logging}/basiclog.py (100%) rename examples/{ => logging}/basiclogSync.py (100%) rename examples/{ => memory}/flash-memory.py (100%) rename examples/{ => memory}/read-eeprom.py (100%) rename examples/{ => memory}/read_deck_mem.py (100%) rename examples/{ => memory}/write-eeprom.py (100%) rename examples/{ => motors}/multiramp.py (100%) rename examples/{ => motors}/ramp.py (100%) rename examples/{ => multiranger}/multiranger_pointcloud.py (100%) rename examples/{ => multiranger}/multiranger_push.py (100%) rename examples/{ => onewire}/erase-ow.py (100%) rename examples/{ => onewire}/read-ow.py (100%) rename examples/{ => onewire}/write-ow.py (100%) rename examples/{ => parameters}/basicparam.py (100%) rename examples/{ => positioning}/autonomousSequence.py (100%) rename examples/{ => positioning}/autonomous_sequence_high_level.py (100%) rename examples/{ => positioning}/flowsequenceSync.py (100%) rename examples/{ => positioning}/motion_commander_demo.py (100%) rename examples/{ => positioning}/position_commander_demo.py (100%) rename examples/{ => radio}/radio-test.py (100%) rename examples/{ => radio}/scan.py (100%) diff --git a/examples/lps_reboot_to_bootloader.py b/examples/loco_nodes/lps_reboot_to_bootloader.py similarity index 100% rename from examples/lps_reboot_to_bootloader.py rename to examples/loco_nodes/lps_reboot_to_bootloader.py diff --git a/examples/basiclog.py b/examples/logging/basiclog.py similarity index 100% rename from examples/basiclog.py rename to examples/logging/basiclog.py diff --git a/examples/basiclogSync.py b/examples/logging/basiclogSync.py similarity index 100% rename from examples/basiclogSync.py rename to examples/logging/basiclogSync.py diff --git a/examples/flash-memory.py b/examples/memory/flash-memory.py similarity index 100% rename from examples/flash-memory.py rename to examples/memory/flash-memory.py diff --git a/examples/read-eeprom.py b/examples/memory/read-eeprom.py similarity index 100% rename from examples/read-eeprom.py rename to examples/memory/read-eeprom.py diff --git a/examples/read_deck_mem.py b/examples/memory/read_deck_mem.py similarity index 100% rename from examples/read_deck_mem.py rename to examples/memory/read_deck_mem.py diff --git a/examples/write-eeprom.py b/examples/memory/write-eeprom.py similarity index 100% rename from examples/write-eeprom.py rename to examples/memory/write-eeprom.py diff --git a/examples/multiramp.py b/examples/motors/multiramp.py similarity index 100% rename from examples/multiramp.py rename to examples/motors/multiramp.py diff --git a/examples/ramp.py b/examples/motors/ramp.py similarity index 100% rename from examples/ramp.py rename to examples/motors/ramp.py diff --git a/examples/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py similarity index 100% rename from examples/multiranger_pointcloud.py rename to examples/multiranger/multiranger_pointcloud.py diff --git a/examples/multiranger_push.py b/examples/multiranger/multiranger_push.py similarity index 100% rename from examples/multiranger_push.py rename to examples/multiranger/multiranger_push.py diff --git a/examples/erase-ow.py b/examples/onewire/erase-ow.py similarity index 100% rename from examples/erase-ow.py rename to examples/onewire/erase-ow.py diff --git a/examples/read-ow.py b/examples/onewire/read-ow.py similarity index 100% rename from examples/read-ow.py rename to examples/onewire/read-ow.py diff --git a/examples/write-ow.py b/examples/onewire/write-ow.py similarity index 100% rename from examples/write-ow.py rename to examples/onewire/write-ow.py diff --git a/examples/basicparam.py b/examples/parameters/basicparam.py similarity index 100% rename from examples/basicparam.py rename to examples/parameters/basicparam.py diff --git a/examples/autonomousSequence.py b/examples/positioning/autonomousSequence.py similarity index 100% rename from examples/autonomousSequence.py rename to examples/positioning/autonomousSequence.py diff --git a/examples/autonomous_sequence_high_level.py b/examples/positioning/autonomous_sequence_high_level.py similarity index 100% rename from examples/autonomous_sequence_high_level.py rename to examples/positioning/autonomous_sequence_high_level.py diff --git a/examples/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py similarity index 100% rename from examples/flowsequenceSync.py rename to examples/positioning/flowsequenceSync.py diff --git a/examples/motion_commander_demo.py b/examples/positioning/motion_commander_demo.py similarity index 100% rename from examples/motion_commander_demo.py rename to examples/positioning/motion_commander_demo.py diff --git a/examples/position_commander_demo.py b/examples/positioning/position_commander_demo.py similarity index 100% rename from examples/position_commander_demo.py rename to examples/positioning/position_commander_demo.py diff --git a/examples/radio-test.py b/examples/radio/radio-test.py similarity index 100% rename from examples/radio-test.py rename to examples/radio/radio-test.py diff --git a/examples/scan.py b/examples/radio/scan.py similarity index 100% rename from examples/scan.py rename to examples/radio/scan.py From c650c37e496deb121fca95303e8c254fe9ceb957 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Sep 2021 07:13:10 +0200 Subject: [PATCH 230/861] Remove unsupported cfbridge example --- examples/cfbridge.py | 135 ------------------------------------------- 1 file changed, 135 deletions(-) delete mode 100755 examples/cfbridge.py diff --git a/examples/cfbridge.py b/examples/cfbridge.py deleted file mode 100755 index ca02b2e56..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) From cfd1209d39f303d68468ea841f9320c09f0445bc Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Sep 2021 13:43:18 +0200 Subject: [PATCH 231/861] install.md: Fix path to example script --- docs/installation/install.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index e9f0003b7..2da50af98 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -36,7 +36,7 @@ create an environment, activate it and install dependencies. #### Install cflib dependencies Install dependencies required by the lib: `pip install -r requirements.txt`. If you are planning on developing on the lib you should also run: `pip install -r requirements-dev.txt`. -To verify the installation, connect the crazyflie and run an example: `python examples/basiclog` +To verify the installation, connect the crazyflie and run an example: `python3 examples/logging/basiclog.py` ### Pre commit hooks If you want some extra help with keeping to the mandated python coding style you can install hooks that verify your style at commit time. This is done by running: From fa2bd0bbb32f97910a4c22ab86e01c7c2965efe7 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Sep 2021 14:50:48 +0200 Subject: [PATCH 232/861] examples: Create autonomy subfolder --- examples/{positioning => autonomy}/autonomousSequence.py | 0 .../{positioning => autonomy}/autonomous_sequence_high_level.py | 0 examples/{positioning => autonomy}/motion_commander_demo.py | 0 examples/{positioning => autonomy}/position_commander_demo.py | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename examples/{positioning => autonomy}/autonomousSequence.py (100%) rename examples/{positioning => autonomy}/autonomous_sequence_high_level.py (100%) rename examples/{positioning => autonomy}/motion_commander_demo.py (100%) rename examples/{positioning => autonomy}/position_commander_demo.py (100%) diff --git a/examples/positioning/autonomousSequence.py b/examples/autonomy/autonomousSequence.py similarity index 100% rename from examples/positioning/autonomousSequence.py rename to examples/autonomy/autonomousSequence.py diff --git a/examples/positioning/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py similarity index 100% rename from examples/positioning/autonomous_sequence_high_level.py rename to examples/autonomy/autonomous_sequence_high_level.py diff --git a/examples/positioning/motion_commander_demo.py b/examples/autonomy/motion_commander_demo.py similarity index 100% rename from examples/positioning/motion_commander_demo.py rename to examples/autonomy/motion_commander_demo.py diff --git a/examples/positioning/position_commander_demo.py b/examples/autonomy/position_commander_demo.py similarity index 100% rename from examples/positioning/position_commander_demo.py rename to examples/autonomy/position_commander_demo.py From 9dcfaecb6177006e6d9b5eca94dac920c8f1cec4 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Sep 2021 14:52:11 +0200 Subject: [PATCH 233/861] examples: Move onewire examples to memory subfolder --- examples/{onewire => memory}/erase-ow.py | 0 examples/{onewire => memory}/read-ow.py | 0 examples/{onewire => memory}/write-ow.py | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename examples/{onewire => memory}/erase-ow.py (100%) rename examples/{onewire => memory}/read-ow.py (100%) rename examples/{onewire => memory}/write-ow.py (100%) diff --git a/examples/onewire/erase-ow.py b/examples/memory/erase-ow.py similarity index 100% rename from examples/onewire/erase-ow.py rename to examples/memory/erase-ow.py diff --git a/examples/onewire/read-ow.py b/examples/memory/read-ow.py similarity index 100% rename from examples/onewire/read-ow.py rename to examples/memory/read-ow.py diff --git a/examples/onewire/write-ow.py b/examples/memory/write-ow.py similarity index 100% rename from examples/onewire/write-ow.py rename to examples/memory/write-ow.py From f4f81dc3640ea73e60dd4e8f897cf09d9364f422 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 8 Sep 2021 09:58:23 +0200 Subject: [PATCH 234/861] Make USB permission easier to add on Linux Use cat and tee to make a block that can be copy-pasted in a console. It makes it easier to add the required udev rules file. --- docs/installation/usb_permissions.md | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/docs/installation/usb_permissions.md b/docs/installation/usb_permissions.md index e88959e7c..5fc5cf71b 100644 --- a/docs/installation/usb_permissions.md +++ b/docs/installation/usb_permissions.md @@ -5,7 +5,7 @@ page_id: usd_permissions ### Linux -The following steps make it possible to use the USB Radio without being root. +The following steps make it possible to use the USB Radio and Crazyflie 2 over USB without being root. ``` sudo groupadd plugdev @@ -14,18 +14,16 @@ sudo usermod -a -G plugdev $USER You will need to log out and log in again in order to be a member of the plugdev group. -Create a file named ```/etc/udev/rules.d/99-crazyradio.rules``` and add the -following: +Copy-paste the following in your console, this will create the file ```/etc/udev/rules.d/99-bitcraze.rules```: ``` +cat < /dev/null # Crazyradio (normal operation) SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="7777", MODE="0664", GROUP="plugdev" # Bootloader SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="0101", MODE="0664", GROUP="plugdev" -``` - -To connect Crazyflie 2.0 via usb, create a file name ```/etc/udev/rules.d/99-crazyflie.rules``` and add the following: -``` +# Crazyflie (over USB) SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE="0664", GROUP="plugdev" +EOF ``` You can reload the udev-rules using the following: From ee71aed6bde7950554fcde8d07b987b31c5e2741 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 14 Sep 2021 17:16:01 +0200 Subject: [PATCH 235/861] Decode radio uri with urllib and add rate_limit Fixes #257 --- cflib/crtp/radiodriver.py | 67 +++++++++++++++++++++++---------------- 1 file changed, 40 insertions(+), 27 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 9f6b40d99..f67d3bc0a 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -38,6 +38,7 @@ import re import struct import threading +import time from enum import Enum from queue import Queue from threading import Semaphore @@ -46,8 +47,11 @@ from typing import Dict from typing import Iterable from typing import List +from typing import Optional from typing import Tuple from typing import Union +from urllib.parse import parse_qs +from urllib.parse import urlparse import cflib.drivers.crazyradio as crazyradio from .crtpstack import CRTPPacket @@ -258,7 +262,7 @@ def connect(self, uri, link_quality_callback, link_error_callback): an error message. """ - devid, channel, datarate, address = self.parse_uri(uri) + devid, channel, datarate, address, rate_limit = self.parse_uri(uri) self.uri = uri if self._radio is None: @@ -285,56 +289,59 @@ def connect(self, uri, link_quality_callback, link_error_callback): self.out_queue, link_quality_callback, link_error_callback, - self) + self, + rate_limit) self._thread.start() self.link_error_callback = link_error_callback @staticmethod - def parse_uri(uri): + def parse_uri(uri: str): # check if the URI is a radio URI - if not re.search('^radio://', uri): + if not uri.startswith('radio://'): raise WrongUriType('Not a radio URI') - # Open the USB dongle - if not re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))' - '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\\?.+)?$', uri): - raise WrongUriType('Wrong radio URI format!') - - uri_data = re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))' - '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?(\\?.+)?$', uri) + parsed_uri = urlparse(uri) + parsed_query = parse_qs(parsed_uri.query) + parsed_path = parsed_uri.path.strip('/').split('/') - if len(uri_data.group(1)) < 10 and uri_data.group(1).isdigit(): - devid = int(uri_data.group(1)) + # Open the USB dongle + if len(parsed_uri.netloc) < 10 and parsed_uri.netloc.isdigit(): + devid = int(parsed_uri.netloc) else: try: devid = crazyradio.get_serials().index( - uri_data.group(1).upper()) + parsed_uri.netloc.upper()) except ValueError: raise Exception('Cannot find radio with serial {}'.format( - uri_data.group(1))) + parsed_uri.netloc)) channel = 2 - if uri_data.group(4): - channel = int(uri_data.group(4)) + if len(parsed_path) > 0: + channel = int(parsed_path[0]) datarate = Crazyradio.DR_2MPS - if uri_data.group(7) == '250K': - datarate = Crazyradio.DR_250KPS - if uri_data.group(7) == '1M': - datarate = Crazyradio.DR_1MPS - if uri_data.group(7) == '2M': - datarate = Crazyradio.DR_2MPS + if len(parsed_path) > 1: + if parsed_path[1] == '250K': + datarate = Crazyradio.DR_250KPS + if parsed_path[1] == '1M': + datarate = Crazyradio.DR_1MPS + if parsed_path[1] == '2M': + datarate = Crazyradio.DR_2MPS address = DEFAULT_ADDR_A - if uri_data.group(9): + if len(parsed_path) > 2: # We make sure to pad the address with zeros if we do not have the # correct length. - addr = '{:0>10}'.format(uri_data.group(9)) + addr = '{:0>10}'.format(parsed_path[2]) new_addr = struct.unpack(' Date: Mon, 20 Sep 2021 10:22:34 +0200 Subject: [PATCH 236/861] param: Add `set_value_raw()` This uses the MISC_CHANNEL => MISC_SETBYNAME path in the firmware which was previously unused in the python lib. Crazyswarm uses it though, so it is good to have it, so we can make sure it does not break. --- cflib/crazyflie/param.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 74239dac8..e05f593af 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -58,6 +58,7 @@ TOC_CHANNEL = 0 READ_CHANNEL = 1 WRITE_CHANNEL = 2 +MISC_CHANNEL = 3 # One element entry in the TOC @@ -247,6 +248,25 @@ def request_param_update(self, complete_name): self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) + def set_value_raw(self, complete_name, type, value): + """ + Set a parameter value using the complete name and the type. Does not + need to have received the TOC. + """ + char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') + len_array = len(char_array) + + # This gives us the type for the struct.pack + pytype = ParamTocElement.types[type][1][1] + + pk = CRTPPacket() + pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) + pk.data = struct.pack(f' Date: Thu, 30 Sep 2021 06:49:50 +0200 Subject: [PATCH 237/861] swarm: Add reset_estimators member function All our examples implement their own, lets make it part of the lib instead. --- cflib/crazyflie/swarm.py | 53 +++++++++++++++++++++++++ examples/swarm/hl-commander-swarm.py | 53 +------------------------ examples/swarm/swarmSequence.py | 54 +------------------------- examples/swarm/swarmSequenceCircle.py | 10 +---- examples/swarm/synchronizedSequence.py | 53 +------------------------ 5 files changed, 57 insertions(+), 166 deletions(-) diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index 5355623c8..9687c37e0 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -21,10 +21,13 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. +import time from threading import Thread from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.syncLogger import SyncLogger class _Factory: @@ -107,6 +110,56 @@ def __enter__(self): def __exit__(self, exc_type, exc_val, exc_tb): self.close_links() + + def __wait_for_position_estimator(self, scf): + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + def __reset_estimator(self, scf): + cf = scf.cf + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + self.__wait_for_position_estimator(scf) + + def reset_estimators(self): + """ + Reset estimator on all members of the swarm and wait for a stable + positions. + """ + self.parallel_safe(self.__reset_estimator) + def sequential(self, func, args_dict=None): """ Execute a function for all Crazyflies in the swarm, in sequence. diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index c8493c5aa..d1319c388 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -35,59 +35,8 @@ 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.crazyflie.syncLogger import SyncLogger - - -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - wait_for_position_estimator(scf) def activate_high_level_commander(scf): @@ -141,5 +90,5 @@ def run_shared_sequence(scf): factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(activate_high_level_commander) - swarm.parallel_safe(reset_estimator) + swarm.reset_estimators() swarm.parallel_safe(run_shared_sequence) diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py index a18916918..91ca0cacb 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -50,10 +50,8 @@ 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.crazyflie.syncLogger import SyncLogger # Change uris and sequences according to your setup URI1 = 'radio://0/70/2M/E7E7E7E701' @@ -169,62 +167,12 @@ } -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - 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 reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def take_off(cf, position): take_off_time = 1.0 sleep_time = 0.1 @@ -284,7 +232,7 @@ def run_sequence(scf, sequence): # 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.parallel(reset_estimator) + # 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 diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index 7c0522efe..a9c30d9e3 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -78,14 +78,6 @@ } -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - time.sleep(2) - - def poshold(cf, t, z): steps = t * 10 @@ -146,5 +138,5 @@ def run_sequence(scf, params): factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: - swarm.parallel(reset_estimator) + swarm.reset_estimators() swarm.parallel(run_sequence, args_dict=params) diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index c29e41a65..9e462c3c9 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -40,10 +40,8 @@ from queue import Queue import cflib.crtp -from cflib.crazyflie.log import LogConfig from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm -from cflib.crazyflie.syncLogger import SyncLogger # Time for one step in second STEP_TIME = 1 @@ -102,55 +100,6 @@ ] -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - wait_for_position_estimator(scf) - - def activate_high_level_commander(scf): scf.cf.param.set_value('commander.enHighLevel', '1') @@ -239,7 +188,7 @@ def control_thread(): factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(activate_high_level_commander) - swarm.parallel_safe(reset_estimator) + swarm.reset_estimators() print('Starting sequence!') From 98f72b480b6de2d868e02e0628c005f8a67d755b Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 30 Sep 2021 06:51:16 +0200 Subject: [PATCH 238/861] swarm: Add member function get_estimated_positions This will return the estimated positions for all members of the swarm, in the form of a dict: { 'radio_link_uri': SwarmPosition(x, y, z) } ... --- cflib/crazyflie/swarm.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index 9687c37e0..30371a58c 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -22,6 +22,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import time +from collections import namedtuple from threading import Thread from cflib.crazyflie import Crazyflie @@ -29,6 +30,8 @@ from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger +SwarmPosition = namedtuple('SwarmPosition', 'x y z') + class _Factory: """ @@ -76,6 +79,7 @@ def __init__(self, uris, factory=_Factory()): """ self._cfs = {} self._is_open = False + self._positions = dict() for uri in uris: self._cfs[uri] = factory.construct(uri) @@ -110,6 +114,27 @@ def __enter__(self): def __exit__(self, exc_type, exc_val, exc_tb): self.close_links() + def __get_estimated_position(self, scf): + log_config = LogConfig(name='stateEstimate', period_in_ms=10) + log_config.add_variable('stateEstimate.x', 'float') + log_config.add_variable('stateEstimate.y', 'float') + log_config.add_variable('stateEstimate.z', 'float') + + with SyncLogger(scf, log_config) as logger: + for entry in logger: + x = entry[1]['stateEstimate.x'] + y = entry[1]['stateEstimate.y'] + z = entry[1]['stateEstimate.z'] + self._positions[scf.cf.link_uri] = SwarmPosition(x, y, z) + break + + def get_estimated_positions(self): + """ + Return a dict of the SwarmPosition namedtuple with the estimated + (x, y, z) of each Crazyflie in the swarm. The URIs are the key. + """ + self.parallel_safe(self.__get_estimated_position) + return self._positions def __wait_for_position_estimator(self, scf): log_config = LogConfig(name='Kalman Variance', period_in_ms=500) From 53bd8b4c2187138465ad3fc666e945161b49884d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 4 Oct 2021 14:44:52 +0200 Subject: [PATCH 239/861] param: Add _initialized semaphore This semaphore will be `set()` when all values have been updated at least once. This will allow us to wait in get_value to make sure we have values. --- cflib/crazyflie/param.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index e05f593af..b96153064 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -34,6 +34,7 @@ import logging import struct from queue import Queue +from threading import Event from threading import Lock from threading import Thread @@ -133,6 +134,7 @@ def __init__(self, crazyflie): self.all_updated = Caller() self.is_updated = False + self._initialized = Event() self.values = {} @@ -189,6 +191,7 @@ def _param_updated(self, pk): # updated callbacks) if self._check_if_all_updated() and not self.is_updated: self.is_updated = True + self._initialized.set() self.all_updated.call() else: logger.debug('Variable id [%d] not found in TOC', var_id) @@ -237,6 +240,7 @@ def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False + self._initialized.clear() # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} @@ -298,16 +302,16 @@ def set_value(self, complete_name, value): pk.data += struct.pack(element.pytype, value_nr) self.param_updater.request_param_setvalue(pk) - def get_value(self, complete_name): + def get_value(self, complete_name, timeout=60): """ Read a value for the supplied parameter. If None then the value has not been updated yet, retry. """ - try: - [group, name] = complete_name.split('.') - return self.values[group][name] - except KeyError: - return None + if not self._initialized.wait(timeout=60): + raise Exception('Connection timed out') + + [group, name] = complete_name.split('.') + return self.values[group][name] class _ParamUpdater(Thread): From bfd71e4f4b9d6184bfe69a7cae919480d9cfd92e Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 5 Oct 2021 10:06:49 +0200 Subject: [PATCH 240/861] param: Fix docstring on get_value() --- cflib/crazyflie/param.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index b96153064..8fdca1151 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -304,8 +304,8 @@ def set_value(self, complete_name, value): def get_value(self, complete_name, timeout=60): """ - Read a value for the supplied parameter. If None then the value has - not been updated yet, retry. + Read a value for the supplied parameter. This can block for a period + of time if the parameter values have not been fetched yet. """ if not self._initialized.wait(timeout=60): raise Exception('Connection timed out') From 67661a7a3bd807bc4683dc0be32dd0530f0dd7c2 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 5 Oct 2021 10:59:58 +0200 Subject: [PATCH 241/861] param: Add semaphore wait to set_value() --- cflib/crazyflie/param.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 8fdca1151..30d656d91 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -252,11 +252,14 @@ def request_param_update(self, complete_name): self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) - def set_value_raw(self, complete_name, type, value): + def set_value_raw(self, complete_name, type, value, wait=False): """ Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ + if wait: + if not self._initialized.wait(timeout=60): + raise Exception('Connection timed out') char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) @@ -275,6 +278,9 @@ def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ + if not self._initialized.wait(timeout=60): + raise Exception('Connection timed out') + element = self.toc.get_element_by_complete_name(complete_name) if not element: From 95c2b18d892489419c806e4af8b8cba9283d77c1 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 5 Oct 2021 13:46:19 +0200 Subject: [PATCH 242/861] Revert "param: Add semaphore wait to set_value()" This reverts commit 67661a7a3bd807bc4683dc0be32dd0530f0dd7c2. --- cflib/crazyflie/param.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 30d656d91..8fdca1151 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -252,14 +252,11 @@ def request_param_update(self, complete_name): self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) - def set_value_raw(self, complete_name, type, value, wait=False): + def set_value_raw(self, complete_name, type, value): """ Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ - if wait: - if not self._initialized.wait(timeout=60): - raise Exception('Connection timed out') char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) @@ -278,9 +275,6 @@ def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ - if not self._initialized.wait(timeout=60): - raise Exception('Connection timed out') - element = self.toc.get_element_by_complete_name(complete_name) if not element: From 1f47b4a0cccb11473b3f8f4c0a64664cea88dbe2 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 7 Oct 2021 10:35:19 +0200 Subject: [PATCH 243/861] Add github action to check latest python --- .github/workflows/dependency_check.yml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 .github/workflows/dependency_check.yml diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml new file mode 100644 index 000000000..87a1fb5be --- /dev/null +++ b/.github/workflows/dependency_check.yml @@ -0,0 +1,17 @@ +name: Dependency check + +on: + on: + workflow_dispatch: + schedule: + - cron: '0 2 * * *' + +jobs: + build: + runs-on: ubuntu-latest + container: + image: python:latest + + steps: + - name: install lib + run: pip install -e . From d38f911af6837ee8e5b0d1a6fd464404e51fee3a Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 7 Oct 2021 10:37:26 +0200 Subject: [PATCH 244/861] Add checkout to dependency check --- .github/workflows/dependency_check.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 87a1fb5be..79c234cda 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -13,5 +13,7 @@ jobs: image: python:latest steps: + - uses: actions/checkout@v2 + - name: install lib run: pip install -e . From 286c97688c7a5604f18af3d99e8dd7fc68133e34 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 7 Oct 2021 10:39:01 +0200 Subject: [PATCH 245/861] Fix dependency check syntax problem --- .github/workflows/dependency_check.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 79c234cda..dd8d62576 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -1,7 +1,6 @@ name: Dependency check on: - on: workflow_dispatch: schedule: - cron: '0 2 * * *' From 6a06f912a02a48ccd475f3698fd6616d25846ff3 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 7 Oct 2021 11:03:59 +0200 Subject: [PATCH 246/861] Add python3.6 to dependency check --- .github/workflows/dependency_check.yml | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index dd8d62576..c2ab35650 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -6,7 +6,7 @@ on: - cron: '0 2 * * *' jobs: - build: + latest-python: runs-on: ubuntu-latest container: image: python:latest @@ -16,3 +16,14 @@ jobs: - name: install lib run: pip install -e . + + minimum-python: + runs-on: ubuntu-latest + container: + image: python:3.6 + + steps: + - uses: actions/checkout@v2 + + - name: install lib + run: pip install -e . From ab44bfd21c31906a8595c0a43933441fc65b5e28 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 7 Oct 2021 11:07:00 +0200 Subject: [PATCH 247/861] Fix check dependency gh action file --- .github/workflows/dependency_check.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index c2ab35650..879a486d0 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -19,11 +19,11 @@ jobs: minimum-python: runs-on: ubuntu-latest - container: - image: python:3.6 + container: + image: python:3.6 - steps: - - uses: actions/checkout@v2 + steps: + - uses: actions/checkout@v2 - - name: install lib - run: pip install -e . + - name: install lib + run: pip install -e . From 4fe401887e386be559faf01395a9244d06781130 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 7 Oct 2021 12:41:23 +0200 Subject: [PATCH 248/861] #267: Test dependency up to python 3.9 --- .github/workflows/dependency_check.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 879a486d0..43524f956 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -6,10 +6,10 @@ on: - cron: '0 2 * * *' jobs: - latest-python: + python-3.9: runs-on: ubuntu-latest container: - image: python:latest + image: python:3.9 steps: - uses: actions/checkout@v2 From 034e6481d9078f5947646bc0e650a34fcfe2782c Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 7 Oct 2021 12:42:55 +0200 Subject: [PATCH 249/861] #267: Fix dependency_check.yml syntax --- .github/workflows/dependency_check.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 43524f956..54b500405 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -6,7 +6,7 @@ on: - cron: '0 2 * * *' jobs: - python-3.9: + python-39: runs-on: ubuntu-latest container: image: python:3.9 From 048220306a8188b34a26586f62f84a9420cf7060 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 8 Oct 2021 10:13:00 +0200 Subject: [PATCH 250/861] Corrected menu file that broke tb docs --- docs/_data/menu.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index 437a861cb..c620246f3 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -13,7 +13,6 @@ - {page_id: crazyradio_lib} - title: Development subs: - - {page_id: testing} - {page_id: matlab} - {page_id: eeprom} - {page_id: uart_communication} From d8cb44b99a3b411c0eaa4aa12e87f80deed98b05 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 8 Oct 2021 11:00:29 +0200 Subject: [PATCH 251/861] cflib: Fix up docstring --- cflib/__init__.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cflib/__init__.py b/cflib/__init__.py index 17d9c3d50..02458b7cd 100644 --- a/cflib/__init__.py +++ b/cflib/__init__.py @@ -39,16 +39,20 @@ communication link and opening a communication link to a Crazyflie. Example of scanning for available Crazyflies on all communication links: +```python cflib.crtp.init_drivers() available = cflib.crtp.scan_interfaces() for i in available: print "Found Crazyflie on URI [%s] with comment [%s]" % (available[0], available[1]) +``` Example of connecting to a Crazyflie with known URI (radio dongle 0 and radio channel 125): +```python cf = Crazyflie() cf.open_link("radio://0/125") ... cf.close_link() +``` """ From 4b698288e9ea24d5fdec3b32a04ba68a077d2821 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 8 Oct 2021 11:34:01 +0200 Subject: [PATCH 252/861] Added support for compressed trajectories --- cflib/crazyflie/high_level_commander.py | 7 +- cflib/crazyflie/mem/__init__.py | 4 +- cflib/crazyflie/mem/trajectory_memory.py | 137 ++++++++++++- .../autonomous_sequence_high_level.py | 5 +- ...tonomous_sequence_high_level_compressed.py | 194 ++++++++++++++++++ examples/qualisys/qualisys_hl_commander.py | 5 +- 6 files changed, 334 insertions(+), 18 deletions(-) create mode 100644 examples/autonomy/autonomous_sequence_high_level_compressed.py diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 46298187e..847d007e3 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -52,7 +52,9 @@ class HighLevelCommander(): ALL_GROUPS = 0 TRAJECTORY_LOCATION_MEM = 1 + TRAJECTORY_TYPE_POLY4D = 0 + TRAJECTORY_TYPE_POLY4D_COMPRESSED = 1 def __init__(self, crazyflie=None): """ @@ -177,20 +179,21 @@ def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, trajectory_id, time_scale)) - def define_trajectory(self, trajectory_id, offset, n_pieces): + def define_trajectory(self, trajectory_id, offset, n_pieces, type=TRAJECTORY_TYPE_POLY4D): """ Define a trajectory that has previously been uploaded to memory. :param trajectory_id: The id of the trajectory :param offset: offset in uploaded memory :param n_pieces: Nr of pieces in the trajectory + :param type: The type of trajectory data; TRAJECTORY_TYPE_POLY4D or TRAJECTORY_TYPE_POLY4D_COMPRESSED :return: """ self._send_packet(struct.pack('. import logging +import math import struct from .memory_element import MemoryElement @@ -39,6 +40,115 @@ def __init__(self, duration, x=None, y=None, z=None, yaw=None): self.z = z if z else self.Poly() self.yaw = yaw if yaw else self.Poly() + def pack(self): + data = bytearray() + + data += struct.pack(' None: + self.x = x + self.y = y + self.z = z + self.yaw = yaw + + def pack(self): + data = bytearray() + + data += struct.pack( + ' None: + self._validate(element_x) + self._validate(element_y) + self._validate(element_z) + self._validate(element_yaw) + + self.duration = duration + self.x = element_x + self.y = element_y + self.z = element_z + self.yaw = element_yaw + + def pack(self): + element_types = (self._encode_type(self.x) << 0) | (self._encode_type(self.y) << 2) | (self._encode_type(self.z) << 4) | (self._encode_type(self.yaw) << 6) + duration_ms = int(self.duration * 1000.0) + + data = bytearray() + + data += struct.pack(' Date: Fri, 8 Oct 2021 12:27:33 +0200 Subject: [PATCH 253/861] Fixed styling --- cflib/crazyflie/mem/__init__.py | 4 ++-- cflib/crazyflie/mem/trajectory_memory.py | 5 +++-- .../autonomy/autonomous_sequence_high_level_compressed.py | 4 ++-- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 75ec4bf24..03fcef6a3 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -46,9 +46,9 @@ from .memory_element import MemoryElement from .memory_tester import MemoryTester from .ow_element import OWElement -from .trajectory_memory import Poly4D -from .trajectory_memory import CompressedStart from .trajectory_memory import CompressedSegment +from .trajectory_memory import CompressedStart +from .trajectory_memory import Poly4D from .trajectory_memory import TrajectoryMemory from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort diff --git a/cflib/crazyflie/mem/trajectory_memory.py b/cflib/crazyflie/mem/trajectory_memory.py index 0643909e9..d6f6b1219 100644 --- a/cflib/crazyflie/mem/trajectory_memory.py +++ b/cflib/crazyflie/mem/trajectory_memory.py @@ -113,7 +113,8 @@ def __init__(self, duration, element_x, element_y, element_z, element_yaw) -> No self.yaw = element_yaw def pack(self): - element_types = (self._encode_type(self.x) << 0) | (self._encode_type(self.y) << 2) | (self._encode_type(self.z) << 4) | (self._encode_type(self.yaw) << 6) + element_types = (self._encode_type(self.x) << 0) | (self._encode_type(self.y) << 2) | ( + self._encode_type(self.z) << 4) | (self._encode_type(self.yaw) << 6) duration_ms = int(self.duration * 1000.0) data = bytearray() @@ -129,7 +130,7 @@ def pack(self): def _validate(self, element): length = len(element) if length != 0 and length != 1 and length != 3 and length != 7: - raise Exception("length of element must be 0, 1, 3, or 7") + raise Exception('length of element must be 0, 1, 3, or 7') def _encode_type(self, element): if len(element) == 0: diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index d8fd3de6c..f44c3efde 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -37,9 +37,9 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.high_level_commander import HighLevelCommander from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.mem import CompressedStart 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.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper From 34907ff212d52ebccaef0878198cee788a3c8a2b Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 12 Oct 2021 08:14:03 +0200 Subject: [PATCH 254/861] swarm: Improve formatting on example --- cflib/crazyflie/swarm.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index 30371a58c..aa6941844 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -195,6 +195,7 @@ def sequential(self, func, args_dict=None): the args_dict. The dictionary is keyed on URI. Example: + ```python def my_function(scf, optional_param0, optional_param1) ... @@ -205,7 +206,8 @@ def my_function(scf, optional_param0, optional_param1) } - self.sequential(my_function, args_dict) + swarm.sequential(my_function, args_dict) + ``` :param func: the function to execute :param args_dict: parameters to pass to the function From 7f952e72c16fe6ddf897f91ee8bdd669c619244c Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 8 Oct 2021 10:58:09 +0200 Subject: [PATCH 255/861] Generate python documentation using pdoc3 This commit generates API reference documentation using pdoc3. The program pdoc3 will traverse the cflib module and generate documentation based on the template file text.mako that we drop in the template-dir. The `text.mako` file is doing all the heavy lifting to create the markdown we want. For more information about pdoc3 and the custom template system see: https://pdoc3.github.io/pdoc/doc/pdoc/#custom-templates --- docs/_data/menu.yml | 1 + docs/api/cflib/index.md | 10 +++ docs/api/template/text.mako | 175 ++++++++++++++++++++++++++++++++++++ tools/build-docs/build-docs | 14 +++ 4 files changed, 200 insertions(+) create mode 100644 docs/api/cflib/index.md create mode 100644 docs/api/template/text.mako create mode 100755 tools/build-docs/build-docs diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index c620246f3..38d7a3b73 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -13,6 +13,7 @@ - {page_id: crazyradio_lib} - title: Development subs: + - {page_id: api_reference} - {page_id: matlab} - {page_id: eeprom} - {page_id: uart_communication} diff --git a/docs/api/cflib/index.md b/docs/api/cflib/index.md new file mode 100644 index 000000000..3610f2270 --- /dev/null +++ b/docs/api/cflib/index.md @@ -0,0 +1,10 @@ +--- +title: API reference for CFLib +page_id: api_reference +--- + +This is a placeholder, you need to run: + +``` +$ ./tools/build-docs/build-docs +``` diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako new file mode 100644 index 000000000..b9fff9b70 --- /dev/null +++ b/docs/api/template/text.mako @@ -0,0 +1,175 @@ +## Define mini-templates for each portion of the doco. + +<%! + def title(heading, name): + if name == 'cflib': + return 'The CFLib API reference' + else: + return heading + ' ' + name +%> + +<%! + def page_id(name): + if name == 'cflib': + return 'api_reference' + else: + return name.replace('.', '-') +%> + +<%! + from pathlib import Path + def linkify(name, module, base_module): + link = module.url(relative_to=base_module).replace('html', 'md') + return f'[{name}]({link})' +%> + +<%! + def deflist(s): + out = str() + for line in s.splitlines(): + out += line + '\n' + return out +%> + +<%! + def h(level, s): + return '#' * level + ' ' + s +%> + +<%def name="function(func)" buffered="True"> + <% + returns = show_type_annotations and func.return_annotation() or '' + if returns: + returns = ' \N{non-breaking hyphen}> ' + returns + %> +```python +def ${func.name}(${", ".join(func.params(annotate=show_type_annotations))})${returns} +``` +${func.docstring | deflist} +--- + + +<%def name="variable(var)" buffered="True"> + <% + annot = show_type_annotations and var.type_annotation() or '' + if annot: + annot = ': ' + annot + %> +```python +${var.name}${annot} +``` +${var.docstring | deflist} +--- + + +<%def name="class_(cls)" buffered="True"> +${h(4, cls.name)} +```python +${cls.name}(${", ".join(cls.params(annotate=show_type_annotations))}) +``` +${cls.docstring | deflist} +--- + +<% + class_vars = cls.class_variables(show_inherited_members, sort=sort_identifiers) + static_methods = cls.functions(show_inherited_members, sort=sort_identifiers) + inst_vars = cls.instance_variables(show_inherited_members, sort=sort_identifiers) + methods = cls.methods(show_inherited_members, sort=sort_identifiers) + mro = cls.mro() + subclasses = cls.subclasses() +%> +% if mro: +${h(3, 'Ancestors (in MRO)')} + % for c in mro: +* ${linkify(c.refname, c, module)} + % endfor + +% endif +% if subclasses: +${h(3, 'Descendants')} + % for c in subclasses: +* ${linkify(c.refname, c, module)} + % endfor + +% endif +% if class_vars: +${h(3, 'Class variables')} + % for v in class_vars: +${variable(v)} + + % endfor +% endif +% if static_methods: +${h(3, 'Static methods')} + % for f in static_methods: +${function(f)} + + % endfor +% endif +% if inst_vars: +${h(3, 'Instance variables')} + % for v in inst_vars: +${variable(v)} + + % endfor +% endif +% if methods: +${h(3, 'Methods')} + % for m in methods: +${function(m)} + + % endfor +% endif + + +## Start the output logic for an entire module. + +<% + variables = module.variables(sort=sort_identifiers) + classes = module.classes(sort=sort_identifiers) + functions = module.functions(sort=sort_identifiers) + submodules = module.submodules() + heading = 'Namespace' if module.is_namespace else 'Module' +%> + +--- +title: ${title(heading, module.name)} +page_id: ${page_id(module.name)} +--- +${deflist(module.docstring)} +--- + +% if submodules: +Sub-modules +----------- + % for m in submodules: +* ${linkify(m.name, m, module)} + % endfor +% endif + +% if variables: +Variables +--------- + % for v in variables: +${variable(v)} + + % endfor +% endif + +% if functions: +Functions +--------- + % for f in functions: +${function(f)} + + % endfor +% endif + +% if classes: +Classes +------- + % for c in classes: +${class_(c)} + + % endfor +% endif diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs new file mode 100755 index 000000000..37de30c01 --- /dev/null +++ b/tools/build-docs/build-docs @@ -0,0 +1,14 @@ +#!/usr/bin/env bash + +scriptDir=$(dirname $0) +cflibDir=$scriptDir/../../cflib +apiRefDir=$scriptDir/../../docs/api +templateDir=$apiRefDir/template + +mkdir -p $apiRefDir +mkdir -p $scriptDir/tmp + +[ -n "$API_REF_BASE" ] || { + export API_REF_BASE=$(readlink -fn $apiRefDir) +} +pdoc3 --force $cflibDir --output-dir $apiRefDir --template-dir $templateDir From ab2ace965548b23bcb1537b9b9ebe964e1477ad7 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 11 Oct 2021 10:12:45 +0200 Subject: [PATCH 256/861] pdoc: text.mako: Handle :param: docstring We try to turn the docstring parameters into a markdown table :param param1: description1 :param param2: desccription2 description2 continues :param param3: into: #### Parameters | Name | Description | | ---- | ----------- | | param1 | decription1 | | param3 | | --- docs/api/template/text.mako | 48 ++++++++++++++++++++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako index b9fff9b70..62a06d48e 100644 --- a/docs/api/template/text.mako +++ b/docs/api/template/text.mako @@ -24,10 +24,56 @@ %> <%! + import re def deflist(s): + param_re = r':param (.*):' + params_found = False + in_param = False + desc = str() + + # + # Here we try to turn the docstring parameters into a markdown table + # + # :param param1: description1 + # :param param2: desccription2 + # description2 continues + # :param param3: + # + # into: + # + # #### Parameters + # + # | Name | Description | + # | ---- | ----------- | + # | param1 | decription1 | + # | param2 | description2 description2 continues | + # | param3 | | + # out = str() for line in s.splitlines(): - out += line + '\n' + match = re.match(param_re, line) + if match is not None: + if not params_found: + params_found = True + out += h(4, 'Parameters') + '\n' + out += '\n' + '| Name | Description |' + '\n' + out += '| ---- | ----------- |' + '\n' + + if in_param: + out += f'{desc} |' + '\n' + + in_param = True + out += f'| {match.group(1)} | ' + desc = line.replace(match.group(0), '') + + elif in_param: + desc += line + else: + out += line + '\n' + + if in_param: + out += f'{desc} |' + '\n' + return out %> From 0b6a7c6c4f2ff2b767ca49257e00c37dd055b613 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 11 Oct 2021 10:19:05 +0200 Subject: [PATCH 257/861] pdoc: text.mako: Improve title This will make it easier to follow when navigating in website. Module cflib.crazyflie / Module crazyflie.high_level_commander => crazyflie / high_level_commander --- docs/api/template/text.mako | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako index 62a06d48e..b13b94a82 100644 --- a/docs/api/template/text.mako +++ b/docs/api/template/text.mako @@ -5,7 +5,7 @@ if name == 'cflib': return 'The CFLib API reference' else: - return heading + ' ' + name + return name.split('.')[-1] %> <%! From 2bc6dfad0896aeed66663d286ff46a7223cb3172 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 12 Oct 2021 13:17:47 +0200 Subject: [PATCH 258/861] module.json: Add build-docs target --- module.json | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/module.json b/module.json index 7ba35679b..1b51d7db7 100644 --- a/module.json +++ b/module.json @@ -1,4 +1,7 @@ { "version": "1.0", - "environmentReq": ["python3"] + "environmentReq": { + "build": ["python3"], + "build-docs": ["doxygen"] + } } From f2227162017238ac3db4c9bf184d05b4a02812bb Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 12 Oct 2021 13:17:57 +0200 Subject: [PATCH 259/861] build-docs: Make sure cflib is installed --- tools/build-docs/build-docs | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs index 37de30c01..dcc0249d7 100755 --- a/tools/build-docs/build-docs +++ b/tools/build-docs/build-docs @@ -11,4 +11,17 @@ mkdir -p $scriptDir/tmp [ -n "$API_REF_BASE" ] || { export API_REF_BASE=$(readlink -fn $apiRefDir) } + +# +# If cflib is not installed (mostly its dependencies) then pdoc3 cannot +# traverse the library and get docstrings. +# +# The rewriting of HOME is a hack to make this work in a bare-bone Docker +# environment. +# +pip3 show cflib > /dev/null 2>&1 || { # this will run if cflib is not found + HOME=$scriptDir pip3 install --user --upgrade pip + HOME=$scriptDir pip3 install --user -e $scriptDir/../../ +} + pdoc3 --force $cflibDir --output-dir $apiRefDir --template-dir $templateDir From 324cb602d20a6a863012dacf2491c65f68ccd6f3 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 12 Oct 2021 14:47:04 +0200 Subject: [PATCH 260/861] __init__: disable two modules from pdoc3 They have problems at the moment that we need to solve. --- cflib/__init__.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cflib/__init__.py b/cflib/__init__.py index 02458b7cd..df7f0083e 100644 --- a/cflib/__init__.py +++ b/cflib/__init__.py @@ -56,3 +56,6 @@ cf.close_link() ``` """ +__pdoc__ = {} +__pdoc__['cflib.localization'] = False +__pdoc__['cflib.crtp.cflinkcppdriver'] = False From dc1251392e85907421d3150e9f224b5f359823f8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 12 Oct 2021 15:58:36 +0200 Subject: [PATCH 261/861] Corrected version in module.json --- .gitignore | 3 +++ module.json | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 77c4211a1..ff8291dc0 100644 --- a/.gitignore +++ b/.gitignore @@ -15,3 +15,6 @@ venv* #jekyll generated pages docs/.jekyll-metadata +docs/api/cflib/* +tools/build-docs/.local/* +docs/api/cflib/index.md diff --git a/module.json b/module.json index 1b51d7db7..600e47cc6 100644 --- a/module.json +++ b/module.json @@ -1,6 +1,6 @@ { - "version": "1.0", - "environmentReq": { + "version": "2.0", + "environmentReqs": { "build": ["python3"], "build-docs": ["doxygen"] } From 91e262fb414991ba4626921a1782861ca4bad416 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 13 Oct 2021 07:09:00 +0200 Subject: [PATCH 262/861] pdoc: Do not support hashes in links --- docs/api/template/text.mako | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako index b13b94a82..b14d60049 100644 --- a/docs/api/template/text.mako +++ b/docs/api/template/text.mako @@ -20,6 +20,11 @@ from pathlib import Path def linkify(name, module, base_module): link = module.url(relative_to=base_module).replace('html', 'md') + # we do not support hashes in uri right now + index = link.find('#') + if index > 0: + link = link[:index] + return f'[{name}]({link})' %> From 87b89f19bab115bc990d4f2d8a872343ea35cb82 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 13 Oct 2021 07:13:04 +0200 Subject: [PATCH 263/861] pdoc: Do not support linking to builtins --- docs/api/template/text.mako | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako index b14d60049..afd8da91c 100644 --- a/docs/api/template/text.mako +++ b/docs/api/template/text.mako @@ -25,6 +25,11 @@ if index > 0: link = link[:index] + # we do not support linkin to builtins + index = link.find('.ext') + if index > 0: + return name + return f'[{name}]({link})' %> From 95f2a45400313dec0eb100e858d5e8c5aa180783 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 13 Oct 2021 14:03:11 +0200 Subject: [PATCH 264/861] swarm: Improve documentation --- cflib/crazyflie/swarm.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index aa6941844..85b66a45b 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -130,8 +130,8 @@ def __get_estimated_position(self, scf): def get_estimated_positions(self): """ - Return a dict of the SwarmPosition namedtuple with the estimated - (x, y, z) of each Crazyflie in the swarm. The URIs are the key. + Return a `dict`, keyed by URI and with the SwarmPosition namedtuples as + value, with the estimated (x, y, z) of each Crazyflie in the swarm. """ self.parallel_safe(self.__get_estimated_position) return self._positions @@ -181,7 +181,7 @@ def __reset_estimator(self, scf): def reset_estimators(self): """ Reset estimator on all members of the swarm and wait for a stable - positions. + positions. Blocks until position estimators finds a position. """ self.parallel_safe(self.__reset_estimator) @@ -192,7 +192,8 @@ def sequential(self, func, args_dict=None): The first argument of the function that is passed in will be a SyncCrazyflie instance connected to the Crazyflie to operate on. A list of optional parameters (per Crazyflie) may follow defined by - the args_dict. The dictionary is keyed on URI. + the `args_dict`. The dictionary is keyed on URI and has a list of + parameters as value. Example: ```python @@ -223,10 +224,10 @@ def parallel(self, func, args_dict=None): threads are joined at the end. Exceptions raised by the threads are ignored. - For a description of the arguments, see sequential() + For a more detailed description of the arguments, see `sequential()` - :param func: - :param args_dict: + :param func: the function to execute + :param args_dict: parameters to pass to the function """ try: self.parallel_safe(func, args_dict) @@ -240,10 +241,10 @@ def parallel_safe(self, func, args_dict=None): threads are joined at the end and if one or more of the threads raised an exception this function will also raise an exception. - For a description of the arguments, see sequential() + For a more detailed description of the arguments, see `sequential()` - :param func: - :param args_dict: + :param func: the function to execute + :param args_dict: parameters to pass to the function """ threads = [] reporter = self.Reporter() From 972117332c572ceaff4b954b276606b7ead9dda1 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 13 Oct 2021 14:58:21 +0200 Subject: [PATCH 265/861] text.mako: Support @param style docstring as well --- docs/api/template/text.mako | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako index afd8da91c..af8afe5a2 100644 --- a/docs/api/template/text.mako +++ b/docs/api/template/text.mako @@ -36,7 +36,7 @@ <%! import re def deflist(s): - param_re = r':param (.*):' + param_re = r':param (.*):|@param (\w+)' params_found = False in_param = False desc = str() @@ -73,7 +73,12 @@ out += f'{desc} |' + '\n' in_param = True - out += f'| {match.group(1)} | ' + + if match.group(1) is not None: + out += f'| {match.group(1)} | ' + else: + out += f'| {match.group(2)} | ' + desc = line.replace(match.group(0), '') elif in_param: From fd6389be2f82893ed812952dc242c593d7529eca Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 14 Oct 2021 06:19:33 +0200 Subject: [PATCH 266/861] crazyflie: Replace docstring with @param syntax --- cflib/crazyflie/__init__.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 24ef331f9..56771bea7 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -75,8 +75,8 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): """ Create the objects from this module and register callbacks. - ro_cache -- Path to read-only cache (string) - rw_cache -- Path to read-write cache (string) + @param ro_cache Path to read-only cache (string) + @param rw_cache Path to read-write cache (string) """ # Called on disconnect, no matter the reason @@ -308,9 +308,9 @@ def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): """ Send a packet through the link interface. - pk -- Packet to send - expect_answer -- True if a packet from the Crazyflie is expected to - be sent back, otherwise false + @param pk Packet to send + @param expect_answer True if a packet from the Crazyflie is expected to + be sent back, otherwise false """ From 58a72a4d07aa2fa7446cc76baeb1e89c8b0868c7 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 14 Oct 2021 06:25:43 +0200 Subject: [PATCH 267/861] docstrings: Capitalize the first lettter in param description It looks better in rendered docs. Used the following search-and-replace in vscode: search: :param (.*): \s*([a-z])(.*) replace: :param $1: \u$2$3 --- cflib/crazyflie/high_level_commander.py | 44 +++++++++++----------- cflib/crazyflie/swarm.py | 12 +++--- cflib/localization/lighthouse_bs_vector.py | 4 +- cflib/positioning/motion_commander.py | 34 ++++++++--------- cflib/positioning/position_hl_commander.py | 38 +++++++++---------- 5 files changed, 66 insertions(+), 66 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 46298187e..05dd5c33b 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -64,7 +64,7 @@ def set_group_mask(self, group_mask=ALL_GROUPS): """ Set the group mask that the Crazyflie belongs to - :param group_mask: mask for which groups this CF belongs to + :param group_mask: Mask for which groups this CF belongs to """ self._send_packet(struct.pack('1.0: slower; <1.0: faster - :param relative: set to True, if trajectory should be shifted to + :param relative: Set to True, if trajectory should be shifted to current setpoint - :param reversed: set to True, if trajectory should be executed in + :param reversed: Set to True, if trajectory should be executed in reverse - :param group_mask: mask for which CFs this should apply to + :param group_mask: Mask for which CFs this should apply to :return: """ self._send_packet(struct.pack(' Date: Thu, 14 Oct 2021 07:03:02 +0200 Subject: [PATCH 268/861] log: Add markdown table to improve doc rendering --- cflib/crazyflie/log.py | 44 ++++++++++++++++-------------------------- 1 file changed, 17 insertions(+), 27 deletions(-) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 402bd6684..51633e065 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -32,33 +32,23 @@ configurations where selected variables are sent to the client at a specified period. -Terminology: - Log configuration - A configuration with a period and a number of variables - that are present in the TOC. - Stored as - The size and type of the variable as declared in the - Crazyflie firmware - Fetch as - The size and type that a variable should be fetched as. - This does not have to be the same as the size and type - it's stored as. - -States of a configuration: - Created on host - When a configuration is created the contents is checked - so that all the variables are present in the TOC. If not - then the configuration cannot be created. - Created on CF - When the configuration is deemed valid it is added to the - Crazyflie. At this time the memory constraint is checked - and the status returned. - Started on CF - Any added block that is not started can be started. Once - started the Crazyflie will send back logdata periodically - according to the specified period when it's created. - Stopped on CF - Any started configuration can be stopped. The memory taken - by the configuration on the Crazyflie is NOT freed, the - only effect is that the Crazyflie will stop sending - logdata back to the host. - Deleted on CF - Any block that is added can be deleted. When this is done - the memory taken by the configuration is freed on the - Crazyflie. The configuration will have to be re-added to - be used again. +#### Terminology + +| Term | Description | +| ----------------- | ----------- | +| Log configuration | A configuration with a period and a number of variables that are present in the TOC. | # noqa: E501 +| Stored as | The size and type of the variable as declared in the Crazyflie firmware. | # noqa: E501 +| Fetch as | The size and type that a variable should be fetched as. This does not have to be the same as the size and type it's stored as. | # noqa: E501 + +#### States of a configuration + +| State | Description | +| ----------------- | ----------- | +| Created on host | When a configuration is created the contents is checked so that all the variables are present in the TOC. If not then the configuration cannot be created. | # noqa: E501 +| Created on CF | When the configuration is deemed valid it is added to the Crazyflie. At this time the memory constraint is checked and the status returned. | # noqa: E501 +| Started on CF | Any added block that is not started can be started. Once started the Crazyflie will send back logdata periodically according to the specified period when it's created. | # noqa: E501 +| Stopped on CF | Any started configuration can be stopped. The memory taken by the configuration on the Crazyflie is NOT freed, the only effect is that the Crazyflie will stop sending logdata back to the host. | # noqa: E501 +| Deleted on CF | Any block that is added can be deleted. When this is done the memory taken by the configuration is freed on the Crazyflie. The configuration will have to be re-added to be used again. | # noqa: E501 """ import errno import logging From 383e467f9212f8d4fe8beabff986385f028e81ca Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 14 Oct 2021 08:46:27 +0200 Subject: [PATCH 269/861] syncCrazyflie: Add example --- cflib/crazyflie/syncCrazyflie.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index fd00712e3..7330158bb 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -28,6 +28,15 @@ class. It handles the asynchronous nature of the Crazyflie API and turns it into blocking function. It is useful for simple scripts that performs tasks as a sequence of events. + +Example: +```python +with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2) 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) +``` """ import logging from threading import Event From 244b7293b910b488d2123317d38f93559128c1bb Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 19 Oct 2021 06:29:21 +0200 Subject: [PATCH 270/861] log.py: Fix noqa appearing in docs --- cflib/crazyflie/log.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 51633e065..f48dd0071 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -24,6 +24,7 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. +# flake8: noqa """ Enables logging of variables from the Crazyflie. @@ -36,19 +37,19 @@ | Term | Description | | ----------------- | ----------- | -| Log configuration | A configuration with a period and a number of variables that are present in the TOC. | # noqa: E501 -| Stored as | The size and type of the variable as declared in the Crazyflie firmware. | # noqa: E501 -| Fetch as | The size and type that a variable should be fetched as. This does not have to be the same as the size and type it's stored as. | # noqa: E501 +| Log configuration | A configuration with a period and a number of variables that are present in the TOC. | +| Stored as | The size and type of the variable as declared in the Crazyflie firmware. | +| Fetch as | The size and type that a variable should be fetched as. This does not have to be the same as the size and type it's stored as. | #### States of a configuration | State | Description | | ----------------- | ----------- | -| Created on host | When a configuration is created the contents is checked so that all the variables are present in the TOC. If not then the configuration cannot be created. | # noqa: E501 -| Created on CF | When the configuration is deemed valid it is added to the Crazyflie. At this time the memory constraint is checked and the status returned. | # noqa: E501 -| Started on CF | Any added block that is not started can be started. Once started the Crazyflie will send back logdata periodically according to the specified period when it's created. | # noqa: E501 -| Stopped on CF | Any started configuration can be stopped. The memory taken by the configuration on the Crazyflie is NOT freed, the only effect is that the Crazyflie will stop sending logdata back to the host. | # noqa: E501 -| Deleted on CF | Any block that is added can be deleted. When this is done the memory taken by the configuration is freed on the Crazyflie. The configuration will have to be re-added to be used again. | # noqa: E501 +| Created on host | When a configuration is created the contents is checked so that all the variables are present in the TOC. If not then the configuration cannot be created. | +| Created on CF | When the configuration is deemed valid it is added to the Crazyflie. At this time the memory constraint is checked and the status returned. | +| Started on CF | Any added block that is not started can be started. Once started the Crazyflie will send back logdata periodically according to the specified period when it's created. | +| Stopped on CF | Any started configuration can be stopped. The memory taken by the configuration on the Crazyflie is NOT freed, the only effect is that the Crazyflie will stop sending logdata back to the host. | +| Deleted on CF | Any block that is added can be deleted. When this is done the memory taken by the configuration is freed on the Crazyflie. The configuration will have to be re-added to be used again. | """ import errno import logging From e61cc2d1c966160f68f4a62be0a82b3e99024ede Mon Sep 17 00:00:00 2001 From: WowSuchName <13381391+WowSuchName@users.noreply.github.com> Date: Fri, 22 Oct 2021 11:00:07 +0200 Subject: [PATCH 271/861] toc: Fix TocFetcher requesting ToC entries when none are available --- cflib/crazyflie/toc.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/toc.py b/cflib/crazyflie/toc.py index 6ec7b469c..e430cc49a 100644 --- a/cflib/crazyflie/toc.py +++ b/cflib/crazyflie/toc.py @@ -177,7 +177,12 @@ def _new_packet_cb(self, packet): else: self.state = GET_TOC_ELEMENT self.requested_index = 0 - self._request_toc_element(self.requested_index) + if (self.nbr_of_items > 0): + self._request_toc_element(self.requested_index) + else: + logger.debug('No TOC entries for port [%s]' % self.port) + self._toc_cache.insert(self._crc, self.toc.toc) + self._toc_fetch_finished() elif (self.state == GET_TOC_ELEMENT): # Always add new element, but only request new if it's not the From 2f411e6af37705c030d0a61a78877ffbfd4c99bf Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 26 Oct 2021 08:52:34 +0200 Subject: [PATCH 272/861] Relax opencv dependency version, fixes py3.10 test Fixes Install fails on Python 3.10 Fixes #267 --- .github/workflows/dependency_check.yml | 4 ++-- .gitignore | 3 +++ setup.py | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 54b500405..6c74c5ea1 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -6,10 +6,10 @@ on: - cron: '0 2 * * *' jobs: - python-39: + python-latest: runs-on: ubuntu-latest container: - image: python:3.9 + image: python:latest steps: - uses: actions/checkout@v2 diff --git a/.gitignore b/.gitignore index ff8291dc0..508122580 100644 --- a/.gitignore +++ b/.gitignore @@ -18,3 +18,6 @@ docs/.jekyll-metadata docs/api/cflib/* tools/build-docs/.local/* docs/api/cflib/index.md + +#vscode config folder +.vscode/ diff --git a/setup.py b/setup.py index 244ea8e16..cfe0e1d15 100644 --- a/setup.py +++ b/setup.py @@ -34,7 +34,7 @@ install_requires=[ 'pyusb>=1.0.0b2', - 'opencv-python-headless==4.5.1.48' + 'opencv-python-headless~=4.5.1' ], # $ pip install -e .[dev] From ef8e1fe49bab0855eabb530a823a0913539f09f6 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 28 Oct 2021 16:01:25 +0200 Subject: [PATCH 273/861] Use include functionality for generated docs --- .gitignore | 8 ++++++-- docs/api/cflib/index.md | 6 +----- docs/api/template/text.mako | 10 ++-------- tools/build-docs/build-docs | 23 ++++++++++++++++++++--- 4 files changed, 29 insertions(+), 18 deletions(-) diff --git a/.gitignore b/.gitignore index 508122580..98f856fc9 100644 --- a/.gitignore +++ b/.gitignore @@ -16,8 +16,12 @@ venv* #jekyll generated pages docs/.jekyll-metadata docs/api/cflib/* -tools/build-docs/.local/* -docs/api/cflib/index.md + +# Generated documentation +tools/build-docs/.local +tools/build-docs/.cache +docs/api/cflib +!docs/api/cflib/index.md #vscode config folder .vscode/ diff --git a/docs/api/cflib/index.md b/docs/api/cflib/index.md index 3610f2270..9adba0119 100644 --- a/docs/api/cflib/index.md +++ b/docs/api/cflib/index.md @@ -3,8 +3,4 @@ title: API reference for CFLib page_id: api_reference --- -This is a placeholder, you need to run: - -``` -$ ./tools/build-docs/build-docs -``` +{% include_relative_generated index.md_raw info="Placeholder for generated file. Run `tb build-docs` to generate content." %} diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako index af8afe5a2..6f3ccc43e 100644 --- a/docs/api/template/text.mako +++ b/docs/api/template/text.mako @@ -2,18 +2,12 @@ <%! def title(heading, name): - if name == 'cflib': - return 'The CFLib API reference' - else: - return name.split('.')[-1] + return name.split('.')[-1] %> <%! def page_id(name): - if name == 'cflib': - return 'api_reference' - else: - return name.replace('.', '-') + return name.replace('.', '-') %> <%! diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs index dcc0249d7..53f9cf578 100755 --- a/tools/build-docs/build-docs +++ b/tools/build-docs/build-docs @@ -4,12 +4,13 @@ scriptDir=$(dirname $0) cflibDir=$scriptDir/../../cflib apiRefDir=$scriptDir/../../docs/api templateDir=$apiRefDir/template +tempDir=$scriptDir/.tmp mkdir -p $apiRefDir -mkdir -p $scriptDir/tmp +mkdir -p $tempDir [ -n "$API_REF_BASE" ] || { - export API_REF_BASE=$(readlink -fn $apiRefDir) + export API_REF_BASE=$(readlink -fn $tempDir) } # @@ -24,4 +25,20 @@ pip3 show cflib > /dev/null 2>&1 || { # this will run if cflib is not found HOME=$scriptDir pip3 install --user -e $scriptDir/../../ } -pdoc3 --force $cflibDir --output-dir $apiRefDir --template-dir $templateDir +# Generate documentation +pdoc3 --force $cflibDir --output-dir $tempDir --template-dir $templateDir + +# Modify the generated content to fit our file tree +## Rename root file that will be included in another md file +rootFile=$tempDir/cflib/index.md_raw +mv $tempDir/cflib/index.md $rootFile +## Remove the front matter at the top +sed -i '1,4d' $rootFile +## Links are not processed properly in the included content. Hack it by removing index.md +sed -i s/index\.md// $rootFile + +# Copy to the md file tree +cp -r $tempDir/cflib/* $apiRefDir/cflib/. + +# Clean up +rm -r $tempDir From 197976957905bb4404b337bd4403d605c38ffa53 Mon Sep 17 00:00:00 2001 From: Markus Elfring Date: Tue, 2 Nov 2021 18:48:03 +0100 Subject: [PATCH 274/861] Issue #277: Convert six statements to the usage of augmented assignments Augmented assignment statements became available with Python 2. https://docs.python.org/3/whatsnew/2.0.html#augmented-assignment Thus improve six source code places accordingly. Signed-off-by: Markus Elfring --- cflib/crazyflie/toc.py | 2 +- cflib/drivers/crazyradio.py | 4 ++-- cflib/utils/fp16.py | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/cflib/crazyflie/toc.py b/cflib/crazyflie/toc.py index 6ec7b469c..8e6a83754 100644 --- a/cflib/crazyflie/toc.py +++ b/cflib/crazyflie/toc.py @@ -197,7 +197,7 @@ def _new_packet_cb(self, packet): if (self.requested_index < (self.nbr_of_items - 1)): logger.debug('[%d]: More variables, requesting index %d', self.port, self.requested_index + 1) - self.requested_index = self.requested_index + 1 + self.requested_index += 1 self._request_toc_element(self.requested_index) else: # No more variables in TOC self._toc_cache.insert(self._crc, self.toc.toc) diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 0d57c11d4..93b6c2f3d 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -258,7 +258,7 @@ def scan_selected(self, selected, packet): self.set_data_rate(s['datarate']) status = self.send_packet(packet) if status and status.ack: - result = result + (s,) + result += (s,) return result @@ -278,7 +278,7 @@ def scan_channels(self, start, stop, packet): self.set_channel(i) status = self.send_packet(packet) if status and status.ack: - result = result + (i,) + result += (i,) return result # Data transfers diff --git a/cflib/utils/fp16.py b/cflib/utils/fp16.py index 83394ecf1..ba8bd5521 100644 --- a/cflib/utils/fp16.py +++ b/cflib/utils/fp16.py @@ -38,7 +38,7 @@ def fp16_to_float(float16): return int(s << 31) else: while not (f & 0x00000400): - f = f << 1 + f <<= 1 e -= 1 e += 1 f &= ~0x00000400 @@ -49,7 +49,7 @@ def fp16_to_float(float16): else: return int((s << 31) | 0x7f800000 | (f << 13)) - e = e + (127 - 15) - f = f << 13 + e += 127 - 15 + f <<= 13 result = int((s << 31) | (e << 23) | f) return struct.unpack('f', struct.pack('I', result))[0] From 54d785740ad629f88385344b0fed00c76d0b457c Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 15 Nov 2021 06:24:22 +0100 Subject: [PATCH 275/861] console: Add better documentation of API Also add an example. --- cflib/crazyflie/console.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/cflib/crazyflie/console.py b/cflib/crazyflie/console.py index 696b9ae84..12c2e0e35 100644 --- a/cflib/crazyflie/console.py +++ b/cflib/crazyflie/console.py @@ -47,6 +47,22 @@ def __init__(self, crazyflie): """ self.receivedChar = Caller() + """ + This member variable is used to setup a callback that will be called + when text is received from the CONSOLE port of CRTP (0). + + Example: + ```python + [...] + + def log_console(self, text): + self.log_file.write(text) + + [...] + + self.cf.console.receivedChar.add_callback(self.log_console) + ``` + """ self.cf = crazyflie self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming) From f5a2ea84847ef94ca59b9acef97be9f5ec511f40 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 15 Nov 2021 08:51:48 +0100 Subject: [PATCH 276/861] console: Make incoming() private This should not be used outside this class --- cflib/crazyflie/console.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/console.py b/cflib/crazyflie/console.py index 12c2e0e35..42cb84735 100644 --- a/cflib/crazyflie/console.py +++ b/cflib/crazyflie/console.py @@ -65,9 +65,9 @@ def log_console(self, text): """ self.cf = crazyflie - self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming) + self.cf.add_port_callback(CRTPPort.CONSOLE, self._incoming) - def incoming(self, packet): + def _incoming(self, packet): """ Callback for data received from the copter. """ From 095b4c9ba7a7801ea511ce7d10df0672f6dd5a33 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 18 Nov 2021 20:48:17 +0100 Subject: [PATCH 277/861] cflib: Add PCAP log After this you can specify: ``` $ CRTP_PCAP_LOG=filename.pcap python3 examples/swarm/hl-commander-swarm.py ``` To generate a PCAP file with the CRTP communication seen by crazyradio. The PCAP record generated is structured as: | receive| address | channel | radio devid | crtp header | crtp data | | 1 byte | 5 bytes | 1 byte | 1 byte | 1 byte | n bytes | Where the receive bye indicates whether this is an incoming packet or not. --- cflib/crtp/pcap.py | 143 ++++++++++++++++++++++++++++++++++++ cflib/drivers/crazyradio.py | 33 +++++++++ 2 files changed, 176 insertions(+) create mode 100644 cflib/crtp/pcap.py diff --git a/cflib/crtp/pcap.py b/cflib/crtp/pcap.py new file mode 100644 index 000000000..866195b8c --- /dev/null +++ b/cflib/crtp/pcap.py @@ -0,0 +1,143 @@ +import os +import struct +from datetime import datetime + + +class PCAPLog(): + """" + From wiki.wireshark.org: + + Global Header + This header starts the libpcap file and will be followed by the first packet header: + + ```c + typedef struct pcap_hdr_s { + guint32 magic_number; /* magic number */ + guint16 version_major; /* major version number */ + guint16 version_minor; /* minor version number */ + gint32 thiszone; /* GMT to local correction */ + guint32 sigfigs; /* accuracy of timestamps */ + guint32 snaplen; /* max length of captured packets, in octets */ + guint32 network; /* data link type */ + } pcap_hdr_t; + ``` + + magic_number: used to detect the file format itself and the byte ordering. + The writing application writes 0xa1b2c3d4 with it's native + byte ordering format into this field. The reading application + will read either 0xa1b2c3d4 (identical) or 0xd4c3b2a1 + (swapped). If the reading application reads the swapped + 0xd4c3b2a1 value, it knows that all the following fields will + have to be swapped too. For nanosecond-resolution files, the + writing application writes 0xa1b23c4d, with the two nibbles + of the two lower-order bytes swapped, and the reading + application will read either 0xa1b23c4d (identical) or + 0x4d3cb2a1 (swapped). + + version_major, version_minor: the version number of this file format + (current version is 2.4) + + thiszone: the correction time in seconds between GMT (UTC) and the local + timezone of the following packet header timestamps. Examples: + If the timestamps are in GMT (UTC), thiszone is simply 0. If the + timestamps are in Central European time (Amsterdam, Berlin, ...) + which is GMT + 1:00, thiszone must be -3600. In practice, time + stamps are always in GMT, so thiszone is always 0. + + sigfigs: in theory, the accuracy of time stamps in the capture; in + practice, all tools set it to 0 + + snaplen: the "snapshot length" for the capture (typically 65535 or even + more, but might be limited by the user), see: incl_len vs. + orig_len below + + network: link-layer header type, specifying the type of headers at the + beginning of the packet (e.g. 1 for Ethernet, see tcpdump.org's + link-layer header types page for details); this can be various + types such as 802.11, 802.11 with various radio information, PPP, + Token Ring, FDDI, etc. + + Record (Packet) Header + Each captured packet starts with (any byte alignment possible): + + ```c + typedef struct pcaprec_hdr_s { + guint32 ts_sec; /* timestamp seconds */ + guint32 ts_usec; /* timestamp microseconds */ + guint32 incl_len; /* number of octets of packet saved in file */ + guint32 orig_len; /* actual length of packet */ + } pcaprec_hdr_t; + ``` + ts_sec: the date and time when this packet was captured. This value is in + seconds since January 1, 1970 00:00:00 GMT; this is also known as + a UN*X time_t. You can use the ANSI C time() function from time.h + to get this value, but you might use a more optimized way to get + this timestamp value. If this timestamp isn't based on GMT (UTC), + use thiszone from the global header for adjustments. + + ts_usec: in regular pcap files, the microseconds when this packet was + captured, as an offset to ts_sec. In nanosecond-resolution files, + this is, instead, the nanoseconds when the packet was captured, + as an offset to ts_sec Beware: this value shouldn't reach 1 + second (in regular pcap files 1 000 000; in nanosecond-resolution + files, 1 000 000 000); in this case ts_sec must be increased + instead! + + incl_len: the number of bytes of packet data actually captured and saved + in the file. This value should never become larger than orig_len + or the snaplen value of the global header. + + orig_len: the length of the packet as it appeared on the network when it + was captured. If incl_len and orig_len differ, the actually saved + packet size was limited by snaplen. + """ + # Global header for pcap 2.4 + pcap_global_header = ('D4 C3 B2 A1 ' + '02 00 ' # major revision (i.e. pcap <2>.4) + '04 00 ' # minor revision (i.e. pcap 2.<4>) + '00 00 00 00 ' + '00 00 00 00 ' + 'FF FF 00 00 ' + 'A2 00 00 00 ') + + _instance = None + + def __init__(self): + raise RuntimeError('singleton: call instance() instead') + + @classmethod + def instance(cls): + if cls._instance is None: + cls._instance = cls.__new__(cls) + + logfile = os.environ['CRTP_PCAP_LOG'] + print(f'opening {logfile}') + cls._instance._log = open(logfile, 'wb') + + array = bytearray.fromhex(PCAPLog.pcap_global_header) + cls._instance._log.write( + struct.pack('<{}'.format('B' * len(array)), *array) + ) + + return cls._instance + + def logCRTP(self, receive, devid, address, channel, crtp_packet): + length = len(address) + 1 + 1 + 1 + len(crtp_packet) # addr + devid + receive + channel + data + + self._log.write(self._pcap_header(length)) + self._log.write( + self._assemble_record( + receive, address, channel, devid, crtp_packet + ) + ) + + def _assemble_record(self, receive, address, channel, devid, crtp_packet): + return struct.pack( + ' 0: + logger = PCAPLog.instance() + logger.logCRTP(receive, devid, address, channel, packet) + except KeyError: + pass + def close(self): if (pyusb1 is False): if self.handle: @@ -295,6 +311,14 @@ def send_packet(self, dataOut): else: self.handle.write(endpoint=1, data=dataOut, timeout=1000) data = self.handle.read(0x81, 64, timeout=1000) + + self._log_packet( + False, + self.devid, + self.current_address, + self.current_channel, + dataOut + ) except usb.USBError: pass @@ -308,6 +332,15 @@ def send_packet(self, dataOut): else: ackIn.retry = self.arc + if ackIn.ack: + self._log_packet( + True, + self.devid, + self.current_address, + self.current_channel, + ackIn.data + ) + return ackIn From f7cf98247d1326e3c98232c56ec206dd28522491 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 18 Nov 2021 20:50:40 +0100 Subject: [PATCH 278/861] Add Wireshark dissector This adds a Wireshark (Lua) plugin that can dissect the CRTP PCAP file added in previous commit. --- docs/images/wireshark-dissector.png | Bin 0 -> 147997 bytes wireshark/README.md | 56 ++++ wireshark/crtp-dissector.lua | 402 ++++++++++++++++++++++++++++ 3 files changed, 458 insertions(+) create mode 100644 docs/images/wireshark-dissector.png create mode 100644 wireshark/README.md create mode 100644 wireshark/crtp-dissector.lua diff --git a/docs/images/wireshark-dissector.png b/docs/images/wireshark-dissector.png new file mode 100644 index 0000000000000000000000000000000000000000..ad9482333292c85671984a1ece4c9495f1fb9d74 GIT binary patch literal 147997 zcmcG0Wk8he7VQ8E1|cF{A_yv7LxV|4C@S4GbayKyUDBn1AkvI<4Ba8!-4a9BFvER3 zU!3!ubAQ}F_x&^Q&cmDg*?aA^)_K|B zID(HW_D^NrUIUxQHN%hKcS?sBst#|gjUAlz?2I5VD{D(5c6$RmBO@z&6Ke+~4niCP zxeJkb{^YGo%GQjt%cs`K_B~|WVOpfZ-7iYF} zE@!y+kL}W>jg{8t<3BdRfZGlK7<6yMkMalhkADo3cGN9@c***YaZ2na*G-83bBi*2 zdR_NFcM$*I_B7#7UQvDipIeqN7wdl?Tr45>lJpS4MCD{KqrA_pZT^XJKi(a~6%~ z;x)Z#Xk-)<8{2!Zqzyx2a86y^^?4FGHfaJpYn)98|Gd?=)kNZ$Q2&?l@gm22Q`Wbr zsGd%KX_iAf3MZ44-=0TGA$Mjf%k8C5cBjLvrXsGzV^+!IzF?ofcgV>X^EyIiI7tV5| zxZN?>$6Hg17DE(PHMSJ|W>?-yI{)$PJ0#vJD?>R$-BW#KI{gCu^&ru2-sFfC1WIdR^YrS432Z|!B9xu1UkM7KoU)%7CTrLxL4qw=Ffe1YNSacZr=0{^t8 zR^IpIKZ0fX(c02dYo3AB!8GgT{rmi+K1q6dwZcw(;^NwxR+!N(ABsPtieo)svuH{B z{Pilj>eG}pon%OiJH-2Zg-PKK>E2-nvBO96?tx6o>67sX#(jxW5msZR?nrS6?%_^n zF~gW1A61De8--6vf9PVlJKepa&ps@dM;%us?rRB@LN8tS)HF3UPr-vbbv>8EB=AN^ z(bY!uvv`K|8XFugMf@z>=D$Nq8kMf|%5BH1Zeywj&#S&(wLqEK3r)+r#-aP>g3KMe zlCOWFSv$M0CNOpn1SXdS(e_&AyiU=)jE#X6V9Zqydk`StOR#un0wlnUvI;Kf9joyPr<5yp*-j1HR2mt+*=zZ$Dj zK=0n?k0oylTN_!E6w&`H;YG2wH#gJzP~#CXWf)hYD<=$_bh&QLd=}U7I_2;#nCr+s zW$#fh{-}17Y;VU~=YFYEfjz(blk6DO&6?_Jo5hyvkbEdBG?~D+@m1j#(VLvTA#2`s zS4$SF>IFBa?b+rZiSYg@uL(?RUteE5P3E7;ic>i+R%O;D227Z*?ig_aDj#xf>i)(o zLv7b3dFI-4YZUTR{NkYbpUF*=36?WxXyT+0j#iaxHi6u@al^z}o?5ufS}L%z{D;Nt zwDzbW-$150c^cK(gSu)b+y|*@H!G`RcZ!me<6+kJI{NpJo`=^(oOT{LW0r^cN=r+z zRO_5l(H9smFA(9cmGn#KE3mHm`$O0?OYwc)qt9=RW%D;ac@OjSaz8=lWM%n*{HmO< zjq`Zeh$;7vn9gzoF==SD_DbM;xo3G%M1(+*xIj|%>$1+SzDE5L%~2tIcW29?r*HpA zl}h*H3N%}%=)HoI^H{^EhL^%MtXsyW0^8d~6_uR`wbSTY_(F}R`n2$Ahtr>+Q9R~5 z9?V_`Vn>^j3%?ehHif5-=R;@R4QLkhzIYt3Fd1`d%IW$@5HIM3y4o?(dy7X~~4 z!I^8hRv#YUXWn17zr=0O<(gmGZr#hb(A3g&@oo~tHXqhqNPl^{dr$=V_?`5LwwL(p zGIQ?L&L}yTH5_G38J)Kdz1avp>|k_z+yZJvF`PbSMb}n)HRRM~aST#fU8&1M)yewc z^OKneZFPKPaJ8E@L4VY#+<%pzAxgMMfif#u2q;!L-DY!q`z2BsPc`;GUj4_nub#UI zT5pXUtsUFTHky#_$ZZNwq!V-uh(>WaeO$NMWv$5d2ECTTvBrmeEXcs#M z@RRsLSEEqKY=yT>5CW>Q zi*Hel7IhC8B>o7uZ2Q(ucaM+hijVN?sOOTyOq>GoAQCP-JWU7kuZK}d4slgusi~=D z(FunJ)=*c>IpAyxv<<9YL26iQ^A89?Onk<|wKY@qE?u7NA7A9+_H<$X zoHc(va3XbV_5VaE&$E)aG9UgoR{B6`DFw7!2{)}J4OJ`56^DtV9pxYNJBdyDh`hVT9t@%?|> z^W6V-11~1wg=V}J3_Y2i(6PWMMd5`+=g$=r`^4rclZ<_(7OJX>rMlSYBVCH=4;6x? znj+AWh{k7-V)X)1{&ThZ%RMR75*T#n&K*I%K1GSk4;*5s$nU&I(St43eC51n{YVqa_Hb_7{V~zA9tt-Cik8%q>2?idy;Rd^feKSvCMBf@_a1AS z%Q>YR5ls&cBi%|!kWN&Z*D%92IXQVuY^vu7f+J_D%w51%tD5^_7F9A~L1>AR4Lf_O zSEjC3>OiR&<`q{zrqv%-G1JJpq-`*BegpyYzO)qnkb|SpCK;AzUyhP78Yz;4ul9(o z)$xO?tUla&^TlbWc1^qEe#jK^hM z{^IoDEk?Z}ZQ*3rTkWJQG!%ca+QFc?Ei8Uo^4}1J~U^CCRUWtPsg^l5DI)1o@(!Mj#T0OW4ud7US9Zgo#Dp* zU`e%E@1)(y&#Wv%LRy&|R|~d@2{jco$~Uw2=v)vbs9LXoRYC#P>m`r9|`H0qBO z{(JsurgA*On$C92pwE+?Uo&5&`fDZN-fGMu}!Pqtu`)W<|x-ZW*yYln1M7$2~@>tnAVB zcFdo}>nxN6((6A))Ke&Q$`Md;eePPSO1eW%&g`^c+sK;h8WoFco6ly1Unlgg=B;{Gl$Kc?os`8TCEfVEsrc_` z69tdLgM*Ti@{pbB#vD@rA*FDor;9Tvo{GbN~|XGLBzUdJNN?YWH*XYE`=|V z-@71FMsA5WuD|vm4YfW{!0J5fMa`J*5qy*kg?kMd*C$*M5Y5_?I};%eN)5QE!tj61 zMf4+*dJuH9?~{|MUkStcSOg-Q8fC>5a3VK|f4KKDYXJ)@;JR#=jOBq zpbcnOW95^VDECfdAcwS~cYLaNy$m)=S86W@+Iuby0??Y)^#>di6GFB7gEt> zHf|}3HY|Sau3Q>TrsQ$4i46_w-P@xP@k$A&owVnAm`DjXw(ph~m_(WWKJDQKOw)Vj zE*ZP_Q(3b|b?(Qyr`r_^J3C&DULx(bw|7@0ct*^IVlvE>L@^B>PC7T3~T!!THuu<(|;y57U|N)95UK z8t1Yv`{+U3fs3zx9X0Jfmda;2nDz7sOIB7k;dshnl~eQiWUI`}d(L)5$g6kKsnBd3g&d*oXLHBLWE|%)V{>lF zl!%Mayu3}LBem3T53q5t$M4WvD?+SiOZ|6$zLFF@g*ZzHe(*${yeYEMf{YewD1uM! z~+256ufYpsIZoplS^#U*%Z)) zCYug@CsmAaqk+NJSiK17x_b=w1?EaUR?M(3(CcG4SwH%f)U5DWB&T- z-{>LgSMF1HNc=>xtwB{0t%#HIx7iYd_DaFB24o3?ec+KPj_uME520WA8W0b zwQWgd=G#W)+vA;|=)D&M7<{Wv3PnKpvCX=jctU=S7BqV{BO$URaWUd(3Zj!>c5cGm zNxzDR4<9aI+;_6ohK%1qp=2EDawi;Gy9WoE&rnWV5;l=y?k91qaG3YK19FbVZ0dmD zyb8C$s6~C!1;MKlFV>}ng+{=JoGpFw6uNoiMuHHG5RE#_ez~R~cYbKV?0L?u<9SXD zS{IkfnxPC=`+Q|q1D$h$TvJm~bc=K$y`WDKBDv|uT((FSy@#hKm|v{ZUI+4kq7lma*KEqMXMf}IWuGj>UtkUe=)@#Z-e1?Be)!c&GKwXHygqo8*a+(vGbCme(twYA4-Br>f=YDo}lbqXO*D*fz!hcY0wyxUQ zXk(xB*Z`EQFN*y;&jm~#o7mji9JPjg{`T$TeGpRFj+^%7C(XmA7iC^J0yA8nFrF{x zifGQN4Pmf@-42%B{lmqtCH;Hj%|x$@tY}kGQ;Sad697Q)Wu#-2K=|a)U6O;-ETs~O z>w^h2*$t-5t9N2KL*B+g%)zhtvN(eR9({%y4%zGt)RuE(tA*a(~|b z`Wwe)uAfgk!b}(Q9rKj8#;Zd7L&!)#?rQb3c_JZ~+6G$rSP^9?+~zo?>h#>@U~4^R zoA-lq5jrFmCZmt_{DtJ;cULxc{p5k-F9y-sMrLD$FNWH}RLecCZcmnf-Te&(65B)i z!^O?d^0zF+&-Pix-A=%Gew@2Zf`0CFRE-PGfg97gIQ^lXQA_Rc-J>JDy}_^el!88{ zyvEU=KFJlR6X&DNX+7Nz8#+5TN|!2B*#JkE`~-bI8L>T46ETIXD$=Z?MX=!?df^mpAZ)X_<62c1XtNkpeOa}5FYSlP2 z>T-QbUeSs=uW?4|O(J>EjJgE;0^@gFCkrh%@&`bQSXkKUYrvqlSZ}=jdUGbdD=xA= zVV^lB=)EUu9H!A$Ru|wI6m%hsKCv@c-`=}y9UCKm;N43`My7mPZ(q1sZ)i{tTrB>^4RcFrejP=PU)1hkqIW%AFvp}LH)Pb zDs6KM01qr5bWl(&y7AIya0Ov+cVP1KbA4TYNXX69$qEDatFS207^?G|$AzqE4h}`| zcrQFvF4F4$(kvyIkn&V6Ghn4lP)KA7VeMiEHI|jV!LP_X%Q5m1g=9au{7ylgr(BR- zS$QRIq$?nd`u(6^Q@f7ba`==LyFj)gTKo}DbkCBSDjQjkvAzF!fn4)C?r;%F>o0!u zZ|_(?_&*ZS0~0f*0~UYkHbEns{1;dP&;CK6I{ z>&tpi3e;e?=*76So5QATyh7?V{QTDo=kCWlt?g4&X%}Merl6Jy`^}2euKno4?U|Uq zRN={Zg2Ef*)e>4}XCITiB~*MHA5WQ27JICGw_A~4Ma;D+)ERTG{*Kn!-rle;V+4kn zrWc35CVWTC#|9r%H&W84RMK#BhjCL8Sj^QoW8OzgvPfLH<=o5fA#_Hq^k!(h*660C zM#T@7n*Qu}2jg}!GdGXm=|zfF2IPFNWl!^wDyp zm=9;m)Ko}uq^`Kf+O}Q0mZnzU>rA^lq~r!ApWbc=^|Npkvin=tJ-di=i7QZ*xfHM4 z-Q5*5pD3M>DyrS3k*A8EYv$xC##K;Ia6k?w9#IP@y?y(y4edH4Z=~?j9cg=r?n4zH~2tYx|lTq8##7(~|`T3)IHxP3AZ1 z++Ue?KiReC=y$ttAtMsqe^BS6)B={qXI-(=C%c)4I|E_diW2W0qMfQirsOE| z=rldSj~VzFh^m6=r=4qZ9@X2sc{?IU!xl~?q}|S68}}x*1B698nWLSS=3q6K7M2WM zTG@U9#4lT+vnN3yJ?xzv7@^z3@>R={3bllF-cFJKTHD&%3VDQFUT*Ol(=j+n6U$ev z3cgfuA+ff$p7l0wB1Pcdym@oahVv3*Dt@*A0aWbEYP!Nh`$zUAOL;x5|A`O8IlP3C z_f785_LlT40qy8E8JXFo@R)W*?AIrCZfrVb0T~(hP$dOZwEEPbfMB)eR0Q*(>qt*k^_yqg7?4j&O!jWaap)-YTWP%? zG@=)<2wxl9KnxYwvxtg`(UM(;3EB*eY$RJhd?@6e;eC=elSM1u1dj043+;C0DsHsStBzH_f|aiO zsVNKBi>>FxLnU#DiTM@j9-^DQWjg2jCz_j6YcDSB++}vA%TOPpIQ9&`xL*Jfw{u<3S@U~b;_bxpzauhvS zCRs7wdhy}~c4S(bHb$6;pzc$Sw=!&scW9=U7DODNh(CRG3=X-h55TA+(mK?M50PW> zlV#?w8Sg@TE^^2@4Fw6RUPy8u3PSunXX?1g7UGS9-aqQcMU{G38{X9NEGr zH5eZ z%{vM(5brB01Ztd+U(|+}0m>9UoJJAR6m!~}N3{2*1_0t>u81=R^b{eSh_26AXrZ*& zR?uIkSJVNvqQqswZgH}_b!Miw{%rx6j1#`(+O&9&;o0*2YJGr@0BnHqw4Z>jlT$XJ z#9UpgXao~l2%Z(4FV6MD z9=MPFi**>qNn5go%((1Hq1LtQgv=r=@u#Qk(~fJjSMcy~xHdMe*N4C0ph!rt)~Dd^ zRpn_et7?*B$JapP4ycZ`Y5Vr=<18H$lMSyBUgO>j#}G20oV<`*YO53!IcSth|5*cb zR#F-oFSQi0vVx@tX=azUw*|-Mfu`avv|O!f2}p2K5NtBaL^A!({Ve6cTGanxa zHa7P9P^JhV1jEQBB`njqo!58u(!9?krX5iQG~GSRs>aNa`ucjl&~W3&>9e(`zXa1< z4vFvGGqYR&sn=u7ODpmNv4sJma%$k^ks#<$fKX@&h*+V7Pd)K52)!2c$>P>lXiN+V zv7TN}q69tIF3{+Y*N9+iX>}Ke2X8@&Q9E3_4Z$}5)K`B1id3e8i>hjsy*GN>*~QgO zsm6&tI3#4ESZG$95pnNGqryxEE&RHcX9qFue4+)iR#dSgD;jkKnr)ZkohhlzfB`{~ znqrHy8%<9?>>WKsv{)PEcAz`^`{PqL4bXkT`Ytvbk`G8ZX}#DO?A6`{B~2U357#V^$5U1d;H4W}qItw> z935S-Pm|0i#|<3Bjpioczo+2r?5td}c&f~tmlWufs?&MQ$H&G3L2_fFle?E;ykYSU zB#$DxnTaxIdeHTW@HzQ_iyJD|<3q?_rV1U9M}CTay0mUW=eg^JOSm2Z8YYPIv=ghi zxcCLPJ7GHy@5Xo`t&Yb&u{L2!6#h>DMyU8hjn6|9dy;q~d^qHFnHi0 zZhJcQ|0u*@sFzMZ6M(!?s17G>bu-x`wQPhq?L?ZclQ<8k_9k%<=3xSPfa$Ib0i4i1t|n zBC!U`is_#997%v&5PJtdbc7;5$$<_WpPFa6^gJUJn0X>ZT_i%{ISdA(zb-GY4hjk< zc}IAT(q%U4&P#A2T59M$IzozSyL?Y(UiR~W3Rtcyk3X1c% zcvrcd8?nXH`6!75oU5HST4*cglo!sy)UQRYQp=@W@E6r_V}Uk!vfNZ2_>v=$cx?TBh;ps^Z3R#t`aw(Dqw3tZexg?v${iBSG-Ti%iAXr>r zq&5QC>(93gK=;Z@6O*|k!k!%8SG%os9W{z`T-RDBDz+ys-;^IuQLnm0~LA|vNn#S6PMw@&R^{)0I5=6L3% zz0+!~`AA8=;NP^Rh^P?ya$u8WHVe=)=Rri;>x9HSMcCt35$P zBw!9A$S*#JcZ|%NQAzx*d@3D%Hzd*}KD%V$<&B|)uMNW8Pi~TvRmo0RK)q5iB4b1} zpO=gNo~j=&7=P=gvHBCiq?I*QgBkuy@pR<5k3hrnn~$t^*m3y!>~Fq?m^k;={|c`r zxc_Ys`(Ml0|H~e|*?NQa*zhNRcf9}lfuI-#@X5cg`S(v9D?|V7^BVE{UxoEd z7Vj>2Hg-@`8N7~8@$gpt+`)q+Ip$iAi)#gHo6)~6-z@ztQ%C)J;{My%*6|KDm$t2Y z)wj{7QTN-R{Zz3YT$MW^^!bRn%WKEWT-(1DVdH0XwI zgU^6rlW|FOoLzC#5#}G`GydhK(|l)F8le+)<<|zjB%_d8A}PnEVwlF?H~yXc{%^s1 z`MZ9JYGL^E?~OiRvldaOn15XBi~sQfHffTw01;E>>l91p)} zqul^^{PqRKB?UA&?Bc=$*i1Tm^O7)+*w~mD8GV6TOCu5|BPods!NS5iMWFx!^a15p zQwvC0JKoi#tjbENsxAXZAgPt~>hB#L6!?A9UCiPH4M*L&Tc3ggK8+3Sg5E;xt&tJx z`2ToU(vGikug?Sf#o*wepxr4~9|uQeP0iBwif?>83$Aa}_Eg2f#)6~Tnkw8l%r}F* z$a1PYc5vqm&=ZkKNwRKk!k`0;9n{Hzs~0daS^OqV)6&we;NsqkL@ zm?)#ah-GJQZ#u{nkS(92%V)OllGxPrbb4k6h}@RoaKO0su;kPH#=?6zoEK#AhmRgL znU(36sRPW#EGile^qe0i12mvb32cu|j_=kvvd-vOmW^s{yUuz??QT(vWMvK|JvpyN zI9ZgMVXM9`i83pef~zQbTwkb!u?_lj;}yKX3q;VSKDrDgXzpxSdY?ORCRb>j>lNp) zZ+6tICdu~8DH8-MZH9E2!$BR9ifoO9$C>Td9~!IaR26TDfDWE&yERH(WYDEJ)UUSo zGy(@e{`FD=lD}qPd)}uj#jQ$D&CIAaHa7eM{09p?-Fp=JmRDA7c6Dz@MU_od+2b*M zVMvgRPnQl23v(Rr83qplYCXFZS^lU?{~-hW_O0K4>KAZ}oIG%%Gx5WPC?T!z^T&@L zn|k{TW+S=`jg3q7R~`YY2pV<5!pC0mNPD1rE{XhS<* z?l)YwSu&9i6E)^aGMV1N^}xROU6cbP@y~o;toW$FllQ~(Cn2K>&dX!n94lG_vbAI; zao$L9UI?o(3yZ!!72I}-S-s3GC?_YUdFlItS=LXJ<>x{EPjB8y{;bIETrF8ivTVFs zHPSxrm46LX?x>OH#WgH1X73-a&!){8%fZ&?8HT?RpI$N-26uFFE|k%iml!D$hM{px z)=3@*8tWy7eWB-|0@~C+bQaulJu*JMzfWsQreB&xkSifzAk4(~)b@b6hnXadnxDA=5%6qLP1HK>*`(0Smn@vvL90Y>vuP!RAijO6xhrLQk(G}y=pztS#By9qOVY5WlqvIMXdAb#$$ z^M`GvSbzcw$^!3|be zhTHc#Bso^mszh|DDW7Qq;I3?Lsx)cBcbn$+b7>ncEPiYuj|X8H8TvihG0nZI>TM&f zu^-jTfJSXGUL2)dpt11!eO6bi4fltUYtR@hYN)V(jjd&+2)pl$YP3>A$%ot1y}0@1 zF)P5tCpK+LHFPazAjotm$F@0A?-X^WqNd4P4vT#K%Hzdib!1pg#NHun^Ba=(al}vE zQ495+QZ#i(m$+Z&mYmJw2L(k{aj5EJU5&2S0d=*XY)_y$me1Om>NzB>v^iK+ismQ# zW`K*=c&*UVVy+~Xtfb((XdHZ^-d+S`NFcz5FSYX~ig-iEoHha?;+J5}D#I9G)HT8z z_sdOLm4=kY%bZzCZ4Yy|lgI4~7{$8e{X@AAd zxP)iT-}wp;I{R4lhoxA9ZUQHvn12zuA`{zmVBw6xwSDujM#w`cyYW(UN^g$zR^AA) z?v9-Tm*bdNInOdYj5f!++boY2K%6PF=fEnoL4OHsIxkbdRbOzgMdUGKr9{W|@LMiE zU}I~TnhL~t;_Xi%ht?bPV?`gY=V2s+)7(DA;pZ13;?=)ip9fPP@LwhB>@>ZCZnLFoLKAG7eCbbcaF=#=o_Kk23=m`_3U9*#S$v4- z6&TA+Z{E0h6R@rs=J9Dg8eGYQ+ae3wt~-|3fmcg`!`d2{tBC_!;Z{rPfn|>0llVWd zex2b7AWVIfvp`>8km98HMoFnBn?3oP4rdZ`m;vhrZxE<*=K~H<%ZcX)*}b%B!*4Z7 z-$1QKcf{UDBuA}t<=D0Nt60O#xYe#I7v{RJqEy=}`v`;nHptL)GhCmKj=t@*HU8xB zV~FFgQ6p6OHlTF|>dDP0GY6#W+#DyW9jW=TzdNnYjw~NyCK1;3$xBN>mVT(xXTqlh zH#GYI|P^rx36>g_~Y1C(`5ZfDgvXKCS`Z$r`m1tgiE0T|y|wRk3t zo0XKBzMzw7(jXif*wtBCSBQudYwPN`Q?^K)ot?qU*eWVITK+M*wPgWN4mcT5(@1X3 zx3|eBo6OaxHZSfwaw}Qz6dz0pNI&dogf*E9Qu;zz7j~F`ohEd$m+O^KJ}m!$r5d9@ zf8spb&^r|mJqgV+r9C}5F`ag7pCOFsp0Qx+=?;28&#PK+!Nr-TrE&Zw+BIeAW&9Ey zjkIoHx!_XkIh%wN9cM#xYjBSFjBL=RhjwCZ_%#(pJ08Lk{V5vUXwe-C1Y#y+--GdjZboUa{mYX!3UL*m^ELE@R^p1`rR& z^wWxHVS+njq#yGvrfbDE6~os^?NyL&E8*$A0Y}s_J=Yg&NNaYFZ1h_QVhe=F6z6QXH)Ii5QGpC;IqMCG;qO3{Ol->ROe9krJ*$#Bpl0eQ6Ht zS1*@*?9+^5Wzb^=`y3)mD3@Ho|tlm9n})OhK~d|QW1133sP+{>Cw`4R3)|Jxmd1h z^&UrG4Nh7)Lb?M8Vj3S`6u#YYafa3qrSWW*R3_NG;m-U;y)i9(CArZjwzQ?bOleU# zo_~t4#OmdZZQpwd^b*MoVo5(U4c?et8E$AE!qL%@0IUmFa_Y4kH`@C9i4=)AndQ-; zEIY1IW!~GciPby8tpO@#DMD33fV26eEp{IF5<@5K@eu*d|Ed@v9_xk)Ato6Lb-lUz zlcP|Kq4CRc%i>Q4f&J!)Io{$+$b|Ah!P9AH zQo*Ci_=B&n>7AKMr9N+t9ItENvX-|xGk_Oil|7|-{{6Gv-BXdXQFa_sA_g-x`r&<(`Nv;oE|qr9?hcnwK&% z*Fh*Ykt*nMQG6r5)p4AvK{`en&Wh-G8r0fVar1bP;q=0N7)B<{Yd>ox%ci^^@+hw1 zuU0rJui5Cn&r4a^@p4O%*RNk+xq6k`8-qPKsML-AQo+UzD^uS)Imx^$LBs+lF@`^@ zpvOoFmLWbod#YNc-zpS?fQZ-E6K)rQ)}x$>iA%Qu0@i=cd7h+-ooynK{Xk}?6n6Ml zcY#786korN@^spaW7h!Zq6QgNE|JF{_%DX)J>3Bkc;VoS`>xvCE)j&ED8E^cYrF?; z@q|U~+3_YQQ?jq1IMr-t8ga|)@~7%t6k*59MGK)5gI`OiX)BrBC|NSG;KZ};2L!*Z zR>v&Nc5}~&4$j)?fy2_b)1<+IPq*Y#WK>k=M=8nZgaqlghD^xNOHMjw#utL(wr|MD zDaI?s{Pp@h0$a@(A5Eu!)U_^0Dz2^0_PQKqn{y@iP=rDAa}F8bDrl~iJ`xYQ3sZ4o zLVi|G;1v^wGP1GV(Xpln9W-}huPVE;>;qh!D_2K9OSP+xT1t zqf&q~m$7YQd5)v0VSV_%~!Vc?S%z} zKqp$YQVU{B^|Jd9xW+N+zzv(AN_(Sn}JM~kv8MqCu9c7x;= zQ^(F9P6`BOAI3j$a?mtd%AS9|QTMgDqbaHR{Ti*JIU$~sszu3|h=$s~StvWl>im4r zkle#yysiZL6`L$PRZ@z6zb`Y#16J~YRGA`Zi>)HizHNK1XlLeKpaowIMSCf(Z7wbS z#B_CZGz<;JL`L2Mr9A;^Nd?#hGIn(?t7*ag{e2)v-xU{61LYmSlcxZ2rV2YYnedfb zPC>ykAJBQoB}2okW~xETexH-W0=&3>fhs#&ZSWFxOA35kJG;_8jlS;gwNGkIv768J z7}_J~+h=BI0r>Fp5(g|bC=cPZA^|luBF6AUz{!A6+1ow5f=|I2*;0Q8YT@Hgh2>Ry zGP();1P_i`=;qV#BzFhL2c8Rj3+y!1-z=xC6}1wT0faL?(OU0V`vOuWsfRn6=29=L zwDLv>Td12mGg)5Prm%o+kJk1?wMZhaW6iDkrN%~IyuJPXrq)*f{Ct|)+FD?6YBk|| zCkvw3*8PUIvA}Kk(7L_lGi>uK^J_~XHGz1NnOIz}l$87mC@i3?R)tnqhIe!9HQDUr zcQUl34WL$2I;ML@YpcbE6X}N=BZ%qfhCsrvB@V5(&kwgktnVZ9+#h<>dIFm@Cop(5 zmT6VrCZm#6R%R9#ub;LXvIO#;VE|#(dx%8473HmWd4x45+s2#W>{mvvWjv`{=k02- z4r3{GVoDWpG|*YphFZ)%UL%Z6UKey6MQ-r=z)Rr?V%B8O@QC0n-tx@6oeyQ$RQb{y zaRTRcWO%c&YQ^f-r%PCGlJ~L{TDm{|@N7$Fo?(n=sXe_O_874bFMsKc`(Q@B!EE66 z&x1CVKKzTWaFj!iwF*d{0M+U)lE+|GuGKH7vc-j@9sOFbEF{ZmGy2!MKa23W=T6FG z$T~C@c!{t!(V!8tf?b}0_za8bgq*(2%9ddG#K#Kz0oV4JlOEGMYN-z&mFPn$SuMBz z_O&l*+2P(l80Y@Ic5E`AGEb#I{}S_jz9$QrJLm51u2(i;!z1FAy)?2jjG3%;<&}ys zegy=3o&RFvNWLAbV1ExVx)6 zm?h(Jx)69QEOB|kJ5%GlRyL+>F#PTH@08oqBgaN|b8l}Cu#mTI-D((kBnL^~R_w~V zRCk>f7OOcDgK0^~?gg2_H z88k_nVz)Il;^0w^R|xPL_b=!w-Og;UZ&{;9bic>xtlgia1_~Y%8=s#>070V(B)Jzm zp;%eAD7KB?^jwE2n~r6CEC_`=yf**7<tyof-Qo0X}Xj=YNhmf_4*xAei5 zu~OHdeJ1=3QYq!QdqsT*s4DSx3U(z1B5RdQYn3fw`o&=zb-qu0sHCO8ncpuc??pzu zFgHx{(RzL1B~Is7oRoUj2ZJfYMOWeJ{qb$TnwbbTJ6Y1VU|?}YD2)(Jvk#yz0|=-C zrcx=ID-|sJzc9*m(gsM}$*_LH%%O76He9fW`AydOG#7*k z%BwPTIz>|wpSCf~-g>#WVpezcP;Gg6`PCcy&>ue>cGO%r`!A7jw#p@i!8b!0x5*!S z)W$44ljQQ~_&8MfMp?Nj)D&)FZvItDohXi7hw9+UZqPdf3-br6Br3HZ%QMpZ6}h=| zc_Wf%QFd{hI&I`uXV}Vl{zmk*^dt7J-|Yq6S4Y^qyouj1%QK3wCa zyT2A+d6w-O^?h6S!G#LdjjJMMzH!|{IS1o-AUsp+vmzrSe{Spr$tdQH+;){{zbE6K zUD=lYP;*GPEITI-}aUQmnCu=g~637TTLHwp4AvYkpST z6vlb0N$>K)apx}tU6qnLHj|>uC9UR4$tW3`^=rN7x$c9%ya2DAkh3H*$7x^{#Uc-h zZn#?M=Agskp4pxY1-JecFkaNO4w$A=ey}CA=M@&F<>#|Of`fx0yD0R&f44p!BgSXnXOxuFM51_E&V+QI8ruO7d#=M_(nc=f>VzOgq6!(CB? zJ*aP)QenEg^H`#1-E*&$wfn{}n3^ltPk#N{?Ck1UPMkvrD;|>#O#WiHwQEq4;_B6K zGt7LZZ#SjaSxZa00h#H2>Gk&AJ7Rp?V$~jeN}kBkQL+FcdP&El6047!!ym9D-am%a z+^0(xBdnPT7}0Nas68hSXK{*9sZEq)1{$MNGHqUO@Yp8f@`;|V?maikdEQB{pbGlYt4W$=IEZtWEbZX~m3CVbgJ$0Siv0mE`H5y?D zpWZ1buV)v&Rn^AL+T-)Qo)n>wloZWy<#&;ZQN8FB@do-d_W<{u^LwVI0|i&aW5Ac? z$Z1JsNoI`-O9lx6iB;5=CKipt1s{}uZ#KiZ(h;*=smbwc)4X_M>k~Z(Yyz*l%5m01 z>5Y=B_u1gL^zNb5=FP&iW)Wmv?~mM}&c3ef^1^`OPYz^ksvR#m5e)?gR*d)W-@ll! z_9JP0Qs%QdN7!zxuo0+wC&pHyd-DkUd11fe#n#-t>v9#SAx`Sb8LPC5qDx2GkGCp^y=EPywzu$`+Ta#9~loWBwpqmGc`EeuCc*9b{JO$kEf2<3|2 z5nh^$nW0(4m@1^9zFn^o`bPHmKRbwzI*^6L$HoCEB&>O1e%pC=Qx%+#xYLT>nX9DiorT|+ zSH154R22d!!3TGmre+nMNa7L@5VX&oEC_Llv9klAV({nB#~S9WGC@J@QXk*(n^z7( zIr@U@^^l}3CLddjH7 z&8=-uLvSWK$x7k8-}3W?7hEWZNBFr6J03rJ^vKl0qA7Mc+*UJZRD#ogauqjv5&oda zNCII1S^-i0`j+VK?KNjM-K9-t&>ejUn!tEe9HC_-%i>$jFsQpUgt{h_$9e6^b3w5wuO zW^!8^=>b!#gSn(zfvn=6z4g3rr(6fD>hB_C3_my)vF1LhuF9gbj-U;(cdfQh*itaw zZ|SV9IMu2@+>I~6^?mzJB|0tb$t4EI1vOJqd6I?_2LBI0AN$nK5(|B~f81g?y<>(o zGrHQ!g@55C0Zpcj?{1CoI*ni{IEnE-+sKKyM5l9y?#B1xH-9c*KMxu$Y#VvlW(6X; znOO0|P^Y!m2`i-jdHVf!?H! zVOuYc_Ylg?^!|sMe+i--Qfny?E9(;_6`l6wCpdt&V?A;_%8@^J`i=_~ufwu%7}Cs- z|3DHkRzmJyHf^gkql{*R`qT9&bo~@2*FtkuoIOz#rK$yozea8rjlHZL3Tj?x*l<{M z5C&s^l$S@(+Y#emDW2MXEgzhseJ#1SYklAu$mXrv3N7^4=51PDjD8G($3#U*X0Q_H z@b0+W&v;^yY#g{tU*tl=>-K<>a)asq{pge}CT{LDV#f2K@o{N8JF4w%FLjS6t6~_k zOuU_yB1JFm0NYfH!v;2Sb=w37n6(p9NOr#Man{U(gnLNVW)Q=a6cksmvC4Ni82bJX zX@4CSRlB~A!viRyAfU9AN_R<@qSAtdAT2H33_a2!B?8i2($d{A^w8ZyNq5KZX7Bwx zdq3aLfA2eo0*8!i&06ceuQ;#sIlAvi=$h8E5OqEX0YtsCHFzn!bUN+r|~ zH$n$&fak(230gJKAK?TZS5^G{w`GmI9%de^8CegH%Hs0!f}a`Pv#YtG?&NVZa!McW z7Wlj=-x=>3wa{?ht@lqOoXRdOFDrKLru~LZaeJB|3XX3vLq;42PBt3u?r1RAjd$N< zWY)$Czmq6sL94OFU!Qg>FPzBjo)YC6$;Nfg&ANC;L?{=R*zGzAQ5#V2ceLm%hTdF| zF=S__XJoc5X(n!yjq(nc=q*3kZa8_C$3vPpsox40oG39S5`=Eg7YGuf;y&;Tp)2*C zvCj|xxL;oO=z%Zr=Fzg5*14;Aj4FaFhN3oVVq&PY;rAQnD6egmH8d*V4vaJ8ss=G* z-QFjnUb>As*XN1`Ra{L0fC7q(@kT^mk6`SK+9q6!r;+gJZ^<2M$3A`(%?}^m$;n|y z%K}#Rl@EWPNcbhUT>S)TeG$R8&UmA!oDig8@yGEBQB9hSuXgX>zu%4FZsEPmlur_m zZ+T_$?qSM%`W!eD3rq0iq`J$|`deMyXFxc^sZJX-CIvJyC}r+W&0QZyOvxotSQgdV zt;&HGCy;~y&;?>@YAPucIC~jyjE8YYfE=%34gng|`l3Jm{>c1-mm3d@PoF}^!GW8* ztPGgs#UqzJCm}vE?AX}Y&!(oS$;n?vM^%6swGlwt2qb`hZ!r!jIjC}1EC9#T(wErz zt4Q1~TyNgIT~^q)ExGR}Mwb0+X#sv6l61Whnl0c0sw(&(3on`?=U^;-2zC_jE&RC&dq(s(AgA5^?^)KrT6P2>?cpuongzYSZIsLyoIsh;Ink8w2S@8 zANlyyC-5=FM%Qi}+o^Xg0nm@l76~m^Nhlb;&G%~~<4%fC`W4Qz-Wi)Mt5A@VUojr0 zr@twvI~SPMI>ZjAm3z)a6_}GF&B<9@i9D`x+KUwBxcPY&MD9+o!IKA|{m`|c8eTlO z{DX#q`{AcfBkEGo)~vf2PSwL_>4Wrs@YhD~r)yYMan$0zF6&VW^IW&YMULd)mdyODJrlmk*GBRNFsRhU9GPlf1 zxvU!GdjI(NOC=>L&@#)Hkkl8qX;1rR@$3Z(*-WL}3DmBbWuTXSToWDr7!+)tAAi{M6YQ6CZ76>ypVPz0Rf1SA?L?ifo-N}SjXEF-N{Yf zpHE!&N}WOw8pe8!Ur-jxn-zq#v$Gjqym$e5|M6qT{&F{H^<_Vqw!;aheYeP@bM_!q zoBHuuMrt4+J}H$+ku=H1nT#ai;BXAha-NqjuW6XBsK1l<}@Y0 za^BMp*}RJWa?`^rZ5%2|HIMVDm~WzLy$~zTR%2NI;KB`@bMfR1Y?pZ-Zbn8X&>#5YMY51z@W+JpVVg%T*?B_Z9~`*luh)5%l__XgxP5hQ z#zY533FqTNZ>VT8mfbdLL>I4J3H$r|CAGDASfk?J_o%;Y>97BjH?G-Lv%wfGT>k+2 zBhLGt3L)tlqsia8Tz2iYH6QQ_K zS8Kjwn*#@L;IzWV^oil&4W!5WQ$$f=k9fa(kF=dmhOp~Te~E7W8NXz-9Z%}CvDLug zJZJzei%(HOPR@}6EFgcP>qDW6IhT85ef1$`#?aZ(ExDCUQp?-F!0p_>e_!^AXp9t+ z4ywLwOT%e*&TJ%?bQf2uK;*QsrSh>RnUL}uMZL!v8EwqkwbA|*ZqY#$!l@K<#^j75 z9S&pKl!*u)?#LRV+3tgz3+Az&>ex1!sNH@v9Hgy7Yg9*wmS9>EL`3_F2;=mAB0HtE zsqKS<)tRoFx^v@9`M6hdj@V!i{+TGj!4WxSZ-AXcr(F^53ZhX-!u}2qk81Z;HkfNS zEaUs`balx{N?I#uYxjXA^U-N+COv7_F!>Yx^q>F;t0Bx+q5K>q6&LC+D;{B>?@_{% zW}PpZJM2I!fXk8%a1#fXv6N>sAS&|Qje0Vv_kO_T-+8M*Sf@cVSj6@o=e{qphDgqC zv?WmaCT$YdcZ>jjZ)|}=p4-6tSC$%(5B36OZUHSf_ucTeu0E}rW~rGWYIk{ulCjm3 zFn~(e@SgKdvc9FGCPTRwu#Z>r`FcSuO_;NI3KL+t)+!>dR#hzG(H5est*c6w>V~b zInU$AkNI2<>5WQAN+x=C7g|tmm$3Yfwn9J=6RM|tZfgEBQGz75o-YgGHnbU@x}S(s ztmD?+<5J-_vwI~__eKHcXm< zik3yOC9=Hf6U53@5azbW213yAq^Ry*yF#kjM-qyuad3+IP|^BRNZQ@_a0lOFO$ra(sp&5`!*9ioHanu6!}59atPrpSr??!%sT=!no~ROSRP-C+Xn zyhUqv0<7FUCvdL4sJrPZU%Q{}zGl;adbExY#9(bFg|wOL>2xZLTFxSV&a_ps)a~76 zRxXPi7*pvIy;}3X>~0W!6h5vv-8pTK?5(|CF_VoE^3Cfpm+tz;(6Q4-v{Mzjns|c- zWm$`lufZQ49@!u4X4?mAHGJMy!Tyuzb~?4RjEq{TJx8d}DF(!}$^0Y5Q11K=-oVjb|N*3rkkRXRw~&Uio;;pOQ6tzMM}dEH`1u zp7xkR)%I9^Yt$DlYG{nw$G zmr@h2Gz;Ez;Y1a=o!y%**`*j98WglR_(RrcisDYQTkgyuh7Pduob{ZJeQCa6hUJ11_Gt&>Sfd&>$GE37-mG&!`c%iu=6owt&Q|@@Z@@fh><0{l7hv&{riN9UW z;uRAN`2c^{^kYylCpVN_$0#6ANmWPQ;}zl;d0}bk3n2;|i4d}9qM}Ivj6Ybh0=61%OCX5HZK|;u*o6+7@}(xe|cwKbxC_<~=UhwHqjTGO)41k$d(sm}^?OH{aS; zn$3wn<}Xz7!f{@3eQJGS&LbEcc~KpWzd7j!U#03>_}X(67Z&#F^=mW}lRx8IjC)!uPeP(EaHH8_2 zGs|&VxQ{l47ngZ=>JTBRPaxL|3%MP#@;=dR4lbPma*5~uHK*$hwX)R>o|>x9mJQMU znPi;Ktfuzv# z)UJo`+KSzNe}i8q$_#TYN#ryzI_RQN6d`&ctfre$CCHc8plT{`E3KtU9?b~S} zU?R*O5;`DkSouE+yfXNf@8WH9soC+qq`6g9_t^M1dX&Ne}p>#Huw1hLB2&k z@O-z4B!=q9=%`P)_JDvkw!gRTYyV=$E%lY&IJg#crVXT_Ai#cxi?JJ?bm;@CqzA2d z^50`(I+Lys>Eh++x_`2?7;q|RXaMcNH0aJOvu5S@mron39X5ys>@h;3%h<0Yvi6A6 zXf<(Y@o2;O-S9>XX!~cy%q;jbS!wU|s%Xd}W##3wQc^tM)4u`E!m)RH0!L!JRr^ zB9;ucYp+Hb&@?78qc!Ju5&ZtZ)|PnE@2 z$^?z%z^YaO_{8b>%s58am8XioYICy0yUE+nX37H7>7))?xifo_`edw~g|upGRM1)= z0iS%@UyvcDW!YctdVv!(uZp4-f)<-@3bSx$Imkm$R|FP}^t8$tAm`H{o zcp!fk*m6lh+Fkj|ajfbb73tqWyS5f9nXsFErGqUeq)k^23TTXs-%1@4IUSyq829R( zmzwlcd0^9)+@-=d*qN38{Moz}<8GQutsgeImoBmSbmgPC-u9D*8+VY|`T~{}=rO)r zS8aGup^%re2p7HP5It?e;d9!33IfWorJdV%ZaNBXjOL?#8t+e02-uPpVAON72reC^ zOvxx9leZvAjD1|&3vYf~N2?U&(WBy;#gRs+Pw(^SK@kl0$n|79qo5$<+qVY*2HHJy zQ&v`fuc;~f6!$B$m4!vHDOT3k2N+?Sa;d5aH?XqFXUlnxw^@K`vH*x)v6T*g952iZ zvAX=x0S!9if$5EYoFU=D*qYbHL#9fPZ_pdNRXX-_|6(U(wIJ7(i%&H1MTNIPR6>d{ z-f7p?>(isTcdhHf(jY}jq77vWQHG+*-y9H~O(1nXtsiP)L8+!T9DnCK3VIlm>$lS$ zT={42oi-PdP1MD%R7-zjjJ<`tFn8jCjKr=mkHz$xd7mnZGzP%Y^~K!QJGqJZOzsNPR9#jN0PNBEM?~9#h(z?!J{a)ua~U+4@+NcYoERa zr5Run}V<+G5Wm*28AK@9n_8w*?X@sAx+o z{)n4#b)2^s7*{RbDV<6vbWhr(Vq%IbZpowp(ESB(zkjUiKD>1)UHKLNo0#3g$>sNO`mXq04_bg-fCY0$eKZx3Ak6cY*k-OBE5SNS305x3Yrhk-1x|B4%fmw$2Mq~m;i{u2s{wr7`))- z1(;4pM@Mo&Jw*~i;wf$;VF^z^`lm&@n?rJdarV#7euEE5Cy5A8G~7G|{hx;$aS{OQ z@%)k`;$EDcmp3mlcfslF>+Xq*UX%mj0Y#&>W$9$AIXBs7lyk1us4_qE^j3PpDjzF; zEOEq46F;I7neAO~H&xdVzJn^lOsEmGxGuH?w`!Ls8Huuay}x&H1vbgRiUoYtENf;X zqpmLDI0D6Oixh@XlD~Q)Q5$ZNh|Ww%3Z3Q&EfhLtXS-VdaDKRrH}YT?6ed8Z_3mBo zEieuxs)HYzDE%4AVw-}RQ_%_f13XHOU~ML!sy%*FQ$)MUYkHatn@p(VM}|!0bu53W zllm_Xomyho^bEV3I-Fg9V;G``q-1TT2In3&#i!1}47aUr9U3mGa|^GBEy>l@v8~j+ zf{vERk6Rpz3353(Ip8*~*(a>?c_gk78p$Sf#=$N~f%(b=Kzh0PtgMH&$8etwyq=Sj zQ?171A^*fg1b)=-%a-MyXwHNcS{zzgx~*K~QDyk_=4@Sf_4$+dJ|Cs}#Djqm!u~NF z-(p6*Efi!#30kx)t}(aGRsU{uktIsqYW|vPFj{)ZBw4tPAhDn4+>Y^Ngo)76RMu{J z8OL^x4K+_Fpt5iA}3saC&mm zYbv3~)B9dc#G&wv0OguS-uDw8rYVRITU(yF9 zjqcR8K!74pPi(aV#S4HqfX=&oizF3a6=fAp`;PCwC<+0X*B(UCTWr`(%xC=+*&d9n zzu49v%~RVPFY*THiIOsY0X{P`b2{wi8jDgS9#C0OJt07?1JD?*f=Gm80UU@Kk_B=| z2%!d9Ft>j|U|>yh(GxQq{W5-xJE#gturbpzP*>V5cV+142rc!(_7wxrO8@Aj^i$kq zV746#qu%T38BBdHmkefajx4va;X(C$+S_XZLaM;7E&;J;cpf_6zqLPkLOQj3hB`b# zo>o*8Ng@3BKAnkokx7|bcKgWk?n5v5>dNt1^VaN@2PLS&T3A>(O--4YS!n>y|Val*G!!>hSP?u&Xr(yTX1JLve1n?N2(7vPIYy4fbT!Y#p3*B4Iv%1__Vi3 zV_>{&Ju+UjP@c*&_13Iu?vZ{&y>FgQL*lnCDo|Y%$0mQ(@nvO&mYMM2ZD0}D8eb5I zJQCG|V^=FKr`=DE+v80vyuDP!7=yF39!+D(N?Sunla!})Nz2_)co^u!0#3vf@|c!T z*DS&dx|40)+VqG%Fu`%I6D@te0+1wPHnG_j%(!c@iD6fC1@bxn(eIBs0W!M zHY`zt`CD!rQW5hn!TK;n6ssYHSJUpL`(EFjr}6kgkp(n<4`q!orKF@@43CBXh;_kU zNQ@oZXj`!Up6H=F!VPNBg=4Nwd-fvQ-1tkYNscpwq5g!3EQ01ge@tGgXE~(0*Fw%u z*wQ6J?NtiZy5p?sJALlsbs!LQAU7nntXzngiinDmm=r+((`qSm?I_tv+BEQ^m$?0p zMw+8Nx$|k(dbzi=?u@xB%v~fFt+v=@$?PTUH=JeMcYx6jT7ET~N4OJs;Tb zppVor&k&VPp17EW7MBH_Q|qe7lGf#6KLrS(ibC2}U$Vpf7v!XIb<-nzVf|FQ zFzV$~>(18k!9lOAjJs5K()@xAy31LDNcah{@DhLilnfeJvgTVG$)ywKbYZt$l7u|g zWPKdp7d}fxmw-e;ixzUWl?|B-xOl?%>17mRB%si^_3I;JH{qgDL8h=dH~r^xmoqL; zG%oLWJSEntZ)G;0afqgYW`d)P0eja{%(q0{x%2EVc^4oX99T3Y@r^kX}nT zhOadrS(s@>NV*gp>6FFwbHIu%yHHd0yrJpS~dsxaVN=cxdnkG+U=KXsD>g;Nkuyk8q~CIt#Z zcB_4$8Nnj&bnUfe^_davyLkNY1}adKcAnG9W1K6giqMw@%`^-Q0f6ie!0(pUfN-#O zS_KnY1<5$fWGq0-m$WSPAD;q9)=&a`$L+*{wQyClx6*OD%E}-R9xF&*Qo~z+uE$tW zINwJgx>MSflys#P@m#3v;9Cu*9yt_#)Mr|@7!-n6>Wh^!`}}LQ(bIcihT~J28VQG_o|MU0}5WuJolaY4R7NHG{dC&e;*Bq`xNpjLHvwe0F2Cn5RQN&e?5 zs>FW%jb8lwT002-&(IL2|I;k3it(>2|EuN>z8Bz1|7Xs}U$;#2+5bJ+M?nf7v1cGs ziOs

L@fJ(we+~lLm>11VOr8k~KMe1Dfb9kv*0@;cY>xEC1X7R-_xeB*|GzHa-$$ikna9!G#O-4oL_JrwyT0C$pdrgY zdCZB0g$5q?(@QHkQz4w*?V*ICf9REW&?Hwb;hK%+rRL{9grH&giV=%rP#NHIius74 zx9`F59>@12Igh{YS7IJjFcX1Zr z7l5 zpoY~b?z!fyPEJiB_6XYKv`V7B#ai1~$W@0Aw~nu@_kBbm!*W!<^n;TsnGfh<0;P7FFYrlpkzyxyQsdux+w>Zl%-mp&~?Y^@ninUZEA# zjO?#&#oO<;%YS~I9$j1ok-HM829wWaFY*#&ae7SOp_Px7ermmau#%X)2;Kdj5=6}Z z?ez4Ns{oemWcT4d>|<%D@$7{UuWDqg8_c)>!Xu}OLRL^4DWf_O!ucfR#a_q32~t`L z@{~n1lBi*iF+Vt;Z>BGhEaiV!m}Wt9fVMY_1}{ivm9k_(@T5|p{t9G+`{@j_i7z5Q z~h$wEgQ5Yq}(0-s%rR!^Mvy+o|TIYTgwnS@)zjyc@i)I!mwG-LixXYS^X3CAAozsO1KadTocIa|1&ev;3sHvp?g9L?4H zl@*#Nr=*50jN$0E4Vghs;kh2tvtP|A&RVV#j_b~s%b2g)B(j3ccFWK{?phrA?pq;hpk*lCZnkn_)MF0sqeCq+9Y2*zD8 z=JPY#g0l+}tazR;bxg;5(^PDG%TbinC06YWCv^)54HD=oeZkqeYz~gf!DxdYT`Gk; zQWC<`P!@TTbpF+>fdel~F!7z0B@$MDDx?EDlA%75ypvpC8wQ1y``m|Wz18$e@Iapb zhK-;wo+uxzVV)$|CN5rwKakpSUgr%NB4^b~^0+Yo3*tCpBb_Kcu{3YBBN40062b=C z_>eixiz2UundGM2N`50!IJ9@`YyN)ms{-hvh5dPclxqHU^vg!Xp$QjCb^N6;hng#E z=Nz7)v2pmkJxB#t4kkr}g%kMgR`yGhL?20g(#Th-zsiRrn9elDmX?Df{l{&7`+cHs zU5F*z=M2xb_?(eh?g{s(#}Dsp{aPoZd60JS4NPx@!;Jb-iM~ZnL#Koh`$v zF~WdcwVw!^)Wy}ix-S(~OkloDDyr)skHAy2ERQ9z8N$t+7Jl^QI{e&b^Zi3o?Wt*# z3p}+gLd(-*G6=dXYwOO@LDPTJ7-S61h<$%#De`kw<1vCFu?z zqR`k^1xq@}-PVc!;pM6s0k?Ghg+(4v3tUDH1+2u^5ykeAak$-?bHBW}++3CaM2tRn zJ&FVg@yaG?VsqFN97;<;*`3-Kvc&0?T4V*s{nP9EpLS%KjqvJUes9|djNyAxbaCHgDM-mFxLY#u#%t(RKl}Ix zO-x$$IN4Jn3t|OXFDReudL3z5MX5jKo9rb%{I$z=u9)6dnsNCWkACR>IYcEcGFkL; z+F0Ie#IorLRM($_&<;}Q;Y@y&b*$S=j)Yie9Mc|%y6!GbXN{Mc56~?1bV+Riz5`m_g3>m)}WdQoQU0vTwEQdZ2{W#tFKGf2-8OCqUl=GZ_ zsIQ6YD2Pm2668?>ohleTtj?skym@U7v}2kIdraH9zAieK=*Nz5M6EqCa3?8;4=Q3uC?NRMN9RO zS<%PppCOylixn;9`5K7o-i%gr)S4M-FD`7W5(eOZ2OsT3E$cNcS)&#gwIMvy5~J_h z&x{#|54W^+i57tRL$Fs^mBDc*-xWIT$K4^e-B}pR@(5*J=O#QeJT^`&)Wd}DG4dB< zj^4(cW51a>ijKOzkb)75%4{D2{ToUJ(5 z1;x~Qc*yPX8q{|%J}pK)-6t*opl(R#?+FCIt* zNF^keOl>*U%???*R^PEwecXTzk2!$Om>edPjatrxX0bGK>&s~!K6#fpN*IdrUP(#D za9i@BFMC0bBudrSh zlC&OPWZ9csoxn~|45x4o`fsL5ksbfAsjNB&(eS3%_tc6Hg=6F5YD(V$;?S5DRM_3+ zB$zojJA2`!!XptK*DMD%>OIVbrJXOH-jXsh8-wSoskM1Qhlh5g6`3iK`#Iu}eXf~w zI>9uGu>cTny5-!ls-0<`n@J{Y{@+{iJW194;&7QM)cwk<9mENA!dE>*>Du=P!xK2< z&l5YQ_Zoo;j)mYqhbC4{mujN(k4R#!tr+2eB20WAX19nABg?HY)PG;p=PMjLdj8f5 zfH}NqVoA#};866MKTYy3$m|zC8`AjcYW6t&_wBbou`&Q=K4=TlD@m~Nn~@PqM;&L! zT3=rWn49n1y!9;&{4Mby@{xn z62Kqs$CJ?!5pU!i3H2pHpMho9gsdnpL&4p#w%@pUHs`z}4A>M4e*AXy^c|u*HUc3C zBFp-iyu2QI@o|T&npvF=vrQ+hifMZ)Q2x-GEvQWQ>D4F)$(02>d60 z3>)INK&^cJ){q8N-@%pay>^P|_*uqArllz_7Grz;x#7 z%Z}w8b#qC?V?IfyHaa;f{#u_3l)0oK-2}Zro{KZ)^RW~7L(jmXkC>IOwAt3&bey^y z3P!b1+Wo5ZF)K`bHz(o_(X%Db2ehY=F>Shut(Ahi&^JGcjyJmLE{=6d-}rvjOcW@1 z2VS70qM)hR zax~{}#XS2?HM~Yh?pOineWzUABUMBfm-VjxA$-|*6$S^&KSpEQ!m_dKo(nC0j_OuT zTNMs=29ITy{c%ckhm1CPNJC*4aRx>P{lmj=00q><#DqvtC@i}Atev!@r=t;ce|el@ zfWu_wRL25kaZbbi^yp14j)4)2v4Md!aSoS=`qpR9SB+-h@dAZHDJ?DGz&RaheSN&C zik(k`t@nupUsS!pkBWe>JM2^YVUyU}c>ncey1T$92PpdR@Nluwj7vcx_d)4~v%32D zZ(S-0Mn*^nqFev6GWTm}XsC8+5}rh;%3iyMRaoqgW^k_pFGU;=KMS$201D8f9cy;I zQ+Aj1lTTOe56SX#XeYLex|XC24%Z%$4*!BLHCTxrQ^wI-tBQb;*DuweJ3gmW6t(TcG zh`RY0RK&#$WdZu?8-P<#7HYQny^G7vElh(ocud>3JOq{1*Q-p;fX)+FVV-e5&cSTN1#V~% z33n{ZRJnqp`h&(n!*0)I=E^MZ}OV31)8`)QK#^~)XYy0x&xzU$QIlkOtu_=KF>IYl|!kI&+P zDwFQg?!v7I&ALAP`PkR#crs9XZ38@0K--@d*dYXM!U!k#vn$HPVEbt2g^Z}8sVyP< z8~2xE*M+k-`T02o?Rp$a-;)qMe}-ym-JyI#t8Y~c3-c{3^0X>K_FMb|O5g(xzrO=* zXAaBFj}P__4M?W~^?I4DxFg09eSZVKhOv%uPzZmdVY&_x9rW9dcN0kzX) znR1CiLw|zY${~50b$*e#W9+)qAA+9n@BiQ@`j4lw{Ucz8>a@ND8kd~zzrBaeo(y|- z=Il(DKQuYnb#ryZ4Zx)$^?Xp`x)s<3=I2(gRmCjoLGUs&kPt%k(LgURE8AQ67RnJL zODtabyasHr6GArK1+Tl~TRg{GbBKGC`Hgx--(!D>fsMJts@`*{Vfl=U3#nHhfk?Ql z6#|N6WeyIPhsN@p*EvO4kF4iV$QlLU=~6O>yHI;NAqYnI37V2A8Schgxg~{j7)GREKyt6KWetzOVW-L)fC~sw`tEytE>{j}Rr+YMZ zYGvs0pMy7jdMc@QrVDCyU#Aj$W-LKN>4KazV5gHcH0_8k*YjFl9|}xPPPY3qseJzt zFKc9kPGgt>ECyKd670n3sl`7k=3GOktP+6gJq6A=_UP7M49CRg7(d4`O@p-b7tI3hr~dk+to#z??W_5&i+i?vcMNbi z%I8*{x1B3zCKzg5OIrH2Wfi{RGl+#f=Rx!o;fi5A^SS-fTEa3Xc!%e$`DbF{#)28w zap?S~g4d?kJEt~-Lqpl?8#13>x^K_az}tH@B&FqO*QcEsFO*%L$G|4WC&&BqWsE^- z9b>L29C#gt>ImN6IG$x~7qDzDW!2T6+~<8v55|b3%!NSu>tu|{$jKj)Hyfw+N#EOr z+}$$2KNnIMiUS5WwY0Q&C)9W>Z_g;l)=x$!hmcps#dd90 z9ek~G2}aV%E$UQ+EFLnmu>6_Ir8DfR^@`~V0{$8VMFEq8KX4@KLj7w50R{1`p;7x$ zB_B4$^+P%d3EB`+@2V<(@Y#BrS{iJ47{9H1c*t8LzD9ij9nw-jWN5a|H9lBC!gimf zD~d_tAoE@S#?h^Dg0X7R(kx^1TFt)69a&VbwR zbW`nsWe3U105u-1xQnA-8NM_Pm*-4TtU4=6-BH$db{USnvBP=V#B zvjffAQ=Br>(PSXp4q7KeQXL)b@2^J~PaIPU-FX>k^o~KbocTVlp*3-&aQXP9wIcmi&7d&sPtUYDc`c`@7Y|J!LPA18qa%6R30)$QSU4BU zstxwr<6)HEL}>$`KTqXE>SBMQu{c8WFJ819tb7#7s6wrj2I?SMd&f7vmjy(QjyHYI z@v2uY+td7(Gxfnh6GcnFp|z(c2sGN^fop|GNWk4TF3ztV_HtE%!5w<;2A5;is+Y|a z_clZk(7DxZ(Ts_Cfra&^a0u@aiB!U5*{}EYLXZm_3OCvi-@jL5 zqQ$pu#RxXlkY9GIRrY6`@DR$?1%@w$+I8qJ+rAOJ&sN4~Q1p=|U;?Uf4R*+tU@^pD zcNciE*Tw@S8r4Fr7SK5k1~_aoGw)hMSCeF-ZsjLwmO~aYXS%pWN!vhmbLK{9=lKTo z#eUtwrQM&$uq*PWg}D%1@Ub?_a@_6$O#V`4+%&hyUfn@V9+7?x4!#=! zh13wT$@M-Nc{oQUmn5Dww1Ctg!GS^{E$4ej%6mr{Jxm!2*~+vTW)_CN zTURc_VBok+6kE7U>7ljzUt9oQZnK0kZsR`jN91@d;2f29qM;McAD(k_M^05(>04UL zRTUee&~Gn#8F&8Btagiu3kVP=Z^XvNj*f{@$cf;U2qh0cSNj^(asDnYsGbOFlk&~V zFwO8ou1eF&wIrGJIuUrRnnPWM`Jgi2p0A;1)vA*BotwEn!`C+!BKEOM^6uvU*)tF3LKtsf=NCtQEjo+6{R?xEFA=3I^c@)d)i?84go7wL@V z5(6n5j9wXzjsxohVK@OzfpXh81uVOh+T zBdWo}0rXEMDQ@HHDe*B?Y9WRzILsx#FHic>hYxtMSTJs|?=vTcD9+AC>>p<)!n9~% z*LOQRD}Y$V;%r?W_Qht^1%#7}6Y^U*H+Zq%#3A~5MP*6$4t#5K>CMK^-r&(lR;|f& zd!P4sS(-fBcry!oDaH^ij%lEUbL3K&;w_*Q?vT)9 z_F2xZB;KJKcd^X~yofC;?HR0cRI>G=>4bRi=IcDYDR*f{+n@~6_;Y*{3ASUEbEQYG zfGC=1|h87%#J?5?khf8S>jNd*(`R@QM1*3hN{J-r9w991NYG6LQT) zt(zyTNQ_KhXnqZ-YAE1?gcHk?yj7=%@>G|{aW3T z7ly$ubIumFS5zcjUw+NAV?AE~;4xD|^aUN|2V!3dUvZz=VRIt9$5qobe8 z)VF8rWZE7O+>YWxIl4pvnZdorX`$R9szTkQNCDWh4d{nlaR)|~U*Gln;CWTP&j8<4 zlQI)RuRe@jrXDPph3uK2hXBjg3%aYpPS=c2pkxXa`${B{b-d-QKM$tMuS*86j~pb_ zd@#BCL5`?%y%`NSD;o)%oY2+P_0%q0H6P}0aU?mRe4{itipU&_JN_QauwSE1zp%7< zr*Xng0f-Z%jcwRn!cx=gVd}E{gnv0*A1CLS7YZK;baa3{r@Ox1HJdOU%X`(+78uV@ z+UC1P3~r}-J|PYcw5xXifj~ltw?7WB+Sc~AtqmdjDdumRxUsN4O&yby@i~1(!sWr^ zQ&?O?nt;jcU>1(NpetMC*RL2Lz>V0bcC=7WQ^_@%s3n2RUv~n(x7UEA>o+MgFfdXq z5_8T`*}I!;N}!Xf&syxVB`|6+ZQz8Y5(S4j{gB(0ly$8)c}N0gkcC?#(epLRli!wP zmYU86Z8dBzc3TNeNQ~Z<`M;8zs#V<`?6D$MK;dMnj_(_$3ut z`u@jfnp zFpihBE?{$OSqjL*Q99SXPt`ly2P0R#;&}1YiiPgj+1a&dlubkopU4OP(EoR|b-PJ* z2cyHm=Ts%-?&4sM2V&n^g7+4-mVIMlNRHnIDwc>lOjela6Zhq*k1+D_;im;*FeMvPXpsWkWkU!UE1RLML zz+ir9P11LN8Do9C=~uW)hT=eqmWJubFVD0zs{8kO*(-i0R|aAVBw&|i&=_{tdI9US z(Ebp(*ZX-olu6Q&klk$Mu-K7YeZ!0uu-m6u=Oyt9c`SV@qKLqM3RaP6X?&Tszy}hV zog2St5DiKzD%uuU`F%LvO95-LvZ^|D=#Mpvc06q`sX~@xKI*az#_rjnch-43j1=FK^AQ!98C14)l19&&Wv4P(gi8^r7_sgfBjfN89A?OWDZb zys=%A)n#9MfqkqL$;S402Us3VS(}aKMTnzaZAwp%UX2c_!JQhH+L`) z$ddj^@40$q!v42Ac$NqqMEQ?*a5AX`k&B`tXN8g?y^)pt1gGIImHeE6-#34q#RnI` z^#}4F8&&%v=i*FZ2C-ve{QAExIv@-_{oi2iKj&Gz`;5HtzX$oxPkUbPGiK}3LWLa2 zHuTz(9JT-Zss8#2zp+l^{;6{A*$!Fb?M>>v{QFFQAC5j5De)LWf~x+1ex*$euB{bw z9$ADubJ{%B96KA?qjTyUyy24&!$|1?2IqanE2`&mNgjG8@nGbM<)_!z(9Kgk8E`$? z1_*uEeVctD-F;&$QvUTY-9awS-CU&ADTu!dUYZd6G-eU~>oX*>23~sCeguz5=BNK} z0F)KL+ivGNYFO3E0iIk?vv0ok>+zfs%?1y0poltIVbL;Wh27rM)058beC9mk^3%`9 zddr1u8%(_gn}}kjny^yyWP2vf3^DuJLA~5+^7F@fp%+0`M(%M`mtUd&`{#`)-;fHA z#nT4ZtrkMfAI|ml0^B|!{_{`{&1?T!-;4~JWVf4!uP#TFjek10Ai#UYCnRKlNJFRc z#{sF+RX~QI$@IsZ({quc(~^fC#`Ihb=MP*jQpv8jFhBsP-})Q+a0Bxqo(OS|242VB z7rZ$a1BTdbc3Hp`jid@v&tF+fLHjn;1+=V$+hwCWNYU=i zYgE}5=z`4h50^DAm=M#G$jt?Jp;?s&uH5OOw<+kxp3_X#PtVNE%vRf%UUosy@iB;y ziEgTV>gt5^V|n#kcXdBzkG#CL@cTVtc4~Xh>)bAC+Ukn6HU+1r08z=brWP6<-6mM)MzZKf0Ex@XQ}Jy{?39nGK~3;mJs3 z|L|_DBR(6k`&zBp+E8}~3vWe$+K?GlFKAZP)`P{Nk(=uzi z&j#%em}0G*zu4W(Zx{44ccf}Ay^D#0Mt6nfGOSmlg5%`Eia{e2Yqd{jKH)?hLM(t9 zxp_mRR>NaK%wbLsf(j;8y%>=V4ZqMQZL$3Xzm#+6ei%h1)4|Xr3&%j3M83Hdh8i=(HaiRL-&Id=Y z?sgU8`sxC3JBUKmb5>XfSrULJB`G-{_-80UUUEK;EsLaCIH$KCOdkSX57G!CEWbL@ zA3Me#ug`3kA;2CcQBeeozXkzV&*`zCxZCNoZg1GR73Ou2lHw}8$;OJ|j&w9W>Y~tYt$}RPZ_L<&b;K&A2 z)s#Xm_xz=F_a9f=63htPdo-LP4AR9?t0^$BS77B_CGs(ye1hKGHQM3E2otYhJb#=C_Y{=k2yDrSSko9w#G%#N0^3s=1J8Wu*tpcfpM z%LEQPoUIiY*5YroeR)@}Q2Xvvpxnro1$$k>v-OK~{Cl6Uw*G2?mbeW_$~cd53!Tdh z&7SaDb&hcvrH*X`H3KWl`2>}X-3Y+uN0a7Ag_rAbY?;l9ZuBsyURkqWSJ!9lgma0r+eSL4>_)(5FILyw+YA1g^ z8Zz$fdIW+V{{c8vRNUoR27$x2kisg0%X!fW$UEAStACX8h)9prvU2*#+?fkzLHlyL*p&J;e3)x_F*v^ zdPmP6e^sXnD}G(t6nPZIEhD?(EfP$~Wqj(qMN+lq0BQKS$2&01;rvRkxmWtjd-l+!-Lb`INt7)ida71Dmev`? zv@V8R#rhGt%#(^IJesWAKYIH`GJM6Ibd_2p%IT>zm)*&bQjj%7#l?7pIZC@Gi~;zrf)3j z7gmO9IW$dpx_Q=vIqjE_Qp`p_gok6F-X&o1Nqup=b7r}ObEyTMKMz#QF=FMY!)J2yWq8Jyg8ik&9qpX!oz@3HS*-6P!N629PQMTz&*>+j;n zK7k;KE*fwe%*W1UYandX{t}7>UU{A}IFpCD$msVsn{*i#lY7kzrlS|ZXM#;|JZbZN zWfo)QDULWSSkBh7jc9nt$aCd?aWpAn!jVvXE%7U=8bSwW&t)8OJ`K^PkYsYcxGI1{w*HK4T?81nTKl#7y7Ab3Fprw) z;s7c6#H8OSct=$+WRBBenW@<9&SC{_-*BETCh#2}H-{o;uMG1`#m+zy%Y2L5`eAs| zdTNTKRzCWm+sLhnCD)CYXKZ}9bol63rY$}@Ow*8@Ixm0GQub%jz2I3&UPUs4vtU<> zR=vs`?pLXGwaBIp73$KIn2onuN9&T%a5!%exGegPTb;F3v7=1?@zc2P3z8vC;O_5v8$t*!>5wl#C>4teY?B9JXB3s zb%Op)uL`OBa6=$EjCN@(J&8p8`N5^{3eZNo*ykV3=UYRbN?(0<#q~6o&E!V1SZJ+i zRtlK83|R0Ucg3-ql+1>|Ei@kY0TNRyS-vkr*k05Dhu`6wOnRJN#~&6J_WOsl2h3px7c+T-E+u4i5?_z>MfGq)Sw2!7ONPFhqpO!(MYog)+KtD{ zvHo6x73qsY9`8=z$%?{od93+gvlrJmEw{HlbywL;zDsHdBz4usf3P+=O)G@LcGPa| z+uP^?q(Z}fA7FPE9STGtnuZw9F3?_QGZY#}_Fo!HqJX5ah{III(5frS^NECdg#b`2 ze2ay0wb(9(up8+r)FJeNz^_X$c4CR*eq`u}s&&ta7us^!4J*b*P;cA^&Wyi3HV`=e z2{pAF0gEp~)MKx;j+8s%@-JXQfs43GZ27(hBwJ3$l@RHOFS!g(cAJv6{QOW(r@hI0 z?CQL)(=lS}$pDrhMdgTVI3rGFMTb7aqnN+*t|380?SV;rpt4<>E8iYL1VkiAYo60# zu`vYcas{3}_*tU6ne{;DrcJ9)ZYk~WDktrW?o>jx2s7&?fso0}%5sD{bm#$95NQk; z>7m+vfP#VowX{svWPCHTzO^@_`7xm^RGJ^S>@-#n2q7C@Rl2M(9eyY|`0Y{M9)@Tq z+Mh)_^PGO1;O8c~b2udCo+5qor|li-@HMiOS@ib>*wG)Yt|yk=d6@s$gsdRNU1C`# z`St^Hc6hE@2M2Es(m#S}Z0}@kzO@bGK5k3r+nRiGmM2{d%+)8oOU{7cfb`ODXbtr& z^9ij_dLjp#_Qg6eX%-8R&7wNZpBJzfYh8S#`Ro)q%UcM}ZHB{_rE}Ytz&~IdT z?{)lkk}$s6YqH7m26XGi?Zxz((rBkah8#U1Ay1NkRZB}4Ad1(Z(mJ{qU3#fnE*_Zu z);}ziM#U5MR<}`6M07Mwz-s$&feQ<@@la*Wm!AXRQ$)up`nW@sKh6C@%+PvG8B|xIP5j#nueKAyJ&lmDZ- z`Z|3~ynbbwh@@nSfi!pUO4L|{ObMVP0zv}dOx!YAK2fHvt-1;cIaO=!8Z%Rd5(dg= zyiDr3_d16Nv&o<8^@X~fC@jZq3w!=EST%i^MX2zKAx10Op{0tN~@wGR&Q?+*z7?kz?=@@5e0;XhW5mm1}63x z9NDxkLh@7IW*cq|r4@jj1N07P2lM!Wy@g>Mu{yXErY;AM(5a6I0E!Px5 zms6o!taPZROkaIN-H*x3uBBbDFhDoGz3IQ4_;TGL*=D)P#~vb=juA`YN_R^xsD%E2 zhj{a^ag>}O`%N3tHv%UVduOXD;cj{sxKyH8-T)082TSW0$&bF^hHJXB_k4Jl-3csQYN`DlJ{{C9>*(>(;SFjE%@T54#n zu9(&OkQ~NeK?gv;#mrp1P+K4ocYu1)4^Fevu`=t=yV!JJU=&y3*{0Xc&e)otOWT+R zW0hq7(Iq+umB;lB4Sl7KVv-k;SeL2sQo4r|RYIe2ocN!Nw`bD&jb3h3N{A)PB=i{( zk4PyeF;NX)(le>3s8m+*EDxr#vu2!bZx3yIavlLemwIvHoBjMbO!~ZHvEkCmjt5x2 zi~!&GD0^_#yBbGZhVKnE7WD6Px=(-T*}2ox19T|W1_3T$Cd6V(y}J%sP`r6YgGVgv ziB5~wi-#z(2`-$$U5?vYudT-JWbn1Of$X)lzx}JFrDyJJF;sphOND^L#>-o~DVLn3 z9;}NyPu%!V_-8P!>{jN&iDlbD&v)CKn_046-*`pC4v!L~ox+}`y1Kp34xnb2#Rh#i zNLclhf$RsV4u&0Y%1W;rgRMidkj!uwPf8SzS4d87`vg*_*nUY&7esR{$% zcl7)~SF`Av#11O21B8)Jr6?1g$bO!gbmF3ch9#Y`oHj@7o$94Or%8pECW^$~&)ok* zWaU@c@X|vK;KHR#T|>;<>L1OH!2dx(!SE(ZNZQvNcfEbM5N8}P!a^q_gXv=r>N;?f zHFtVz?9D+LX!h(-DYZN2GG;f?*c~th%7sFB$m6p+g2`H&DCsgNgX(8(DBrt3fyA>5 z;5*gEOzledy*f0qP|=3pIVsk2f&8ytmHryovUs78g_xWUJXe&w??T5-l`78yX=w-! ziP~CHi>bT z!*U1qFy~|82+%#|k9auRPm+3XPgY+mQkEEH%c7c|);uaS>$&Re>;4Pc?qgPwcv6}OCETehu0Xo1t0_7zP7=mvrl%VkuqxXq z$4)!j@#-0kW7uBsC})L-gF|A(Uk!=EJUs6JZ_T!13*{0MO~st= zglXS-(?f!Iu#w3ep(cNJ1WOPWt7NM7cK*E2#b@USMamt(Z}{~h&o>nU?*`T0(M~ij zpmmY)ks|*A8h?tKfB0Vq$4VQ4XDGI7KSPHU8zmzI;jJR(TXUG-B|=AiUJ0=eK)=Cg zdO=U|OP>eR%@2{T|1dZ* zTvN|Xuh3;rm>5XLw)z&?SK^|Hpr+J)t`^uFbc%Qo7{oyA??QZ<_!4)2+rVtB(mObb z6biP*b@t0xDH+6?gt_s|ARh?1^59)^+1^Z34-}bg+-Vr>GaN`EgaC!heu9bEFX+|_ zSG%FYN8aCTDq0#Dg>Y!-LBbDntcM-Z0=E1$s&zajErEB^rN`-;c0QYql!xj`s5Puyj8zP`cXSx` zS8_&|m}$Shbo1Fws)t=#?D;uKCf&l$E(N5#r&0Xq*x~M4o4KN|mf-2oQq|nw{+tgHItM(Wk%)7wG05Z4py z9JUAM^j+?Czq1?s@!1V}vadPC2Bb)+=}w)mVtz~e&&5l`bcBXsHulZ_@;z7pwv`WI zm`qO7ct!dR$r9&5x~GbJpdiR2$<=>H1tuG>e<<%xQc;+2 zaqY&G0PQL+ zg_8UC{rvL*FDf7X{S^LO=&xT>yZ4W%_8-4Ne-m-LUi!`dXW?m9h$A8lJwv7_c zI_lZr|Fs_VG^4As&A6)c9$_r$MQ^fb-u7bu@xtt)?Wz(UMs3lmCDdI#HB#nIHmk~0 zR^t`+f!VyyN&SYy)rLu$$2;8&)sg$A0b8UKgsrRBP;jc%&lZVOT+h)cF3y4?Q=p5L6b3ws5IZ3CA;hbROsY*ZSCk^iE7|;s}3TC_G)FtRtAmOBR#Wa9?v^7y7Aki5}HGlPWMnA;G zzDEr2?r+|0=_ay~8+=p9>d*lZz~SB5d1Q~ z9?ONTk@ecqEpn;zt3anHvzg0(vvOtErz?>)TP%HVb9!J3P88kg7*3(Kej~BEN&Ag4 z%`I*L!-+zw*(R4-SdPa!W7i+js?}DQqe~MAiIhFOK~HYdftMm4^$J9sQVWtOMobD# zGrh*P8G{*V;5L94Zz7kz3xkfl8pZO-UCAbxdakoh*V%M!QHX!@HU zS400dxyti1nRrVe29OY%^|=Ya@K{~EV2PJF{{ZdqDUuh3-H62oX0!JA^D0Z6+r{V# z*~gLaf5-=`(c0tVzuyGJbF{z}S z4mKoE(Nelj!lsji^2lvozV9kJv$^H8!_K?!#0wAM*F-fX(*3Vr-)x_kT)1QGSAt_K z*Z7b~C@}sR;K=RadhaJso@_PjErXeGt0TM*b(%=d-Ogk(pb-rp$%2H-Q*$$L?lf}p zb+sxd5)fsJUcx|l029YteI9V1*sXbID*j^h1OIG@sy=ZlA@};~GNRf8S?hgxI0Yjk zHZHd%M!Xwe=yG2*162;lm5KdE5+e(iycg2S}?0VZ?vJs#KTQ;FU_# z)4NVp10HZ2SG%*U?@>s`>GVgjdfuICQ^;mWbLSr^GDCX@{#Dv2*7;^)cO0&F#4f8? z8c!zOws<-44>D*~wYdl3l zXcY$C`4U^uBmzj#~B^Z?lD;95$maMwI7aRTlzFHs)X#Z|XisUnX zxVoCdbX&Hzj&!neF;oqtn5(pwzQyQcf ze1wstS+$hmK@MPne2^SU416^NRfx7xNp4q`hLZ8&*s8tt@!(+hxW`nSoWeM=oy34C zx;Dm@@~p_rQ3<{z)m7*6I8*{Jj?3wSV~*|~2$b0NQ>7r#R1w>Cb$vACsnQ4g z38&z&j%fUu=Jxiv)yM(__FuB)8Ywaa5k?^SguM&C`chA7{PbyE5`m6N_2Uo-Nu@CB z9qcdQ;dy`I>J+Vd;d^-|KxJ-Tcj(oZ3hXz)5zWC}$ihwksAA1{Y6Gb?sK$PRumB!#obHmqX)adXk?^xhGu|((^_In3 z68NZpiDJ(NI4!rjNn|$0NOIM98(q%6PJVxE{D9_?!$nA5}?W(D{~H~fmM1ss^s~%F2=K35*a56lsdv7 z{L*Lc++EI$$W|O3wEf&e;rxG5EIY1A(bQ!j zI!{1H4WzXWkNf&7RW^l5a}CGD3YTwVU|r=2#iSOL4uuAzaf2rZ6gKnJNj4xIqks){ zW|Ua3FF}l5ZFMw$K0%ZGleBU+NSxr9OUkTFO)c2`c?F#2f}^Lg>-5wRG;_QBKijT! z2Y|UqHPF{N)4{61a#s($T+e}@HW+&J1)LJ+6vqGnr@OzB78(o`{ro0?rg~KnkU3@9^v%b83=Ho8&veGPI-zS^r+?c>4~e1gtc+l2mxkBbM!)@| zh-!u1_&q&G0~~Jq^VT!sYVE~hS_enRyv?7_lN>-;=pz1ncMG9L5f-42l%vhJS4enwHU~spY38^ z(%vD7Q-`#PumwGT=9)C$pCLC3CV~0UfTyVcDWI)BhA0nEgZ;#z7n^|pLUF^?O+fPB(|9H9_x*=4LJ$m%6-*^xq&j1e*oGO`T*d`AF zHFS}!?S;I>!5F6+@eu#uegcic^xT|apMrV^`#EGgu<=-0P&OXK)=Vih#e;G>wY2mM zpbRWy)^wiThflxj?XP&2gcGOq7$8u#z$hCeOkM!M(-oZ7x75np?=6fJt(uHK3;>j| z3&&$d=b%4!SeAj*E>F<-+e#vy^UkHGKQ6P|V{kSxJ6UhM{H}8uQI3v({kQ&l>+2{s zvrI~KqC;X0FW*4+rz$0yq_$V_@mwEXJc!XA6;4j>HySS1Mp+-x89y`W`+)`4ngwX_ zAYT*~DPuF6c%c!jhwv!ChMiHbunQmwzLTVyQZI{>nAn*BMu|6hTG^|+(JHx*Y{kIZ zh+xH5#>!q$3All(%jn)CbtIoH-hx4f1c3?NxK93c`5UUt-$q>aYhJ*m?5ru(!I8Pc zFDnMJ5Ym;r5i2OKUE7UcpsW67K=NEwGaIZ1QR=$w8qzy~JS~KcwCeQWGCJ&oabl1$ zjZRfU9Pe0ua4;z}vM@N&C>zFwDpt5%V?A5N=gV+YGcHsUi96gFm1=v~kv_BP3-8;! zh7%`Rbz-HcsA#&%{`1V#)HBfER*p|G9T!aK7#C=3w2h#-23)Tu1m^mKLt)R7tagSR)S}wt>AI zs<%Jh1Tet%M>O_GZ`SDdN}z^{zGQTEhQ_AayohT~t_N20y53%XI*ZxrN|<;-`VqM5 zu7-P6z_HXfHNgxg(K?F(5R38p&k>$Ape5fiV$MwmqG|kM+dhO7elg)q-fFfy* zt6f)9sm2mhrLF`xQBd_*Zl-k|^(3L~>jF7`#$Zq69(Uqfk#3K+OQEl$gELb~g#9@@ zHC4JYxl&@Y^k}hH*U4txs4!D?nCa;qrT6j~tcH{EoX%l^LjKpMa=dB)&KU(Te3I_E zL!$O{*jD{t>RSeXXlsCQMn6bSH`5dXy=NOAP+mS3dp?o}g6-27?r1=bZl~wy2gDrB z-`GEQ7g^t%6?VV+lRume2137Bpi?v`%mO1MS8V6^1h*<4UjI%kdv^9DX@%RnQ2;=0 zA?y&Drr@Ar_C$3X-*WAlArp+{HkTn7oXPH+dB2R4&-I4pTUYh#NriG7!z5RjE~;%+ z7DCX6(AqV6q}KQyzUWCr<=S6fk&%%>2>l}4|4;+yPRm^H*4Ne|_}1?L7m9EefY52~ z9^ro~An-+3M~A_zSLY(YOJh6n)0?O2e1k^_6CmJ%D0Vrv>skK(XmEQ^$jN(BiMKnEb5GSn$x|C68lnX$#RzkgGsL&nt29$6#a!zhzR=wSa&CAx%+7 zXY1Vsi8{1TXSHC(jFtQh#c!viB=`Y+!ThD?cF6no16NfpJAKF+aj2*q<)V<9mUhn7 z*R`{g(KfISwndkF1F<%ttFQUDfkA&lu%;4tAc6gJ)gL<>**d#k0M?oglsY zaBa9G5WF`?pJYLC1yOkCyxrU{Oi2aHcE$S*iF8lkvJ!fDLvEe0w)Wdq_|5@l|PM4)D8?&qcoh;en^7J~f7haKa zR_TUPr3q)v+DWKyI2}aGlJ}YSF~EzMuRqC09zZB|Y^58&WUh2``^?JH@)xwZmL7d1 zScP2VzwoXr=i&`DH`qY2;3;`JD!u&0W5zwu(H{y3;?_!c9CQIQ8@4X)t&aNwdIPE1 zW#<>A)1k|H-mamZSq!$nkjF?b24$sjA_;B5A)Ia31YQ2Nq z@o4#-);Qeye=L+vTY*0lDE7Zp;hh@N#~k(-SCg8gzFF>u{cY^R#Kx8a6_oQ>U;YdM z-^QH}@l2f;Odf!OfhTpKXuS=d`M&r#Gch_)sVt{RRfV2Xv&vftJ= z)btk0kSn~5QxH9n(u~D-RH`w=1m%>2lXHRih{Q2D)MfP{%3VUOnIXo^TWE|feDOTq zlYtk3M;~L@OLyA>)8|_^D*k8vjC1{O{jB(1%?;Tu;^y)zF(EO20(9oW*bo1^KaZaqIg-R?>I3ghYHAa8zr zdbbhU&|ZW+B?t2i7HA;d&nWXh=JRXaDRMCb3haP`0|-6 zD$p>z;3oo2`t!$+_(E+or$-XOQm3IF=uHl0PJuR5+YS?(6x>WCNwM#?VxEmSU z!;;g%k=k}C@>Cz<9V(EOnuKjFzxu-HKXCY-)hi3ZpNe~q-0L1ot` zvf*OFh(BdF4_TS4V27$tr*45(wZbj72%jz2ag_~uZ#|fF4f=o(b`-nKQT@U(kn`81KW-DO@SR`H_c?1-tL$i-7Jp6K6Nkzl9S{PQH&SdUaSI*2F4fle z;n3)40jo=e$g#9cc4472cr>inPi&!%X^8L+1}CpM3;MxqSle%h+X7It0fdV2Z*}FH z0LflPw(pgMgg{15V&4ktBrGS5k>hLk{wQlZZq3OIuKy`Ee)0Vu#KwER|D)K5*7aY+ zMqcO3z2)&>XjeL(%$o&OGa#1Z7gTkAEAC{H@thuA=Vehv%Cyjt!pS8k&I`nGcvClt z`F%^Spso~2(#K3elL2R)TVIK%)Jx3bk|cRl-F<4qCv6WbNAi*@D-p^)dXwLLeG9(` z$f-a%Z}t1(LR^+XkUa2)I|9`0!0ku}nWP68oShAZYWJnm3J&gIFb=*KfFsTW7jtSz zOKpyQq$8ja??tVAP-LWtjX0`G@u$9W-8*$oWP^fagYwBCb7c&aE!JRYt+l_X1vw6a zw{JLY*Zw7sEW8<@YP3SwDEU+T@vhIT&DDobw*Q3vvNDiuTB!D4g^<}~{Ht7P+9=Qv z%w$*l1d{63_V#p>BaF<{xVZ%J^9!5w(=A899S<7IzM6uhW-?lR4L|zvoBgcXL`Unijm2=|h|jdgmL#k{PUjUb zawSwsdNW%x6vy48d7`0!hOPyDgj~KdDP`#v0(P5DP}ac7(AnAfCPU`a%iYE4ou8Xq zmThuF+mzDPs!Cur;cz%&fS#Cn#mS7zXLN$X__P_5M{-DR5u6@EX!sE>7cg=f2PWcx z2K72kx5XFwFd*${0!Ou-Bm4Q?$$kWj*;92F-dH`;5kyS1OybuZZ}dN7(5FKJ6rv9j zA0j5$3+r^0je^rLs&XV^wB*v&xYL+ zIF4x7d>=C5v^bj^80HOXvQ=&VRYz@GltNmBbY3M z>yP{hp;T=rauYftUn2VtRoN4Rdtfq<2qE?vj=d)G`lG^a&-^8aIS47Ls34j+5oTxL z`r;M2fFF#g*%GckGGPKi1hfJKyX&Pd0a9-zzY7k6JwEYQo8L85-~M$nEs_h{uiN*XHn;${VNNG`zrPd>EG??e6hup^I7f%M1Ow$-_OI!z)p6zhUx16;Syi(h%YTQd<;;FthTohL~>mzJ+EWMcyUv= zb>3Q|OW2VLb7|E>9QltIV5_~AaymzMQ`j+G`PE^2E4-?)wU=vBd=jxuh5I^I3Y!!u zJ%(}w1ot(Los)gU0$TIK7qj^O+}6e0k+onRb%G*lx8+q}(%z#h2r5>k%0G|iGAqoQ z`dIVXbBlwtg8m7Uv-#5&WK1OGBA=3#irycU%zW8KwX=RE{NX>lPG?@_k@yGY=l3S1 z&}_0>cICPZOej7i@{_Bi4Vg=N;xB|&WgIz3mXqpPA&tC1?4`XMc|D`+!9o9yrZqbC zLyJcw!}S}!-r|$E0zrJHuIlMCF9M_Jt~(gq^kute^(0QNX@+)_X1KVl|IE!Txl{HcHHbjiO zhKta?a#Ia&9vI-KPtqsdO2qJsDGROr;E@+o6&pm-F{>5q#}e<%W-k)2ThDAu@qJAj?YwOHuxsJ7S(<8CHK;2 zg`N|m_1qG&i<8n>m{1HN-cQe8Wwyy$sNGs1Qd;2Gwnr7a@G59+GWg)khcT-+c!Z>(YzI=Vu)D6p`j7LMmad zLt`FBpKSMHP9F)J=#QgF4n!O5#U789IBCS`%LmAGskr`G+GiDN5lFedk}u~!a3Ql; zHBG*iP_$`$r&LpJW|O{B`_W%o)$!XRnL2HJ$zi?52V(msq%C<@g^>_@y`i0(*}UDd zEP-K$Igy(il4$Z&B1zvfGfTODa5!^+It=$zp_LjKEtWv6CJDp`X6UJfcRM?+JhYuF zV2gwIVsTV{oah~$6h&O_T(GG1c+$+j@g<77%~qm0p~Au%vw3i1atzKudn|md-Nk*K z9}jvN6N^y3ZVTDHeRVy=bac$5ZTPz0y8lbGx!vBa=gjEjw%ZH)>O@h>~uWNRC~1S`oaZ~FrO4!j=be`>O{3m>EjRl z*7v#z2!{WlD_#2il~{Vyr(Ntb$Ni-t zn6umtd1qZ5}5?`Zdf(YK7T^C-&!Nct=xE_*tR&#!5$1jgZBB z_3;>ftU>#iKzX!Ju7#D8^|2zcRm(M7)BNQVT@?pdvADhUvGnXOt!FlMMCbdNva7J2 zNF#}uk=d;3>O>?mYp2$QX?ZB|mwqm>ejW2;cwMhhS9+l4XOvYuvMCzU4hC!a;l0Sz z{lfDW?7U9V&ZL%;#Z}M5{|rC8^x{Yk$J}m$6j6!?`aD>>+*9*GmIo_Kf~4x^-ZN8j zyEpQjdywy{iX`ex6dIDXUFX_bRBusr)u?`EH(wo>rYSFg|BtdT~A)Kb-y1pDpjVdN5mVY7x~`Kfgf<}lhE3S2s`!$U78n@=jFc>q4@id zNQ@b(w%$jlU@aj6+y1%LE1f@_MUB z`}#^wzlG-hwnIkb6B#Pb&Cy=BAT)A$LaA>mtk@5U+Fb=b4Osn|dcMX2N){(>1IV%&+pYcG(~Y zVN3AQqxqsYWZ@Y@g&rm8i_kkOLk?WO zm@w;GqJ=I>7=5%HGu;FB)0RaSe{>#|Me5HFTX4Of;_p4ti!a>E6<>F@5PUapozq!v zJ{%7_;_3pMbV(X%b^!H7X;}N*dQaFvsPaJY$$Qb0VEAv0aGl3Y6U}YOFSjLqXBV$!^qr+;^s|&3zL1--(5`=!Ptt7)^TOG&6J+4;% zfi<8clt(Rr2#yXM8QWIU%w-wuWM@T!_0jO8-m3?W99&Zq;S=pOV5OEJsyMKs>3!BD zYIwyHK9%5{#`{psE>&7Y7(+(m^tjOXE_M6!nx@oo9#@*kyP>hPS47E14{9={2o>Q%3pq@(yp6 z?(Nc-<|lf}@}svq_i{EHoBQt*)O>wW=5p6%bodTRmXG6U=z1W;AeVtgOeuXVWq+SC z*LGd3j->evNi`F`;GaLVw|z!Do8N{IV2YFLHu=Ylz?TwvTiJ4IV5H$}q|g(I=}xgb zD8PG~^Q0%G(%KVgt01jQ%uU&-YOc9F7}ukOK1%CC`D~13ttRN$YiX_XRJ4nGdA%m4 zxdLsR@~c8yLj{{;1<6N34(>F>UkeEbCa3#KWH?;gilx7C>WU6tVj8-g&}vV@F`8pP z&tm$eO?i&>#2&odrL1|seImdsA+*PzIBJ*2jx@<(`TxE z{ejP^apdn|e2@Dt?ndj2MpW`fS=-CtOpJNR_w}X7N{ZN%`cELi$1v&&v+%F^{OVKc z7~|us?EC4zX&>pHLAi`Vy)3nY)}5Hi3@JKcaSX+w;)l{%3i0@l)Q;kB-#ZTyzjx*g zS|-bJ?)nKT5HBsiZ3|t{jpej$JCVMQwX<4!KReaFTVTi0=qTYc`r996`Ac7RCTWM) z`veq%OYu#=K`KgF7e6H&u1;jOPi&VhrfKR)Tb_WxwbEB+$8O?RiulA!3W}ls4 z0o6EVDq1QALqFwQ#^D+{kOM(*h)^LyOG{WsL#v<+nKRc)xF zQkU) zuTgpavdz`cIg#`{aYx*d`>7ZP>OD%9NJk^rc+au<=7PhB;Fo{4L{nXNGJ>kXLrOkM z_;o3@3I1byX)AGtC~>wTe-DM=W!A$hhuWb<;lDSt2X| z6DEX4+s4IwDxIrwS|%t|vai$2w)$00bR|irYDCT^#ZifZfMvn7eEQ7`PpaVcJhkFE zZFI6TLWU~|nvwjc>=nD!y}Oc5;hW2hI0@ftJg&cMsHts|^SJGB&v;bgQRF_ZRial; z*cJb=G7I)kHFfjcTTgh6%VoyY+rpz0Gs9}$PWlJCQ&=B4u zUB^^}{d2Ww+(2z0k{z38P9`Ew%aP?QCm4IXg&gX$3=RZLVKOS(zZqzZzIRlXzAWF- z00+qjxrBU=ETecRrO)a+xl^aXa^Fd+t8>>|{`t!+Jjn_1yv)n>SJytd7Quaqzb^mz zuI0M4w-DV#KW3!H+;$#WvB~IIngAKLawvPCcwgG|RHzQQsc$jQU-JKkqILwV#(B-(LO)EQ=syEakj zvZ$LiK_6SL(}&w=wGC&Q>$PO^4Ts*yJ=J<#aM+kAmvM9NO($YcbT_A+@FZ$3!;J1;NK zmm}pW-CQoNHZBY&VtZ+PUvF9ShsLsH653?7iZ-hF_*~^^0gP zB~{zQL87H?JGrF3G!wk_m|zzoDa>e{IYbHGqnjrpG~qlvZuC+4&4(5yYSw;#3*Kro zyx-JbQcPbXv|vYf6kB@}RO?kU4E%^;7IDI~47_?*w8Ye`2sVxNVS`M~M^f{CYweRx zoQ-;W=~Z3w^B7lgzaM3)qfT@ zIZ<^i>XpVx$tzE)h72>iggW$k<*!llZtAF6bhcs581i_rPVRkadK)~=E1cE3QW5et z{(1Uu7e8_@FLuSaHfp9`!#94WnwIAl_6R-0S)<%`V?whW4Y7PW=T&LmORWZOo@9TJ=(N+A% z-Z)c3GKl(K`bq}Me1iRY)20twVALz!T7psusSIgNaeFMT3Kmf~l`?H0j)i(zrzlfU zjrLF*75782S%8txiJ_%^YeXI*(4_ANkxGbhf8C+P$&`OS+Z#Yo9d@AFD9aw4{$=vl z$_Hspbz03HWIys(Qk&$eehX!}G$Hp~YZLHz}2jWWO= zKxI=QVUGz9q15FkbU=P2B}smj`%9M!8>?6)XkRNRhc}Ch{V531nfnrgn6Wof?%mfv zZJ5q%-La9Eg+dn~TB209K-(lXeqx%MVz}XJii(q=ap=)-*Eoza}Xl2e^zj_`1a5O{i)Rp536tPfIo+q6K)b)%M!G7?N}8V*n51@wq>x^hKXixJ8$|k% z&ZvE_hnVAs`+tPRJjUF(f+))m-PEoN{2qS9Yu(gRzfV!;BZ>;4x6p&8uK08!wdUx+gjrZ^Kk6puLEr{ zMt52~+kq~^hcoe8yVZ)V*SUajK}o(wkaipt0PuptikGVG{!!o|?#J`0(Qf}4$3j0f zeD7a3;aiWz{x0Q=I@MU*Tv=w><#S!&Ej+t%dB4d6=NkNZt`;+n#*Ml5*|*f=Jc`iU z91z$90PvuiZ~W@G$*VwgBiaMKsVCD9dV9DNg`gQIXj7=OVG_sv_T^V%%|#bPogw z>JvTN7rO``eaG3bx7Dpsb(wG6ozr`Z;~H`C)|C851U;fsmBSLhw(_S%c&29dQ{+`A zHGXw9r!~6$*t(9cM`ufeHxJY$XHqA%M1I#f`;S8kCE=N9Hc#+w9}Z_L+S-d{6r{k5 zJoI1P0{*Os-_|T%**r_1KBa9NWrwu z)ngu3mx&2XdkR+U+C9{}w-(LH4Moj`mT=2iViF{HftmSq%82|TpuPZB=02|lnYVed zAe1LG@x?n+cHiIUu)Vb2G8%cMg}|mWnwjsR#$eu-qra$(YbPEIih#Ehs9aj$Wf8<2Z5u?o>G;G6l#aw|^d&F^O_Q!%|=GPl2vqKitt5hb;veY>P7$#IR8S^XW ze($l!pd#agii~$u$Tz>UkaAVrCEptX^9Tq!(^aSjH5zv)=8^6hU_gSo~b*kbj#!*+D<;}4ZCg*X8E0vg+;GiB7(gd}CU`ABY=KS+4rpafeORtsr{sj|6snQF0DbuaHFE{|3U*F#+ z5i!^5-@XpCOw6>Ws7fYh+)w;{)n&Ohtap6R(MxTlavb9EByy*F3f{R7VFhc}7xud3 zX9WJO9(i5fm2*+eTs}+JMjcYxo6Pq8OHdPV8nl`(!lSc^JAWkbdVInL53A6FyBm0x>Yw7gZt3)Gx z}~b#O6f?n&U~$6bwL3Jzh777 z2CRItJ##y*Iedlp=kvBvyoT6M9@lWf1qdoNXc z3kVJ2#a%|GDyCKk+#HE7E`ObeqG#_;d2(Kg`sN|1V`R&NHQp|5+AW8cN7`=l4I|jQ~QV z+a}<2U3kj6%09PNX5E$P=bMlKhi$&&40y<<+V7Fn!X#ctp^|MO(L%u!%O);pU2Eekb`@^>US--0hSQ4w|ipHZwg5 zyxo>9O%AArTsK5U*{bU61-a}uy?+^Kw3Qcy%_DofKlXiC0Mk)6JU`vXx%ZR%f6?|8 zP*tznz8GMks0c`_2uP5^`ckZ#x_Agy#Sm6q=Al3dc=-MQ#^b9vrA_uMzm zeec~j#v5agJ=_Dg!uo&z`OP_hWmfUJj4LBUC@Wv6q%$i`K2Kv)Yi;B@)jcz{!=0tq zMa;1#62Y+)&W9FY=mrLZadC07Ki|d`9irp4Jhq6t1UlN={ZGnlhssTTpNFvMHzgJ?x0#+e1iv_*htGnRn4(9cFe~_->W*Nm?DpXF3SXn0`L$0u{ zq=U85_L$qb{e3&c5rOkL!BsQ=bd0@AU7OO%;1MafW^Qh5eA7H}aN4PP8;z=iv>i_4 zZf22_f*6M(%QV5Ik$6e4W$5BpNCS=L7Z zsC2SK_=Nv{i}r0RSkFxWmo8`a2aJLWV-y21SI{csZZNmJyDrWg9fijU{R z?>!4+(yyJ@1}gNOqg_gk6(al%G&)ftBg$tvSY>sfAUdlmx^3YrRf=3Z9~g$}ABU?? z&d$DsECPaLsh{F$VYp87CZ=QM4L)*d3wyi#Z4rDPAbe6uBxX1G);ljtFBi~fHhOqn zHdFB{_=K?9O3R(O!pRU-QO z72d(>`FnfA()GmB=k} zY9FFY)!lVe&xJrLVceg^VLxQ$>9TXn_oK^!zQQrDe%3gNQsn;XL!f!JCU~pE!tY$B zYFS50&h-AVt2(>klmb^T`q;zwE-p^A&~|>@?#SVTKBd=)2Da$xGsLR>e_U>^DA>TR zZ5R;3@d2Fkj2@DmXMH#Q@3^*cEovgk>`!Lyh~UR%aVN2MUR)6ms6p z;4Z8BIGD~TX&Cz@{jougWYJ6lp*{5D17 z=~V*8SEGCd#HY=dX~bh&miSuJyJoK8wiT`oh7vd|vVRX}$kYr zZm7!z7;7Lo(QWQZEbq-S^K`4INElcD>)bh9Quc^*&&THbR)_EWLLz}dNh2BEv8!6< z?HvH|0Zy1R4&Hov%~arpVg?R{M;)pWe!@eArXI~YFA_!5wi|xGEk!$wZt`DutDk`TpCnRE&_wNQ+4bb*1us(ZxpQ}cpSh}~R4xP5mhsw^ePl-QRXNdun#PZ`HIYFfy^HDU4b8*qf}2zV8myQ}np8wZZrr8X|11I0X0 z9h&_NWtbN8Ntm_?nb}>rgM(7ki*6tTPjU72hh6Ff4UucZu5LAyxY`FnFGJ{+y0#)g z8n`K1w1z1sIOt{F=)W7i%bUeSbNy3u*2anzPIR&S)-CMfdzq>RBPsVa;wnwnE1i#b zk6ecGjdWjk=h|C>J!O5#HWBMx+)p4fqI5ld?VBXN8b`w*)hYEtSeVUjz&xtRu*taM z2;w)0!44_O`cVo|o^w4s%M~cPp+6J_Q(kn4#y-U3f5h6_nob;mGoP335tst%{ zAtl_EJ@yqM3GA5%kI)r_)#o|!=oy~C`Iif`tpau%ZaC;A zy+aq9@$Q&>B>eg|1x1aA@1T0Ii~L7IMxCR~(PMT;nj@R_1jnLGKQfL0e%m$WE=G9f z!vV2vr<#1jTqfT%YCf)v?v=?2fg8%i#;G&eSQUz%HF|6aNQBU^H2RU%oR0Dna~NH; zoNw1(D?jjAcAQ<3bEw*+dG6`_3skm?CymiNF4N*S$c1XXkO3NlSye*Y>$xC94*l|_ z8s31V{tWGca*GBZr#%yHkghS#_Zi#9~aRmz10fz zHC4x5>HJr>JP^pzTWaP@jk|9^^0z+ z$1YEte zWzNc35+>&_oCgh)d+)q_hN_5$1{RuvoFOFtpXPoKDu`&XVn!_sx}rIC=?h3WwIGKF zipqeItUP2$Ed0)75bjt`6tx?fR`R4jsrP zo$&;J!@fq472oBwHd4~sC?5U6!a@ta3SNZVm*Hb7dAiML7-BlCglPY6G}{u^Sss|C zUc?Mi>L~~rfue`0*rnG=y?uOgQ-w;ToSXz+zkUq@%3R%H8~Waq26*5bySoo~c_Rk~ zcwQQI!T)`OAsnYWL5k|7^<`Yv!0gLqwy50qy-@`{DgD(;W*sa~iG_|5a&d7kL>HPS zniy|4p!%3pa<%)JZ{y>eWCjMIl|kTzCgD!hJMk><%HW4P2F0qIE7;c>=CpF3bLTer z7y>z77juSXREF^)Sb~f=NuIH&=C<fL2a08^TtMffK-Ciepr&DG(;?-HxZ zJUl#aGeim076<-#wM1+o|0Qt~~Tne*7~Jewk&x7!>b( z|I@yi-Cc@+W}OF;aROY<%z4xL5*TtUIM^h;i)6n;ugKbTFeCiuEO$~N8v2{O6iTg= zwZl!tj%{w!O2p=h=J7oIw5D1nT_Q~lhYL(Uf7k}DfxfII2#U+Zw_Ys84#&vYBLP8y z1u%xgf^-dx`<|eq>l=nQHM~;IB?N6$Ju=BEyPKO}p-3+u|0A?d$s5{^w~6xPt>d}s z3d$8(2_25Rr@~;C^+-LFOhQ5e*RBk~9O<`YZEZbyvWJ6d(7*^GMKz6Zp5)4^Zk20LKtDgzHOt^6U~z|;nHkq%j~p}F%$QXcI;f@_NMS<4 zf)d{>o(UrwmQx>szFji3CG_rwjcc_4dK~PJluD-*#um@QrgA&t5Gbe!O1H{+S~)3k zT>?{59$`iu`S|Z(eg;10ezS|rZ3AgSEcBtG9?4#pbX!9jhr6!@(^;387-Ef#j67PK zODbOZRDThRb1})=7+fMQj4zsnb_};N)afH-`zlW^efIUw6Ehx$Dl1F>7{b7kOqTmF zuXb*mwQ22_rx!YxuaA^EN0)5RbrzlRw?%UdTWt10C&7t?TSzKi?LwL^K}#r=LB|T? zcDtC4&cn4eQxNJz3+?eFuF^ivC<^b#Pj+@yhTp9XLYuq(!tkn~m-{q#KHm!S&HXAR zb2=3f8%t9So$1^iCI&Ua!>uMh*Ul+AqpW}w{CsZ+ni?}xQ6flbPS$7+Ugb-(uIb3{4VsNQGqoY;AbsUWMk6k z!o@CvCeo9{R9h&4b8RtC2Pz!Nj;C3xR*z!2$6UfYvaGarK1-g1zwx@>DW_K?qCMv@ z-QE2H=JHE*xkmU1khSw$a|Kb>4RQ1JO=hCz78VYA&c1LbZJC~NGiA+pL`yCtwsOsW zpj}~Z?-mTGA$!Z@O^1GQ0zplH^8-~VR_1>4PEmVDg9h0y`Ah}0SA?tx+f}rh zV1$r%61BN;7H8ojwR8xTyY-Nkc9`@0{W7QWiV7%gg`zUWZ|_qGIi>7tVZwQ~ubgSO z{kl~QR^^uT+}zwW;>SMF;(>1%OtRWTcpR5saaTkOE^R-+$jF+Ty*D;4gAdiy*rB<3 zq{&D*F_=2OQ&k0( z#U*jE^yMk3yAa_;Z)?tnFvxg%wnybTuB(CH$&CU1ijX;(>?h}7jEL=Jr)`Q6pNu3q zZo5Kl!q{qo)O?W(IVSK{sqUWclIFR@wKmmxeWeXV&YnQ~{9Ic4fu%m`A&mBZ!c=lb zN|OFQ%^{ON-i!LgiBW{cBsL+C%*IxHu~XWpau45uN^RPHvyq7m+e=t;k=J3A9;~os z(H8Ol@nTa8wF85+-G_sNgYO`|EVD(_Il^5bk8(q;?Wf8b2^ke!h2*PNOQn|`I(sTq z{Prg&i#?U(u|q}L?WJhH=naJ0`k;HQ$5qfsAMDgk?yQ^+$@I%)I^t_Bi06Mo`z30`ZbwJ!;gN%nN@{AFZF4*A71A) z8Hs?h-B~R4{MgCaq~)JflvgbMFNPtPHDkzD=d|{vd@FhWjfLwtt;SFs*hY=9D#kRd zzt^c5=58IFRqFJ|3Cg!Jzr$@nNXFsTj1LSbf=K0=n3!0=R;ujx^YB9Emj1Rp$*Q{i zD_UW*A(lfQWdU8Xuph=SL6$<~Tf{FU(U@VNJ9#SJGCMMjXqe+G1 zze4vdRi=0SjoNMs%J&tB&LBq#3||Zjj(>G(%l=e62P(!#;trBVq>)M$rsUuVUDMRW ze^ejX{4@Pm$|Fpk=YSLyRKJ0(y{gUW{pnxGkuv1wd0QKFj&+5!c6f6$U15gojg}Ta zctcshNd!%F*Wid_6;3+1F4Sxce0WKRxljY6!5PL*mz{4&4UQ_EY&KTb-qP#dme75j z9)ng(T6n>dC6JhUe{rV$T6JP8vB>5OdT)MRr6_&L8GZOTdI_H4Bk7cz;aTr;lwF|n zgRv}y=klpAvi|@iDA2q%P1!$Xj3avS*YSapfGdUXJT}O0Hpx~o>rVH036WVJ0*;;W z)6x&_jZDf)f|R?jnt@a>uRXnRsl;jaDyR>C7|$zMhWv9>_`2j#ay1sTL{3iUH#e``-NEt0iJvX# zGyrKtXQ|wqkA%!xa7I4Cbww!>uwnUIfP~w~pk!jKTuJ6xSk{D=!6SIaP}Os+4z4OE zMdP%=$%5Pjuq%8`#^)4qv`3i@ei$FandZ@Pg6s(=C)H2k@@mekaI_3T8LO6=gDEsj zk`hpYF(TrIn)_DEL0q#cHp7MQ26Nw!W(UmH_%gYksI7}rAP`}fSPI{jUj@4@6s=P; z<~y30d=3+_Fw9x-G!12(Ez*Pp9dnIVmHX1it7FS-l}hxkgKy;zvvoDD#MkJA!gF)_ zwPSFVQMppgPJcyyIIe#!M8?JWJZji>O=tfN;$$tjWvE=20Ty;#87f~{SbuNz|FpM+ zwZcHLy{|Z0Dp&2w6>rQ30)IO1how7IO#yY?b#+UFJ(6W=g>%3}dJ|Hu!~TJdb9;VM zZ%+_=cnI$aOewI#5DjP4+(S3F&1(x=F7KLvKw7b}w$G^;N0#=Ak ztR()>=*K`+P$G{H;jx`OuS+4AHmJe+B_t-~eu~y|<;`cuwwP9=BaR^x;dCm@f#Lq zvGdJrJq}|w0B<|ApCyoowqD5{u}(R>?owF^HcQ!NXXk3yMyGco^G5dJq(*)I^@`Ao zks9^`nDYYeSjq(kruOR1n3hhCj@r8L618Ci8Hpp+^8Mrp}ex`4BLET94+= zXu+91%7?WhT!I^D+h95vDIcdP6gj9H3Jv!DT>$G{o33$STLlW86kDKw_U7{WtKm#| z(FGe0d=q+(bEA5&**kUo?1|!lpp%o6!LbDu4xU~AR%D=;vm3;q)**;%AU|$%b2mwM zTQ5w%YluT!UfAjQR%T|VA35T$jn9|2uyNRRem`-j>}&rs34g@C%6jCreqcIe6?Rg3 z`%J)op}UL-4y;LcVf*sB8M+b*p^^WPkv(|sme?;7#y9#=!6RkYZQ0mPr@SJ@jh!EK zI3s@@Y?InjZna&HYWkE#6NFF~Y78V6hrN}FXnao+lgc2N^)oYf$UahtE6H<7ut*fe z0A|O?`L~G{iGWH$n2B%{mkV)IlW-=O^eA%~H-d=LE*`b)vm9UF*dC3ERiF>Os;;gM z2G^*O!Mu&_H_?RKhW)K$2alwF*s&cSAMc-3pM%Y9?4&a_W(+}{o?HUqVUlcfkd111 z`u@Qo`&!;t+%uhvn0yA%o%tQTzoYKV@12w}ES>Q7z7XjqU0Pq}tQV#J`9B7Y3)_6Q zYh$w&-{LtyVtEP%PPU<2qu;GFO6~Iy7ynKNmnS#=}90P0}&@L zs!rD@Cf6j=2;1#lHyo8*X!77ZvtZ7%A4IY~wba|5UP12L^*nk&6~6(`SkHe+>@Ig7 zyQO67T>F*4&of7q`kRBroBMW--AQt-xPjrRQW+$0nZt%yI5;TiqidW1F1f(xXV}*g z5>wUuFX2)AE;n%$6Q&X;sS>Laab|zg`LD;>rRYorvr&q3KAc=w_yzUuZIOh@)YO#a zJJbG-tgY@5b;WGED_E`Jj6dKr_Ss8S&PamwRNDddXFRV;q~@K%{{q63%^02d1K}Mf zx2xwzysWwZUb&191KT4i|Fp=Qp};_9pXWcpQ^2Qn zsu{80=5<^f;b54D3nXv;xJ^Obuc@~e9U1xubKfk7FeaFFh}&iiyHmS2bWgcANrjFz z9$6dp9~Pz2{0-X;_>28JGfbYye-nXbLHys)qCLd=kDSoI5y6sA|G*;u{_p>Z5Ptdp zh7f+e=EQEq2_RkUqGPC0&jW+gkFLeSn7lTKC`whTy04Fl1sv}k?)5~y3Y=s6odFI8 z@G7#%Bg#P>a9UGvoL1gCA;pHi(`MOw2z$wGBm{*(SF|I7ya{jFRl?E9@@ zmV0ZePy`Gh8uP-~TA;ECd6R^w=nsV|?#q`iYrx4N-a4@EHG=4yw%9UFXp`Q++IF!o zF>lt{y{7v3rLR(+sUS>kmvJvoK;~5f#AI+KHv6b$>G#DTq5NFKKu2Jb0n%2?H3Y%T zcgzCe=sIy*D|^m9A1AGkJ~>oYQ~%#$eeG()Grj!|=Cez^xYo?j7#X{&X4;LazkvPG z>$$o=B(l+BrB#r#=Iyvp>*>phf@HHV+I& zyX#|y+oeo5lv)z#WbD(_)yG`TzO? zxI%?w{nXUEp7`B6G1?Gajv+=3o#>{a0<0 zDn^?GgO6AoSL~*?hzu;4z#=XU8X0lQdXH?XDuK2Uut(mY3heDnBMj zZf;5EMdU$Lhn@?lmd_lQWdOOgJt0b~(g7`Km( z<~GaG>ZL!dIJv=*B=H^YPkBR6ToMCxNsP;0U&ijbr=C- zz{#!9G^fp31u#_Fp4~^S0xx%q?p?~wJbfp#SkD-fxxjkt=A#Z-sA zm8Tn9vlm%ZI^Yuc8h?0I6OZ3!Rj+=54JJ?$bwBdflcT)@qppW*qg5zdb;; z`OoRXuZpdQvJ}a=$#9jBvAV#xpZKv28S-rX4eSzHl1k&M%x}5ZXmsnx^?_hgy`GPO zH!x`|f3w`~U%3R^wHMvaD(!>056dNnI{daiuuBZ<{PZyx-``d0G@48YcV|b zKA(U6!aV?gJmGK!zRpx_F@b<`Atxf2d7B`i-Ngf+uJ%Qf=eg2iO3`NZ= zI`prDLoJDz-=^tZ_aS1ydD48lAyuw|kLcNRlOY-@sZQuFC#I%ibff+)+-JHwL=zau zH4zVA-VI^zss$>J~u!m>aUKC$=X1pXmM@DLdShnoRw-P#MR?++565 z{^(uGms#|n$=@x)&p0kshOFJI^+80)*sH@qoKjULLi`# zieB8tEV^m%aZfew7OjuuwA%6(y|rkwj}kftgWg5h3lp?NizLXWzVp5^-Dt7cLXc(! z--TkS6ZRgZz&+M`ZW81}-9@%6t!&1%^;wu(t6HGp!_W5RKE>VnUu@dvETi|G&s?;$ zv{MzzxTp{Z16`H(j$FP4ozB2lKfso&X*e2CZ+ysfME{h#i?H!a^ruh703>;EpKa}| z4;DIK0xT~xR-KxQIRIX5cI_eAH+&hwt*}#S8*fi|2w+d$d_1CVkau}pEz-VAXcSCX zF^J?AweKUj+=N*U2t}U`H64v)v%U)Le=FBNU16s_u(V}0+;_)O7EVtpJ?+1~g`J?v zIZ$qTJuRFi5q1=jbDHdtJeF%@p$u}l%4)Dn?}$YNb{bGbM*Wa%Hs7;?v&S2~_OUF8 zQwj2{c`x0dn1ag@hETv1<^mCC2`^=a&ujiWj4x7G5DNe-F|<){#BLq#P}Pt}zr&uU zl=)}5sFlL#E_8u)G!#1w1xAQs_pjcDl(Sp_WCkmZxvSdZnkz^77soOf00*7!h%F1_ zgvT;as7;&a8qrZGPWnhO8{(!)@8Rbz5 zK$pmY(ItfGRfsfQ1@h+KJe^nT9#^msrjt|D@5-&bxsT8}jNQ5%vvU7?91ROzhgP`A zFrg?k!QS}boM~Do{Nu+DMteFa$-sxeq+mPjEWYU-vR)j@Bg#V7&5h`~<Pn-q;2X8Zz81=H_UBORzibcgHAVwcP5(xi9O=oFTf`0 zhxk!|*qJ6yGNk{#!oPXvMe+^u6vgl7AqLUuZSF%=ep@YY`9I`@zeaI}_0sP>bl1}Q zY))QXSpgB2{r1dvfCW~DT_@y}uKtnQeL4Q*&|=kkDxVZY9xa@A)Z_g!yn`KXmT5R^4 z@a|u1&cv&jNV{?s#~e5(0Ft=PoJ~WdohZMV*-7_*!U~5RVHb5ZR-&mEJa-B1w5<>$ z-U9Lxy!|#+b|xn;-YOL@8x>3x%dBg^RfWVOZkuijH0*EoBAc7T{Ji3Vr1(Kg=iP%i z7dKTZKmI1?zw&MkQ#oO`MS@|P3=KtrWgA^)Q`>qK$G#ds{+QRAfGizJsFEQsld6Rc z23HpzN}dpXn8v9wc1eVAR)X-*-m2H|vZj4h;F#J*jSZHWo%w?qUwz;2Rf7U$@WHY+ z>eV@3=sX<1x4Wr5$MUW|GH>2)_c-#jA_}ILLxHuIFDKXL^uCzg$LnU98Y@Q=L6gCu zD4%X{Oa5Z|v-nO8Q`1bItT*8-92WKFSRJZ7l7P1Qb?NNb(&o}qAE_D+XBQMCMr346 zDU(kp%GJ1i66xe~Rz^8mi<@WxC&N51u@W|%|yY&z0JeT`Y{Y7;f%^df-fUy!h z)tr((6G6&u_4`SW(eHNT6;`R@EyYG`j&zFDpWg2Iv11i1Gv?!zsu2#fasRt?OzN6+FS#C#oi*s>W~8m_|0LNj+HiGo^i3AcTFK7 zAt8K@!AXbKU=?V<0C{6L9B*}W5gX{}+v(})V}l?1@}#H^_ZBnGDtlasClxZILYhWX z9PN@E#Gh=r8C|iWqx}+om5BNM2IZ5#<=srfZ!)uoSDHOg=k_}$Zut&l* zW&G9hFJi073KA>{F)^Ky1UfLvJ9n1&=8;l)@#|p~6%}n=-Iro*RB-7%YbR-5|29NI zBH9tdo0F4sE{Np|D&mw0dR5rlYCzIY%)vGD*N?>`;oK<|YEhpYI*9`5>aQ8wD(mI0 zNXYYx;>3yxgK_I7{`O~}-a>3#P7%ttqN3ty)H4d8b?yI7@Woa-i6#@UiU8oEyLj>l z;h#zeo_6)$PEQ71C$M~7)LMSss%s8My{>ZUDZcXXVYx=?`m=X2v7JT*vOi6E#KFJ2 z6Z>bv!^ei@&p1@G%#ifP@b?fDT{|OS7%VvAHXSK#NItoQ)hro1JUFYjetLp2tejoC~J9X8Fj@aR_GXmyg`2z?l@_1Mfm852}(>gUhJy>o% zOnS+!+egH^XMGlJZW}L_D&1qe(mZx@WVp0=MMoUa$mzAY#&RVD31ePMWQn1%jHyL4 zZsPn7XKN_GWIdw3x%rBvrR9$w>!cX~aI)G_+6ZG*o?C1jGMZ*)S*|Q;tg7-mld;?L zO~YvfV`>=#FhT?;xjg4acHG(8c#_Dj-9tpRvAEN$(qyD(IuI_C)~l$jh^5qz*AuS0 zyGJgwx?-_BUW3~#$rmk})=MnD_g+)e&`P^;nftx2XOQz!3<3cL<`@S>>wCAJgb z{1`HF@qYmaS1U^76f@*GR$9Nl7%O@H+eeoM@ATOg_W0Ipi^b9ovHL89(Iz}QusHN3 z_X;G_onIS;JX<&tzL;Lb%rQkOa?G&gQkLdbRc&TdHiZ65gGb`N< zFi+IP!^>1F)5|^vZVy<3Gf{=~Y7_f*53 zvr+Grm(4ER1t#T-b`fFdXfCNFSvoSZ6%L8xsGSxc=i>->uNx4cc(?B&&H`dwEV7=m z>FnWQ<>ogD;mru;tPlhW9NN3-L%nmmv@|tq&aM)1?H89uftQV=qhnls!*}~tCooJ6 zU;-3LBqwJ^e0tH~{^<=~CZ<%y%5tf*Rlqkm4Z7MUPO{CGI3gZUQx<*1KC?W@OxF{o z6e<9w(uU#LUn5m#)f)EDgSgFTb36fFWbsWxy)Sc`Vz@v8&bkz&neKXGJ^by=m7o3g zeE3Og`qHJS8Fi~^@lz*>7l{reB&-p+d<=A>$-wxT{XIzqUXlKFB7t4sN1G!52eBwih5Nz{wC!JIm}fI=8SkU03Q) zI+$&L&31iMd)4zghE7JITDAb0cmaCQq91=a=E0*7(rH}b74Y#-R3V}a?n-FN`#|;a zS)Vzt02w@6sLvmt_x)nb$v`g%-XnXb>Hu|=M}tu zwghZ6;Ib?4g5%lgg*le3hQ`K5jnWd&>q1&83pN;I*mv*9e8W$l9b4A< zBH(^|SbhxKx*ih$Q^yve)v0dp_Tgs-+x4#@MF5K|%4i)vfqSQJ?C7BDqF+eXBiIAy^m8dz79l@?C z0G&>k+4SfT<~#9=W%VvD<`E60Z$SCF)IRQ&Z4~WF(4W~r<+R2|-%|IVtEuzi85r*F z$;&wvtPu65IH2a@;u4Et4~-BBd^EAt^84w9&4Yw^A%BQ!sRa0UI;JH3)SBsFdo~`D zGImz_tWD-$;xsb#EKysS@}O$Sm|^Xv-#~CaROC1QNEo-jD$N3?6IH5&oR-h zOx6^4#w!+mpxSm-zWii9fP4X}8|Jm!$eyuXvoY9XyDsz-6-=8Iw_yA`zof<^9qB~0 z9lWlE<+7>zBD96F3#`?(v?$nIw(d|g)=$7A17h>Wu92>^AhV}OXPyc-R;Fs;H-oPa zgbhQEp)|tI64g?e1Cca^CgXn%g+CwyJ# z!F;O{p)>>>cjtf5#z;=G)<*9%K_=esw}qabzy>h4vPy6o%wUY3zfJ-&Z!zGE`E!FO>mnbM}j zvOC?I7I~dGW{t}bP5h!y+M=!bJ>^(S%hf9C%Hx@`2vCOHCOx`)$h2C;qEsI8SjYAvgG|pxZ@HM9s*;$36^&-nmC3giRw}a z*tzMyVX;zXx=!l!0u??_`$P)y_O+`t9W|RiHZ|Mc`ZgWxnyQl>B`xOKnxQz!z)CSr z&@pb&fqr~8CL-_{#|)J~ zhv;N`(;`YesRd$D&@G}kBKWG*FtA3c@e&=3di|hXB zKd`gUD~SL&D$cewZ-t&)FHd*-rXfooTEj>TzxmXW*;Dn0L#zW>np64<*HJbFZZ~aC z_hd0}zxX>??w63xKb#-WE<>7GlH^HL;%RHK`Q{Wq!@PwZPvskzx#6s24Q=i1juuS% z-MT_)&m|`ztIJTv0k;a=*E`&E+B{%2>PtD5^!g{Xr$hYjK)f+acI-cqeSa5$KPw3x z4)%Y8*A0Hc{Ws>5T=y^h8}~}_U;hTh)4~7$`Ny6;nD0>krX1_=uGnrfHuM9pK#Wnr z^47d}@OrwR?P^-i^nuaJK4-sNiZXeXZtL0HXZV>vLm;<6QKfhrev(uBmr1Iz2%7tB zU$4rgEhCKkGFxSplxD{X|9PTZ({XNuzZW=7f8hTD{b zfBW`AR~``%R2d@#8QRO)MYoqr(TsSPC)O$+K_|XA*5DPeI-D5U?@PpD(BFK?U%qZv zoRlNoOF2WYGYC0IQX1>U#sYdLbu2U-n&? zaJ=SV3l`V=p|*;YNAFbME$kVZVR$3i;=-Fc`l3UjbS5lt;0lO!po|5a;P_8=@#+s{hiw86nMr1yQHaA*afe5!T`0 z)%Nvqy}^KXKxWpG+c1jfgbZUcgw@**N_1-Fb~O=K@DK9!@mfNdWA&oad2BG-6xbE8 zLS^m4xdf^*ZHsi#gwyMC^cY8d*-TmQ$>&}ZOG{f4Ef+Bs!?0ek*FQ*J;4r_LS9wOk zUp7J}WDzn=E5Z8ym&VCq`o0R`GabqA{5D`h$qT)5EyN@MDU|K~!cB|H6V)D&`k8SKA|fLv|8)L+8XIoscz6b?j(jS;#;oGxy7VPbdGn}9Xo5hu zHUVuT?~-z=wBC-j0TP8sUVUa6nt)`WrhHN@ZG*Hs zOm`!h758MzBQ!W!g2Q$4!H*yDA3m^AXuJ*5+l#Ak+M5=`uv}on4pG5(WRjfUjFsCP zb`^h>IxImj&AcALZy+)wWIBk1#bu897?}?tT`fjnwm-;<24L7#faXfHGmvEiKtgVwJf$- z<>l+k&ZS`5nQws~P+LwCoGzF{D87d?d zR&X13rqf+GLeK85zUoU?5kXa+D6%>dV!~WrallT(eHAo)NlQhB+k_aV4fJ!k#5~mt zjkwgCk2+z0;d!0-1vE#PE%mbD_}1}(Ws-Pg%q&1ElK1iOoQT>u#GcQ&)J;6D!hFra zX4xp8IW16C)tL|f?(Jt2OE&@R4NbKReKn{KQjg&xsz^4+8_z}jG{h%)};J zQ)d>U9eJXMZHw@l<6@G}{T9ck*4Bhy)L(Cuh^e13OJdVLETU;3u~`HOVq=2ISl@7* z%6L;m=#Gqq9_z-L?NQM`{rsnp+he#`XTU$JD7B5(&C|mw=HZ_nOt}y8t!_N5qSInQ zILlV(MGWR>e}~jK1WeQZs!1~(?<0i^9V_3cGY0rVdGvM#ogPO(4bKJCx72#AtyDbv zPVekg;v3m4`B2zjoPP_vr=JR`wXl(FsJc`=-6UylWnMkRr%-CxXFBlNd92*;bjw$v z_2Y%+uhYGmMXkYJkbf?nEYE}$vpt`z7CZD#f*>#`7h0vK^OAgk3#eiZ6`|*P7Q55c z@`Mps=?~*pL|>P&C$5@|m-tFeH*TX7u7IGSB}D+tydj=$)7&;0nm$bb$DxPPb8hd& zF}zv~!VO1ZT@UpV(f!`E_lkK@0Ya*n83dXdEeiqBmX^@gtMkVP=gb)tzqnWRPaik1 z#^>h#0ARTJE0x!eT=pN`i;BdU@o2Tni9`sF57aVB|pSGuR&c7v@UDNMg{NhX$>>fnxG?`yEmbD3~ahn7oADv zV62h4od@^g+=%_kNC7GA54~(Cw|53POvfwRKl0|G7cC;@t6dvfW~$mhU0mw<**vp3 z{VSB=M>Q`D|oLC~n5U5HA$qAU1CbKHn>RfR8 zOpz0I+3w0L(YOcM$PsBQpN$4&<&9K0N+R`iHE#0To_Vexm#mBwV%yrj|{WO4_&w!ORdI(5~iup78G=9YG-z%!vG*=8T_eSa&d&8z0Xq+-D95}4@iDGyi z1K^k41qx;X6#N}T@xpnB{p#ZftO46f7Hsoro@(d_Pa1V zfR0uN*56LYlkMD5VAxx)r{lax|7?OV8=d`TwvT+-P_;+LiM}vfpSU% zT>6xgEyeE%CV#_&H&ZLPFH~o#+{*TWMb_^!ecO{u!J;~Yk@6khDt$#oeGJDokP^`$D=_FCWTPuO zd?7_9jmFshf}vtn<&V4X)W;Za5s+u77XosLK^HcZwtSJDg=X|`;ybfVnrf&ny-o^11>Op0|qQpCX@3fkC(&Lg|HNX!KuJh&9tPeDYF%nkc>bv^mHy z6V;-v-~%U8+tgHIKa@d9+RJam*QeHYktKvqH413PSij?qiTZFdewcrMHUpg%XtW?z zq?TE|&#VYIRKWOcxkUXYsJknugPi}aAV&E2%h2$(#G6*DCR!~05D0%OVH z>cILHO^XL4Fx~Of3t27F<{0s+#|&+cOp=O?d3N%aS14|rk3318B7gE0EG>1kp{O)=WL1TR5emkVp(jcrblFgDBNNhEniKz73 z#SG{CiYtwOa{=IQA1HOc1dzy+t+TSi_tz3*i;7Mc)&3O*U#*C?RY>m6G2gr`kB!aY z*oX(^Yauoy9zJ(%eUU|JN)03}3!iGs){HC5CyyM+cqBqyoTQd&erZ^HrOM86k%_467> zQhhMFZck=N(z*W?f{Wt$FCjQXIBWmc5S;5K7sC=r1EGA&W~laSD3!45Zm3K}n^qcn z`}1tCV^gzdi zDY^h8Esi5&A|g^y(`vGJ)=W)t4hXkcohPQ|GO88^GGl9Up{2`yph19XFvm~3W?TKp z*vQ;0i8^E|-|@zC{-fo1-jc(@#&*9hpO_pGD%uPQ&)?+JQsv@t@SFUbWse^WMnf0- z!TN({b1n9Zmm&~{s!hhT&tGx4o2;s0))+{ET@BL+AbnS^5wWlh^wrj$&yY9nlC8pg zLnaLXaA@>}rVG$ky2yMC4ejM$Rn z7xV0k6Y0rXkLxba;e-Zbbb*B?-BGYWnyg;0>P~rjVV9ie6=?oy+G~^sx&GdL9CMc| zT&TvdNuxPEAjfN>%IXTWFA-{s9gb2?Zteq`xm)egT;KKlXZB=Gu3x9t?}!-~7=Rx( zQUFZ~mz=-<6&Xab;{PMSH=zabg$hee@Ydu&tFm`PW~w+#m+2tsN5 zH=trgbeCN&DfOyoqO@+roXHE}v2)+tmeYK#N-T%Bs%kyP>Br%2`H~>t-IM0;q6F(@ zOwQgkmbvogqpyb*;b<-}uKlYibko0D$zgBK?6S zd$+zsJFlxtd4=-iYOl|qq`f6>tL;TG7yxDIIfR|r&Lbs|<<>EJbDlX+X$ z5DbQ?)yE#!-jz7`cTUs3g#gSYZqwS2FDqZXd?^F}1s*2B5^Gs4zl}I?T6>Jdqli>s z^la%h?*HuT%c0DD5N!w>M3ly0V(a7#DqQmHp^k^eERW@GdW}aMbbeD)D}GRa0W8x~ zGlBD%_*96Hm+@_~kJM|W{}P3a_M2+X49RBz+Z0Ec;a9DP;?a*}e%=A92QjUSxPxwk zkV{+5bFT;<8(ib_7j}?lNn%rh)2_Fj$M?_M{sYF#Y5n&wo&^6tVZ4EFA*rjEf>^K+ zKK|5ZB$6Wf1f-;-Ft#o9HF)7M$WOqc4!s`RV^k0$Xd7TNHU@+01@qaTn=%3&2iw~U zItfol`psTM-0>1k{E_W-35I?SMHdKRgFk`*@8jOmnEzKQ&+f)Ff^(-X7x#}Q<+Jad zYdyZQu@?{O)5sx=`-x{vSQrFjj-^OiZ}%~ga2o1uxD0TUBgDT31wv!ZzJEj*vK!fL zanM2_vS-`F63r0BzG-0CJTHG!HKkxfOLSwAYvFFpPPD@RGk5`ftdvCW| zn#jlEi*K<+oj!0CqWvK)2#I@QO0l6KYu&ph!xjr0w*XyjoQxM){Jl0UHv7*hB*D%s z^BK#312!Z`Ui`0cL#($+R))}kzIdhKcG{h%qOirwd+YYyZ&eY&6sSi3%dqu=o*Ljc zn29dmP&|*-ksXTvvI~NEH8FiH6eUsU}ah_%*54M<(}?V5O-?1_IJa-^n^Z;H5eCF9bUrj&g?*KpO$G zcfP@x-}%hULrD%v$(5Cr0g}?u1iO$P2BA}Ism{)k3fn+fQ?TnzJ=_`?Brf+oGFjF& z5>omOepfKdF?%JS2#vwYv4Z({Ne&1)lp%K9+&$%{Tc0)o$FBngxZ4aJ(e`|+$bRe7 zaX102u3uRIo5GP{@Bd#!jDS&uYOmV9w%;Y<#@l>jM)=W&w%m^fwMdW=9idgRX`EsE{btJ$KewwK& zt;Cu$I*z)ouAnN@ajr+CA?w}XayGp%QEiLps0Vt$Pq|dBY&An6;{Vt|RZUlb>nG4D z1RQog6PncTX0HM8Wj?!C2m;w_dE7Ux zXW^?l?2_y1>14d^cRs_J!6oOsiIn79>W0s2lHZjA(Gl4SjV?waSZcfa3}vc{HuStf z_Jo;%K@Sxz3DDw=8#g%jQ$X*y!=1{w-lqm1gU-sQ*8u8E{gkD1QQN`KOigl=zb$DK zDpe*OC7fz9Y?}>$UwVe6RHm3x81tdG4*`8b-8ia3Ti<5avyVoFw7%9o@%x=OgDA?K}?LAxbHzBx@E};(CYL@ z;KOnJKX`lVxGL9eUwk403L*lcgp!I9(j`cU2#6pGNJ*n0(%qqy(g;X*cb7V8ezg znDXVycT%|B)ZQeuU$cd!%Rjw^NUrJxW6h&|cOte=x47gYL|*0HIk)bvniOT%V= z$z6HwlKzE(5!yk!p}iU(KDbWcsXA;cw>9Nze)G2Xf;-xuCab=j z@a=fmEX>9RZ|C6;YyL+XAk9K@$Z-nN@p5%C{}~4IAKZoiuNcU!=pwxj2s%{fp(5k* z?id@{m*V0Hi3g2ldJ{g-U6=RfA*<1?gV6sv139b=zS!KB(11BQ)NyMCl&95T;Brnr zM8=0wYK<-8nh*QD=paFf8!tWaEOrRLdUEBH~b?|s>L+Q%cy*0 z+%BP}^HTZaB6h52wiSnVNVCI5@-zIuU)JkinF}`_|0`ZHj|lk^{tkfr`Q2ANLG(Y; zOaIw181DD4#@|u@#iIVxMe?rx&s~<1bd?$}R{Upizcn*5t=PwRuT7Nkj)<_%H-CXppH**@9NbW`CD-nozfqKZ-9-FF*4=1h zoHrsO;$wf@W7OmQ>bBK>`^$ff494L%S#BdTnQM7IvL}Qb_ zH+hoOhgAjhvSAkmU3xuGuW;J5_aH~3xrgS<6Z{%^=nX^Q8*%Q+D9L&Nz|N#qBgD5h z?97t*GC6c!VnOWnfP>hNht&X=5b|4N0_+9iQ0B+76nRnrAs#+b?SRfKCqF-4XGZuM z3AalG<4FP09ua7|HpjfkdfOfAayl2g3q7Cmfn1wwp;K}#jGgd_8Ul9sjf%JpqWkt^ zH_|K_P`B7ED|3 z!W_TXnsmYB40LIpfw|wCW6Lp&4jrB5+~9?Zc@_O}g~wuNqtl=;_~U8Y>a;OLcM3i} z!Y|Om1{wfKUMZDXM1qP+-9W2hv(xQT&g^Y|SPD$$+d_N~6~2Swk9BJ$6%u8o@@?4T zpDdgg_&qR|^kVo)uei+$J*ei?ug6F$1`@8>%vVRCqbd^31w;WTVG4>j5J?Nkn6WW1 zG&nm#GFGh8FbnmFyrIN=l!3+}Hd)0C}bTX!j=vguYedbNznh{D@u;TSXBHsdMg#VNL=Bwt?p1}Ry{$$qA6cm@UuE#=NN2rI~*Nhz7P8DI_XeY^J z$wQlIq4kN%&SF-r8}=7`{0^MT(d!cSZUdQ$NIMQv#iZ7>P#X#`z#pnLQXZaoJ4hG< ze4WX(L&?ql=sw+?%?Y$J5t=Z<1yd6FNk^O!Q?01fG)Dxumn7i;33R0xHx7;^x zVpYP|UbM!uc=|#FtS?vsCq?6+uM6wf9U#O;M1!R74C8uFrm6OT{>b~B^?$`l%6y<{ ze!!c_Z{Qmlc^Bwk3A|>| z$KMKC?w{=a8!s3&9xy{EhlY1E&kX~IOscA%07_D`jp=~-00u*onRy9JYpcad)ew>H2zKnu0UP^t2jQhcmKEN5~~mzAGj77|Bct`-<>B`oTZdi0R~UG-?OsSmJaGkj zvL#5ufwEAl9#cG$&7W>CizorkGa;iU;&py2CpaXLv?^@0=OOIo$Cjr*4S4;)-00CI zL9%;d3LK??N%-Me7sYfGU!sLKvDeY3gbMqo4QB%P&zhO>sWss9g5nfJ0CR8j=-={X z)CGRS-_@6S(WdbHMH?KmVDW@`=mzlU+#RuRgSNE9{)7W-zHK16RXqw7)b{}w9Qb%h zW+b4|mr9PI_uO$m$=+tOOC2c{@Ow)K@;w)aaDaac)`q^par~zE)!Mb4zwONwDhK1I zhPj|RV@pxR5skWT@^{8iB>_cL>H*Q%-YP7)+bU(Rqx?deW>b4Zm@KXh==?66=<5xr z74b1)Ud1E!c7wZmG}Fni)05)=;wa$(5V!;GmwP)#dB)g$qn}W7>@z=e^R3E%PRF-2 z5?M9b|FLkytoViJ_n!}GbQfbjA17N$|AtJTOAOfzBj7ssq79;B)jq%Ui_f6*fpS?s z3sVz0S>nmV`rc&8P+*U!+#L;!s%gu%L9GPO!M5jy;Pv%0?^kzu;2^!XK3<+I2|x2S zvdpp8@?6QnVDw8_8P5sMxq+V~sLme)W7l~7E*oKpg#OHrN6143zrkz1YkUIU9NWvY z;M_jP&}pk|<;qusD&<7nr&t{kydv=J(nvZd$Q~9v-^44Ntz`cLvs;u2&Zyh9HR1-n z(`Pa7D=Sxux*O8(4^kNzG14w)Zog=S)R$BUSaroBKyt`><3($R-xh3yF1!1X*5|*e z6<^pg#C6!C#UXtXW3Z+O6x5_k(2c;M?~KhW8a0s3>WKKzTupD(CEPaod*AbA2ikWsvASG&Lv4<_bEht?f_O84Vka`eDB#S z(a`{k(7AQUFp&J_kFg$Yz<44hQp?jd$9`%9Xc)$D`hJnEw9)s3&fnvE$QhKHKmpEo zL{$4c3J2(t%B=B~bGR%De4ecV4HBu_xf)@; zf~Z!ug>fKy09~>(j&5I^v*hhC&aDI>;Hc*UNME8!o2K@E(jeunNq=q%@}Kx;Wxk;& zy@F6QGo{ksH*XeKpt~e%4DcQlvRSGY2kX2=23Yl=d7VJpSsX39?a&&W-`A-cExg1h zhD-R17=rYP9d`X+>^YGA|6&k%A@J<)3}QCczr!G|zPR-tF^EwMozVe)egYuw8mch8 z4gCV7Y(5$qUAWK8><@*{hv`B53kw(xlBypM!Ri^<*Avvwjy*Jt?5(@`Xt@GSV9_o$$GN_a2~g{BV}@7HXPLV`dvR`sjDO93;?$G zJgPN3!W6Vzy=PR)j|@`=DgYu+Zh_zlM54)D(Z!I~zj3YNHk8wl(IJb5k$gm&xBD>F zljg&c8TTy)m725|HLcFW&k&rrGzbp|?c0nhXiJ=CbU?2_Wn;vsG7T8rxJ$zCwQC*6I;7{OtJn z_+WoqR%&G)c9t+g)Ywlp|9Cfa&X}=RDrTY-$sqodK#O`kfOV^mo^Jonx&nJ0he1lQHP?x~o5f z>QhZW(o?~7VJp{?1f;*v?0#t2HGCl^MoB?|0*1%=HdUwA)}?x=V*-*LYN3P7wXO}+ zZYQaC(Wd_p03P+!RNa8Vy#ox35At{SA8cih(Nr6Ei zQ~?J8m*do>^OwP|^Q?@W^+CU)0TG}%8Gi0i-SYLRhBSR7OSFJ$D2mtd)r{N5g-r5) zi}6uaXj?V+yK zI5ReOwk>qW@Qeh{kxW;2<=R2n|F|a-5?aSh<~s1Va3Gc&i4&(Ro(Qkt;o)5ycaMAn zV+#ZA+S+7>Mn_jBT4}w?Q?tY0o;dXT|=QA#6QkaBJd{a(OM*C%t~Ufy@|^2B2yTgTY% zLSx@Yb0eV$MRufL4Y|5tgFqIZ0Fwl4|Gg%avSlbntTxkgK*M3G~0XCN&n$Dboh{u=vbsKl#_dn&xBoG7{gA1X=`P)H^S~b2hl!_wPVU# zlcLZ?5B#KQ@zk1{nmSZARXi~078VxPxm-z3HRQqV9&n9}j_%xJxZbY2@Gz0ZRIeN0jK7Fj ztSw7qZ35tf_pvgk;aJ?$04?+lcYEpR>YFr2AY~27JeE;-6koAEx_iN>T3Q2MVH0ow zI?qAm_@(c@?FcA~rv>Fho`!*k0iu;iv{GWC_Xkyrv5#kqsi=${hAZ3B9AaLaKj$2u z?OYI&NlT7Jai&Sn|K4LiQlbA28S)PlH<#V+L+FztQvYqr;mwWuCQtaBtn-r&Gv%A3 z>`#zJKHr(fqv1}NPO!4XxGnr#gw=YP?VQUY0(K1+&HX#j*!N_5;vgU$N2~FNgbUJo z%3EfBKRN)P;xo#zCsUL+38?VgTBoK!%9$l1bORKvNsP&u(qWP|@OW9BKY=o9raOiG zBfH|6B~19p-uahC$~O+Ms0GVj@vRP5PJC1r23T|exO|YJsWvb8a&#+X_e?Qj*l&P=^YnQIh)xK_IR|s9 zMf9-7I*zbzgDOCCt4H;#qk+}d1E>I9g2`Zfl45+fqoj_evK@D>HNX;4T;BlLs!Npy zVGvEb{TB8AX1KIG*is*B@YVwhVA;WXE+ohx&1b$@%Hk*8b+$-{Zz4E+IC6{#uCS+t6_wR!FuqV!%6{Oe8{OwP8bMx~x0c#GJ zej}BAcz7Er1Olq*RWxsF$hQEHcWG~)Hy39Hryjj`1hs(i9g&AF7$IEd!~+$@6Yb&>{{H@-Qx>sA z#l>m>1l}B+zzOO}NJye&;<`LMJp5Hsv})?BsM*GRf;{8L0*8ESlZyyxe0;EiV9LKucY}#XbeXrq zB-QsQD3M=r7d|YkXuA+$^IAm`2f{_4X=va9dEHeiEg~My_Qt=V&=-r8&*rjZHk#q5l&BV;%mM<+= zU#9OmFYOmgB|^o(r=k<8-1rLNWKJg+^UPMHr1K0D9UXmBrQmWi4Nb}?oo`Fh)U|6n z)ipd40`Cx4XjeeC4GC>e!2=Sw&8S-OgdvItm0Vl#^@O?Z*32^d&`vL3h+LMJf2})Y z-A{S!x~|3l!ov?xGe|m@asS{Nno4u4ZKohP>hO*7BAh=GL zJI_c~aj!&^s7&0X!{EGFwIT)hiHcdTvraCelhuf-I|RO3F6r{XV12%!Ireml5O)d?*kd zAoZemyc3wqq*i-ifo2Sd8hVM5asuB;Z>KtAvLNJAag$Lj52RuP^MTB-z}6>D$+&HjDRXf6 z&8Gr^kFN>&irGS=f(btG)--0q`iF9?S%rPW5%HJ=UFG5UTH@h~lN?9hL@DnHI0hbd z9I8r6;yAps%Y{#6-o}QKeeb@wmv?uOgf6|pWM+tQ<8brUW|~y_GIjjzgTCiHJ2>;;FfAx?@j)fh)ALwW+~avn)h>E5EPCPbfzt zt`K8j5=mm-Pw2S+R5bE|&&j3ED1P_RrtYr({`v)H^qTRkyW_uJFq)J)qwhA(>yPHL z8gU}X%`71oDgA*}^d}viYFNoddIz<^tlBA0TpzJT*DOC1`s3^3Zrm8fR%i zWM#C`^rcE!H{VBcn{s4ird8Xo%aP^E{`nh(>MiFqZt8^<(UV9E;N#-@&%<;w@?A6Y zTvqa&zE;d@*FTHild`48rljnlFVmOfi z7^qdqm-uFJX-WO)d;QRSC2Fz31Vf?8dP75vWveXxyI)t}xk{A1j*Dx5OgzF?m9f;D zqp?RkWWMNP80_fFsNL?O%rok%%>KtNKpBTuOB3_$&#V9SXA9RW6p6oH{hxm&s1qWn z|NQ^|`m^W8H3YnyfB4e5g6{Os*I~Q-$BX@kD-nM|QTwOMP-0gjnEv4+@cFo@AwvG? z<%m9GLWuvvMTCSBD3CAb*abjFEd9GYlQ26gQMO@hNWQ@QqwfTxDor7a*yv!?&Q{^V zsofyEt|nIHU7m|MV)#WmS$S6ak6uNV-+I(gFl@>fLd{Bt)jl#r%V&GA#wew5e@;3b z=g${puQbPJf#&kP`Tgc-DHcX84h1vK)g)@_#gRGIwh$H~ja)(md5{>mYHq+kPU&b+ zLjBA%<(Uh{Opyxf3j32=sBR@sX#=J|T7_to3Q<-=bfWRS?T0G&9@O^LUFMA_QwQHc zS9dp!l)_PSTU%XDx277D!+Ytnw3$FcHu$E?rt7hqF5mRJ%PVF?Bsdahpn$d)?q}b3 zu@n2%(^sxJU(1{L$_dR)f#%({xp-yI_EQpYJ?l$u-o0Ovp=3Bw;p!gDSwYB) z=}pYh#@X)PLE@A%lk8A#6T;?j2Y5Uedy1z_OiZuKvIJ+>p81PjmX?+VL)oSLWJ%}5 z#OJAj5F|Wr9cLUbocO~r%QzreZSG*AwkhQj!>I`_Km`qLI?@CyFQ*Mdzp_cs=FY%v z!N)m2#S#`4Hd-G+V};ACmu*R@FLX|Jl^=F2G+x??W;I0@#wJS&^!@SUR$54SZFSaN z@F-r!$Dipj{7AGLyOQCTYw&bDAz5>*K>~!6caS%ysp-nyJ9jtxGeSS9y0fydbQNlE zPbO~**eaLfD48R6+(U}9=uv;THfELW{>! zP9#M#lkd-!yK5X2irD1fcf7nY=6N6M7a;d{p*g9owM8i?EMT&2Fhf-RSaY~Cj_+|F z3j`q!Ae~cnI+(uC#Ln&-ssAqM$)MiQS2>+*8H1mCBY6bwK0<0UBmKxZJL_YUZvT%|K*WO8a1?mT*<1r2j_a=9OuSY*;N@2 zi5iP!wyv>)M4~IU2OEuxTlhH+{bEC-h0bInSY(1=Bo8Wo5oZ1E zdmnXZB$>z)_yl*CyHSz*zJ&$HdSgA*tRF-rB^!I5W4-*PEV!NerkvYuF#GK*=eRxf zrtxUMs3@OqD!23ue3S9l3x`DO95cvLsqTb|4BTW z6~orq;`Wg|ow(y*PvsI#aO>9bo-QHLTLj{_$G4|5!WR*ZV%x}N)C#k=ruZhX44&~d z0@(1xcK=$dot@wADv`sfK`G^!dq=dr&4FO!z7N8wD{4ReM)N<=TZoEx)l~R=!}vPc(&dXB>z`LW`aQOhw<-#RX$?SH;P7 zg2An^f-u02D9@U& zvCne(pD5{!0lB9(T7(S(25dx%T=8|icf}2nlt1<=_|7nDV8PdpxlN9c4f=l%OZz@Q z*DA-v&iuK;!8%qskdZ?FO>zbfa^FcvPfssshl2F-yh! z5NdBTiQ_sftJ+7IX+H!BmATAHXDK~?`hh^0A()E%At9XBua9_8Tp~I99y>r;3~|6N zV-ys~%`M2u{zY?HC33g6J!TUR<3sf&<#&5q_^GL*qrszJ*9eG_gmN3#PyJ~cpaWeT zC`_mHU0u~X%`qW(b!vxc5_+xN#>`!b!C7=yGIFD#L1Qxq*JA^%UF}8=&z0&}+pW`j zESbUr7hSU3un8wwoRFO!Jpfls)}aGTr`TN@d%2|?Y5!E{;?cXa(V*+DOK*)#>bu zZ%UPHu6}=RV=!U{bC8eWV-MHDx4v8L;p)wo41ariN{ijTansAMs~nQ%9>cqy9BD+0 zcf*lZO8!BAah-pSA3?6|F{9)^g+XxATiZ$^(7|lX$4#u0ty&wa%wuK8D`#wn;;M@8 zs`QTc*RR;@Y#b=(!0tv&lbVVgF?`F~md57|g*gL_;4;0xXpTm^;vIftJiKq0krmkr zr{U)2)UyoLaxwp`2#d#u8<7FV8|16y!_Z)BJkE8x!3bsN3=CRmo=S^WQ$Ds8C)F*&b}IY*fa- z3wAUZ!Foi;4f!+T0$OCHdS`J0+waD=iT zqvlwmiLYg4Z)I>(p%l|pyHn~X(OY;LWSZwpe*dy3X%AF=| zjbQHU9lq_}e zjyf)nj*1G^Qd4v4qhWFZn2wSTx60?nt8zrQ7V-kO{)Wj%CQ!n?e2^{vgA%fg^(Zqq2%Tx+nL z(t(4g&zBET4}P=;(YZ@&cYt6fjO)fv41P3>sqKDU`HeCKf8i$fH%*0wkc9E|iN&2C zw{CIWApUYjQH_js{Bq!u_IBYrWkQDokL8r8(7l0-&Fb9RDP&ay)x<-&S4YL&j!Kn2 za@i{Cs-ibIvQ<4(cDk=zA4`|D#~e+*Zn37Oqy9PPnR`&tDA}#+FJEGD{o;|k@|feV za&?=Q6=hP1k4~<-$X>*%?Vw?GsW&jH!~{C^#fmd{%LtRwI{*iY98OVL9c?E{k%n^^ zK7|1pSaxB3&=Vc+uDe3B9Q%RC_L>Qa8|hd?SGFNpVQY(|`o*c9@pSx)N)jgC8S3TGR9{kfD7Ji1=bZ)xx|sj+uy)EMt93ik+WV*v``Se}mb>w3U z=ZkD8OXqIwYi^gRL7clcUss*w$MobZ9W@ zBaBTF2hB0U=?%2?s+$IV8E;U3YA_$~EunrB`VvxRNices#~82tCr|v-v*RuTPi7e6 z9Q42qE^bz@0mT>Et6@UIo7aL>c-03xZgF#SgA@_%cyG-aRekCx2?&O^v~dl!t6MIT&0r|s3+cLe1)nCBK=AoTkwn| zZkp($nMA$rj5HXot0T-*EP8E2tN1a}b;xp(TybcGhR@bvB)LFEG_o^-*WKWiGK?&v zLbioHhsIr<3#lrosP<6CqXp5v6`T?F?&=u0#ca7gAD~$cYgtiI(P(Yv3l%^PJUp=B zU$kEr5z=rR`ed;wbcn61e5NF z%rCnmJm_&^BZ@fS-r{H{XI*JG)trr>JI=B8yce z9LdF%6;0@UzO>T%5KK5G@0MF+DvFAU)vWv7@FHPVG=lX+nLtl*_G9JQ$wN6|VRQ@k z)#?ZZRvCkiK#K_naZ3Ly1m0d#9u2zjC&A;mo-79>-2_FCah_wCL3J(Bu@qP4qs$?rEDi)l?tf=$qRFdlOe1dK!K{nt#B~PC!6Q zCC3m72HFb4or%wInD)2KyqK2e znzO$9?3F+v>1^rkWy?v|wYQkDt)Yq`F zb8W_K$z?x8-`ffNiH+9|FE{Un5P*V;;?R9bqn$gRCI|fC#wQd^i$KD<>I_e*S z+_(#MDIPyY18C<7 zoxQnhw2oe8@pyxu2ao8gva&KEq-)lNwSB^9~URB`Yd5rQf)Z%J;<46l8 zG#M!c;*!N5rLE@C`8@@jm}S*U6AArS2MV#0#3R1uFLbE)4(NyQXmilYaY$!eXhq#- zVV%$j-w>9UNxcNk(5<2t>3Bf~xl)?&$VX54vc(v>PDU4+i8XTNH@&4Vlu;lTNc+j* z3ZosfF&<*FIUt#dB@H?36WM40d(O~b`GJzQ4EW!e%uMgP1Z~bF~*@ZR!`Zv~s}!KRZO zvpX7t(l6hYGC*OdarZ9bzNLgrVB+4!Sza^r_|TYS9;$RViLO0)&r;fA*2&GS5k#i- z>w>C2?na(uIH|{ZRxmT-eq2U}h3JKp08z46IT) z4grjSB6sELaw?t7;2<7X-%yCMLlL0#ryaXSlaVp4c(^77VOzpF!;xjV0ADKHKRkiv z^0%+&GeyNPG1$1Ic3lV6BlH?^{>F3r(f+efOU2m%-UF z3cb`&d*B69XXC6LoZh*DBSJ+&5)wj~u6*&OMGSTc@Mdl9?^cb+2-Gi(KF8WzNu2QC z72iX*RNs43tX!JgnvtQmNBq_$Z#zCd-ua?GBXu{oK<1(m6M{Icf_|Xu@HsLHrr4e$ z9xn_8_Q}Y!EG#X4*HBY)f|WW%DJImr+MV-=__Kct6Tka6Xpu2tw#(muJn+6K6kjX zd*BR>)Ro^;zHp#|c`I4;{N7tob&NGFUKiVNKkLFF`y%l-Ex?iV z-O?lADH=gZ4Wi3-$P5Tq+<>KZN%dlTttdOJi0-B-NQs{{|GNHuWNJH z(3Ft#8e^qm4!}y8vc0bOPHIr7&`z*dHAIf%nMM>)iw`9fl)t;I_kKP*rZ}!)EFrt? z4jv4RE~oS7E$)AAI}hbTORyfn4T>*gJ+(la>nR9@HewQaM3Y z?TAYKKED<4fHwM9?}kH4MpJwv`7O`xd=X3NVrA@p@;t5vgg}JSH|;GM#ac^~6|y>f zVtsrQidlE}gG(}OEK)X{=EkFm27LEpRmOh$wa%50gkPLkxkez{Zv;6s^Kd4y0q{^a z8W8QOva_|grI|5Wa#f%s&~rYBj%r`I>tHy4LB6>8Ho(_n()11krTCPTzK}rh4%nw! z!1ew8v+QbF>FBURH~65CMn2lS#g1;J*wwKBNH`o}0uROj{2-jVpQ7IxOq8OATW);% zA(q(G)*^E?Ev1a{>tZFc>L@YiLH7)1Xv_>DJT=*pL$)`1qeYL52EHA1M$2qtmUX1T z0!()+WGCfy@XCJj#CUIn415ERk?GbOav4sAK?K01pdtv_e`7a71{ZvE6#k+&qBjvR ze4%#aV80C5Cl(f#8wB2lw0hSj5d0S0!X? z?TRP0@SQxN13;dyPaH1cknw&Ir*^)`LOj1wk@Ek7*OSKhzvT7AiTPg{DUh+4&3>$_ zZ=`#azcO?ckz+82Y4&mB%0a1WbyXE&-17(d8;jTj2< zzsUlBcu4phzqdNFLmBw@*s)KExU@it3EkmEw?Etd*ptFsq(9;`-ySv>K5x%cWbo8; zzFp==CF5xmuG!M>t3Nk*gR(L+u@|zGjTc2PH+bElDB%;l^UbRco};fqlp$eZaXK>% zL-S=$JSXHoK;w~~-83{qR z_1&Cl^!c3r(y(yEn1>oYUS#&ppz+pK0DOOlg?&GLM@`w@13#vxlut~;Vc4)}jIRcB zg@0O_)_CbiWBsPC+<6d0Pcf)q-tg0UX=w?nRYqVh8O%DLk1oY?ly4l%u~UVg~wELzJMmhKz~m%Y#?(J zBJ{ZSOg<}7*J?-dsPC3B6Cs`s3Q-Ko#=7*C-n){zQ)jF?2}8S#NFEda{Q2|Hu1f&m z;o5M{rRNNmu=x)Cg47=Lb##Qw`gH;LoG))kKYH}2WZ$?Nfbc_;`D;PRIlv#w+yA*m zFIGMaJfAOoGJi}^pf`A~Iq3RzkzZd#fc(;zFSxiC0{2r?dh+t}x>^hQ(Q)oL0Wuu; zNddt3-Hk290z!bQdo%q+cY1;ID>fOSzN~Ej;N_ zYs1YhA)yO2Xy9C;vKTDTFaNMn_*_i^N8qPUS^>DK%#SfZk)YS*GM}eUkfL!YH@jM? z^3%mMCa^hXo9rnQrwQN_wtc8#wNZ`?Q5n?1LOo-08>VaO@8$`Hdg&x(MwT2{tS5}&@a;i z|E%QUgQp~$Rcts>0~oTZ7pW_NIrHK3&j!T=hPI>u(uH#aDP*dt@FRz|DMeW>?)y#a zRjyNu^*U$%i!c8ExDjO*)on|?F((G}ibEk3TxN_FJ4d-yrweTn$P$KTE*_v1AV7?A zblp<}sH^MhN_I&U(FrU%aKq;94SRZXAvg^Zjjl6(4^V&q-sQT_^}|mC;)R_rh=_<5 zNAfNGvy`8|{k`@6MW6Lz)0>YUHBlLHEbH^;_zx$0=IDkhAIRUg*YW*qUaxVwzdkZp zn`oFCMAerpOZMlJaa_{!9>y@bd0PeTFFSL-#1zYK?^DV4i{F(1i1-@27nT{)@DLmt!;_@n)FnU0NN65aYKhyE~9 z0+M6A_Ng!HE!M$$qx!a2=;Nbutd4S7}1(7HA9#5tIm&b3-%Nk~rCT#9qO zKh8jL1jB;vR;&!tCO_>yW_rINiKGn|aciL8^{*deRYPC0v1o1;v zz>RS75EiCA0we_q|G~bjHZP}2QDt~iIl3ts;b-dO$M&CL_)Wd(< zx+Fuf5UOPkceOj8tJihK4v~>&SrEs25KOHtU4^k12mu09Q!^v8D$qAP5Bmz^$ai^MyPOOEi#n*Gr>fw+WtOtxx8*C}6L zvEVjtk751NsHF1IwoQtL!F5DVK~5Eb-Ap>i*Phs%63X_cAz?*2+3f0X(#ih&%LHj} z(>*}S82@fC%i~F2e*RW%%UiCk9h#;OEI_3~dtO6Frw{+ozJI7^~L> zjTTj&IBs2nDsl=qD=PQW)U$*=*eiq}QIBB4aIC_cuiTcV0^?uvkA#Q;HNpKd?#2cP zSK4gQRh0~ITqF$%tC49z*AlupS5ynzz#iy$=nK|eg9vOl_>k;N zb-69mkUp0~r5ze%B+3HP-O^$^o|7hv{bqq@a(<04C{HX7IBOcYtcaLAOse2kTp-(! zx(lTpg!-VExer;cycf__>XxEcV7pBd?|k_5!>BC3BiGRIR5+t)QgETpYcgR1?;C_S zyWh+yUF&Vr_D3PpN(j?Iz##n=Dbgo7-*`6?`D^Lb}30 z_!I}DhUI7161jw3`$`k8q~?RX#e-cJDO^SZ#Kc`>*%QBDssm0g*x;W3G#QzjtfV7f zFoc#DfKYiZN79yH#?uRNYx|GU!p~K!3Ka5jfAS#qR{Ev&@T2{l08Uo}ZPmw(#n&JI zl28b3&`DpsYinz%sk#4hYHkVT>{8VEBl>o~r>Nbfn(h$=p`l!VQGq+kIkTnxD|E_p;GC99UYX;1X9Mt za1KEzil>mk5zqW}DDN~j#u~FLFMJG?&r6xl{~d0ShX1pY`~+sRh-f9W;;r13k!46K z8T5i*Q+oL|ns>}e}N92xMVIg~maHL#IGW5=ekGL@_ zpNr6-Y3*PFPZM6G=NjzJ6DN6bjZxAPSw!le^IRU}WZ)x3@^QPvs?)wJoJ>H|T_afhdu-kw^!f;-tL5*SmtD9NDl+;^TV; zNjlVAzblx2>(T3cVeA4+v#QvyUmX+qphx{$VTJojnUBtdG}>e6 z`l5g0vl+7C4fPxiZ|34hL*Fj@6Ms;T%$hs~&d6sIPT}pj)=N911SQBxB(+1`R9O1% z^6jamUj+B7>gp~>n=dB}SRf_=Xa!kH4N{1d7s-oWiv7&j;ML+_u`i{!5am-7F-#?! zeW#7%LPE9}{sFzAE3gs+w`nt=%nS(jbC8C``91mDx6QGlyMT@hfn^&70fK3B6}yZYc}_Ua@qyG@-erm6U{T8rU`<;IJYp7?>UsGL+lY zWJNxL3Ky*9Si38u7#KC%j(L2@$ytIim1HBsMfEV96?bt1BN_}wqe<+S&dxTEsy@R3 zE&@P4?b|ds+9YEXNZz^J<-4XiBi7W?QnNB>=+R}nn~I+|(a+0qXKgKq@!pka^VNqb z*MDp{uze2+Opq_SP$L}SDv&$Y8(vS&TXS#uE|&NevgyigW>1Y$-1AvqMRB{rzM7^C z*cB#^8R7NFvY1Rwj9ro^mq8PqE*`3cwpS8dP~}*ECn?FL z20drrH_e&IWGN|Ru5FXB-h5u;j3Hm#ecM}4@AcTo&!(e~lz$N2o7U|Ihv-PHATXiF z*Y9{C3rrZJC8l%9*_ru$7da?}m9}kRW>unbp3MwCLy}DO)3pWsj}{ z117{{NpgKqMwK}~%V}A_0G>B~(YsCL-(Xtp>+j@d%moUZeM@7H5jJ;;3EYMhCLGx$J0ZD zf)bl%K)#ugnGv|1`9}FHyu|T*j}sk-Fs2~m)tcfEJdQx%jf{+Z+4*UDz%bttij9D{ zN@@^L#(fvQhZhSJE=X;ijO)exT1|t~mCb3NxvW~oHoC>eQY;e^V>0l}=%XV9csl#O zzEb|Fw(Vjpw&X?kR%&?v*@x$F`h^b~-ju#pY-#2sn&28O87-Vm%G;MFss3TE?}!yj zWg@(9H$kJ+z~Sf!Gj&kT-`NpVfBg>_$u#qS&?!!4qG~{%DVmo|;SVII=`Er0^V$_BbOvxqX>u9%u;g%e7fv zpd&T`yjP!@e>4Q<{+=Qr*8$W1B(bNf{~^&V19+!6Hq_@Yd{8GY-oT1eNsZKw{ApHQ zXRJvuQGB!^uv&idX>RM@7EQ$VFKY4!F^V|bzpJ=4#0d}3h}re7kT39> zQn_K*hwYINpiG|jr79yaJJ_kdKz_w!#o24V($AO_XZwb`YmHr*y0^tcxp1{RBWsf~ zN11@OmC4delD}ww!uVdRqMl>IJ10d74d|#xdO^wGT$Hj*oN0O9eX&!CRfst#?hIhB z)5x6+7dFqQhLZjTW0x_>*Gq!$?LX-9V5TXYj3XYqgJUp1VV zt)T!<-}SrS4%HA`mB&~756XW*joby2hr2(yr*?<)jVw$qy=Ga%3-bj4d?4%ejZSEk zL3dJS`T1*jryX?**{khx-A4p^rfr{7Q5Ej^HP6!a@~$j?ed`dib&gZyA=t#1375yPAa-oN*n~*nbwBc~)_kwLKxPKOh3P}bGx1WO%hTCGz zP1o_1;-NR_KrDkRhH~iyXdEL=xcx*EJvMGTlzxS{e^*r zo|iL4(i@jIM&5ivX~}={21Ii87={Do4rj#B()#`Qk(Br99^TU*$@)mFD9B)y{?vj7 zME~)~A$IcMY5{zT9zdbl0BMUUSWSCtjcWgbwVMq17pxuI-&nhf);W_pe$m(Iq;ROQ zr^_{-m2f4_8EI*ZIP@b_R2O?T@yIK-64CL&pfAaM7!IVuK=qZs}s-w zic*|JeMaSqFIp1eo8Qo|@4v51x7D*-R0@tp1;gBXO;j+C6t1YgR!C)3rKII19*-g& zBDtAzXjk?T#i4wc=X+Etx;bXL@6WZCvz@k<7TN4K&mYq9!|rjY+8!oG_VVT4#LP!M zJuf*O9lr`oZ@jdJ3%5#@eEuRVKgKyaXUQWWf;6w-^V1bZv-JVxrL!C|V%8KYPO|h* z{e-7yqU+;j?u^v08uGO}gMNVh8O6`k0Np8!{S#JQ${Z6XtXdf=$gm}R#x~vF4o&Sh7CS(-k&_RhH?nFtIkNlh0PLP z(eY6{NESW9>wyzO$`X;7ai{7pr_Qu~(H9c+U)2pfxHBhaJ%@&DHx(+7=M6AlNLG0o zy~F*zmKG&1FDmebeqsrJFFtr|u!M@!to_0Ll|7d6+qvyf?L=X~Wxk)$!kwI@eB;i~ z+W<6E1T^1(b_&FwMjfncp`!O{dN%Bhwu&bD=dhKOILR)g-I!jeB0ov~m z&4SrSR=bDHkk@gUT>JR(PD>E(3B%$E{6NO&EEXy%YU0v7(w>9sFJcV2xe1Ah>N`bi zO`>uYM*$!yYqKOPRl5F7ChaiF)(hK|M`ZtDa8JPF}#@6Ec zFOIjr$W>|Ldfj9t^$=9JR|S_>p|@xP%ww~g3JHO4U_RWxVEs>Wh|FjOK0g18z4wl4 za_zQ9QMbA+=(kl+L13#CX`)oAv4V6dp@pJ!LX#4DQL$`6Ktoe{F9{t&hu8q=HS{9V zLzPZwcV!pxoN>Q%&-smW|NA)xiXQdj(#f_IrTUc3HWnc+DoH)q3zv2UVwB|@m{7LajsAfcJ>c_3R{!g_6<>zN2KYv z?l*SMx@>>j4@AV!bUR&8)y`so%)gTy&pevn=Hp?xuJva)enLP^4Me-gkfZpiK#`Fu zMLlti)>`p7Ua~at4BCGhMQ@07vCsqGD!iBG`7!3FU~TwX)0Xm9Okg~!6s*iL<4R25 z$s~mG62@sZ&het9&zz~%eE(6JuG!K<&{E@ejDGoR2!}yQ9J*uAU$3Be)oc0;C} zI%%`tPcnJ#VP~%bzn=cff@gP`;In*}t+^ zRY72*JxbuU)*(CrrPD*eRtl!|1N~yEmhl8kDm#f5{fC=k5HHmDP6Jiw0iFP9VyV=g ze+g1asZ(-yqvW5w7E?aAx*tkH94ma25QoS6H65gV3Ic7JYmokk{XyD^z=qxeBeSnI zG}k9)X&}u}d0vXt+WG{@Fo+mm@R>EVL2@IUkW$+A#k!nd#4ux4S}3H?MqiecVa8ul<{!RuwUw8; z1g*7&lOIv@>KX)$o3Bqyk`2wS-%kspJ9BRDo4bjE-1~p~ZEKb4R{wA4jK8(WP%Ubs z^7KX#JIDG;JWJbaiy0Y@4DzCKtiE3W!6jHn6x7g3OY=5}%alJsqQm{pxRVun+maQL zLMNSOtPoD5zpqd3Xv&}0^_S{F{hMp}+J3>mRmi{odGq|ie=|{U_f`IPRmJ~wIse_f z!T)ur=REjb z$iVwt^`HpK(D?V9RowP;`Rb+fLWY6i8Dv$QlXV78)ji9%T*exQK6OWaS}W+dlnQ*7)$7>Q9n`i<0AuTV73)t`ZA&ji+h;`6K(wq5J=QPeXI}!d;qu zKmL3D#ErdYe*BLH`)kS${dBMQb>b!kA0N-mE-mrXgV&(JeF(RjG@CD-;`BXi# zX{YF&)CLChn@BAaEL9dmSd*)@db+wxi^(z3_!-R`#>R;~<%4%BDk>;8pReGJ*n?yp z#tvFElq{FfyXg(+>*I9Oo=8nJ>qGlnENenbs)c~rRElB*ozr4+S$Uay0-6qE)v3|g z*eKkYla%!|-=+I7uYQ37q}>Ds1_q)#Q!`hrQFR~AkL~cf%zu5XQ`UK-CD}Yf>rC+1 zoGz{l7j9x051{KxNH6I>O~pC~HZ5yZc)VnKo+x8v_WpGoxY;9*_*>_4k0#l@@Su{PbUh;OiJC}2&E(O!1PkZ*ee_44s zGFMAQb|rB9=|cKOo>YthP z`%rf<%`s7Dr701s;KXscz2ahGoKZ1@YJmn7_IGH*3!1;Nj9Yw4R@5mr&A6FmpgHP0 zDg+iW*;Wt@sUOg~KSyl{^aoEd1jZ#R1E-P6Dl6u=cAS<`R5eL1wE4Lr>QvXfp5!aH zRA@hke;D_Kf&c3uFTD>vZwq#m+i|Okc0EZE53M8ccY>~$YRvp1Y?~C!shVnW{}$8w zaH`Ef1yh1jZE#vUeq)?VFE^2u(>h#`OS2?cDN4#Z?WD&-qVH+m`04IkS*-oom&|X^ zj!Zp`!dOjC#zQdho-)i1od z=}Fw0WQzZlq`T3&1e|8u22*cu)cmhHMOuDgSl0-Hvx;;KGxZahP zmPtwytgD+!TsI1QPjSUc8LR}$c6DG~D+InJwB@e%ta$4sJnRNDxNRb1^J%RvucbF{NlNpRkm|FA}tV9~PqEM3W1 z>F!(62i?9%oowTmnVX(Sseu8UUP6gjS!Y$Nk}mOJlFZ_B*7dbD^VMj{68E`ixzl`^ zO^7Wm8yg#}6{C^Q3-655p&^Rt1hMzZapr9T+-_^_s}c7cK2qnGTc3+{|8`zIdk~tG z%AVKF>hobcr}jUr^k`V=>DsvyYcGQsB%LA?$L04T$xWF zTXjDHlB4vFm){VKHf($S!MRzyv?7f6H*&4Xc8U}b1h9*)Zx-d zpX=7j#vHkCTe6hg)v9^Zm1wka#$=pNUw!P=yY$)%3Gsr{Q?{f$cadjLBmJDc4|*dK zdEW6`d@mC%ahZ%S@$8ndfHo^HR(n#7zkjR6awddk>0}%i!(>P1nQkdBC`M?Q$c128 zm)DkzM!mUXDi{}iymSbCs!gLYy!mY1g2F4ah*&6lY_YJGQya?W2iqVH)udk=$(FZi z*&D53u-K*?F6F`1tlr47V&cC@2UDp~ovrZC=0OU`ifq zK%u|uyYuRo)Q|mgYsE?%v;8eeEAUz55tWtcZZ=s_hmqz0?*e06Qs%AnW$X25_V!hI zEYz05Gm6-xTBTugm zx~yWjm>xKtGzKJ!XDTv_u5d`~M1rkP7@T*qTkgxMZ$3VA9Qt_2v)lU-j3dvY%U_eS zDs)D?eP%e5QdGB5;yE7BLma#lWLDqKp(mBS-edjdp2N-NZ)`oe)-SxyEc@pg*Tf?& zJ1i>61TKSuS9;&?)-z_c{j`S?f**DNR{ zC8ae{$y#(EUN&~DDKZ&uUJa@8&h}h;Xp4C}m~VJ6ucQ-dgL&fW&kAty^Q*G?(j$|P zo@JGd>&%kJ((~xYOV1rheZMmh>f_^MKVF*z&1cZ-IHXghWs^IG$ReU2g04u|Llwlf*H}gsojQzDOj4hPA%Y{ z2-&(gWSyzTXq#!h>W97idAO6m)z#FWX_9w>Kw~ z>=iz(oj3Wh@(4C@ACW~B@5DrK9<;?KzJ9$}`@IQg@AF}PPSUk%W!lbD`6PLRO&0o5 zz4>te-mGr1p`=ZnbGBm*R;R$8(5P4RdtP_?Mepr20>ZjEO58cxt+B1@KTfW<0y4)$ z#l*;vW8!3>=5d9i3&BxOs}l<~?$cDCEy%sQ@NA%QC-nKTGqJEFw8&ix?)R{&$#>81 zIHTpUs!0lI;Ut{JIA1AGwRN!*itTe`w(FD=38#-Nb{M4|A`BwG-Yd~6FfG`>ST!YX z`!{rP@e@UT#9uFL`fB5hFL_+;vMG2`T1-sjxC9L)4b%yw@FYg)%^488@1TU!4TyS z97^VOD@@_yMr4Q8AYMy){5Y1==ztz=x#_xYsVS}ETYg29^JY4X7a2H6fTtHbep%1! z;unU_T(!EN0u2fRMK6e!Jz1FOG``W%(b=@ zc{5~n1BthTS3ZUvRgg}ZTA(}YFAW+tGCEDw0tj#})uPvzx0l20jSFqheO{JS{4&=$ zgDMP>D6V!Wm(RBx4=8aMFVM-i=7S2Eqm3c*BreTXD2Zv*?%Cl|fYdb-it7?TMti_V zLM^G4SG-CIrp9M)rJ{%5;~CoVmPn24_q`f1)?1QquikRQe~z*35iD`zZO?SE3i+1< zwP0fKRnG(X;dlHduQVm<8I@OYjXz*(K>wkyU8lHpSf#Z;Z@FKsX1(nluLW+w& zdx#jISz=OFNVd)Yx+5)(-dr7|BA9my>>+8oywc@GCln5druNF{(RHPM5SU^+6;#7& z%g5*H?#j7?S|5q#dFS=^iY%bZH!ugr8Vq8ctaZ2z2FkFB62%T_+V_nV-%60MeT1X{ zUQf2f)YMC~=Y(%oKMAyKg)<@K;)~I@uvIl@$>7GvV`@J2ZxwIYQ|1a15BMBa<}q}} zU`VCQn*^&-Ia0a?lDjiqr2&l$;lFdZl#*j&<>KOE{_$;#nB77!RBO$WbYHcgq>D00 zJ$zIfeAz?|VVbwjIU1;uBDtP^?%cG&o7F_Xb6jF#I;VF^(hZ#EI=XC$E!F+RW|&u} zn3$}c_iDMVtO~{v4VrZPt&SW&ew=m-{fJr0BNeXO72a$!kyCCXhV-n7p=sq0%XIeO zwSZ4+>GSz?4j&hr4k<@4GW*TV-AK5qb6Kfr$8yk^zfxzr&*4e;x#1obDZwhrF$2^U z+t0JeNRhvnB`Z(qPOkZtGwm}$f!~&y?{jdNnKVRplgGbr9JYSI|88l##prMoh5 ze7M<4cnS)I$%Sx*Y~^%45*=C}f2wd+fIMC7pfS2}>|ArJw?-I>zZcVGEF2-tR zD#u`TS`BtA!d95UbjesXyK+PvvZNf89Z@uv>_qH@3i^s ztF+JKSO0(o9LHqJ3D(>t>-%KbCc?#63_+YP58Xr=af>M1vNesv5-T&!xkSP>hk3`E7N0 zH7571h<@Wg!0;={xK01{cq2mRanReE!D_eFHQkFumIN_c25$M}SLBu%fef8eji6)+ zw-{q7#*@F2aOxefkc2>5r0%}@(a|{V4D>7K$*Lsll;fusVn{kjG{TKIk~(w?S)gE1 zJ{T7T721VtT8;Heo%Hm9)VWuYE>>u3COY1B9MSx0q-NQKnB{npM}hiU8pYV2LThVH zCKN^P625aM5mjiDcB746W3&d#)>aVHQ47n{qAf8;EVS=p@wszMhQZa9u=S`G+fz2k z$aN9)1&D)jNUk*3`}Mai?)w~oRs(pc*#)n3YdT)_yW0fIw88fu+{^{J6v^%6)-)e} z69vASf}EDLes>0e2Us0v4}+nhAu4v_(!|BCisu(jm5|%h(;`G{Er8ZJTq{R zpmXOiU4B~DQ;L_YYQS{&$nbE2!RoBP&<*oC=jNTLo)V9B-wFl^1-OFRAdbQ+PlTM8 za(bp8>_4@z|4exXSf=-gA9J4T=H}yT(h7>L4q)lr+Bm+{vhn!)09W*%dr^-NZmKb= zltI!a65?G&>ae0pLy-n{Tg)D1wZ(UbZkE^$KOfyzPrs+n8zDrP>e-rV?}$SLq-ys% z$%R}aV< zKxN@aSLdEZ05huoMs+ks$KqRXT5Xs}pkB>j*T9~mWk_XnWuerVEn8b27=RSuhF;lutOPF7<(t3MxO~`mRb0~mY5Ohm z3b2+`vY~?UDo@TWSwYw``7&Mqrj6g@+19??04nu!0dH)CAUaWwj>~-Ku@9fv<=+S3 z)lYeB7FDnyZp`%Ldl;RNw0$DIXL)ptjAJ>?qcD9>A9W;@Cj@rJ`=1WvFe9J|iIB~| zUMFISz;+6c`H!FO{Auy1?WLAUzf-+OqL7cW)CmCKUcU}OooLhl`PjijU!aNlzKxZR zY1c(Mk4+cD7mGcj7)pL}Q)EUKWhL}Qsr##8K!swilzfK$?aEIl^v{hw6=7VgT6}3a z@b|GkPtLVCGEu$4axE2Fr+2kqL0#Wv$KTgI=%jD{`!ZJUUHIYeFMr>2boW8or+SCJ!ljj^UK?-$W>_Q57kP@AIl3!u z_c<<#qUH6C!Zrgq(Z%tpkCpOrZ!SE;Av05fjiiQ|*!F_94Kc58iL5pEuYBq4MLj!0 zf3LGKYGiyIUs?-#Y4v<$(l=X;SdKar- zAt#on7=-7!<;!(Qc)a}hF#rKnbnc0;c_KAOLM@sqw22ceak}qRA)<2_CuNxNd5U}d z0rcx-Sve|Pc7hnW(V!?cXP5)fP(`@#gGfC)4!E+mmq95e{O?YP|1@xX-EbvMk>bip za>4fs154zv3nPt5r#RKjKYU|z1HQViua8yMrlybjS!J=n1(j3`%EmbE+ziXoRRPLj z6bK|%?E<-#@84C#5X3WElHE$cD;W#R_q(mMhCr49VY}@!sqx?PpvI)eTsnOI{hAYc z>l-+avT|gpGzK_Qe+Z9_W>#nWA7Uu;7t6Oaq?RF@@qo__>?v4>KsLq!!}NvET8EUO zhT;wXrS`*6i_O4AY5xviWT3`&C{C-gbztGy;!KIn?i|R9NnwBV>7xyL2+MXD&SvdA zqiJ&xxxj0oK zm1NB~N=NfBd@s*&k#Q%~kW0fU;pRBIk(U9^K89TU@k1kG9JZ&~U{vSSA z1$;2*8z0PP%bYQo-Lk;k*Hxy59}@^7Vow3C2C$%-qGYZ45a@@trEUj!o_>;YSYxI` zjN|Ob{fd!dx7HU%RFYqvJ&_+pueg_VDYgQLX*Ssf@00@R0Oka!1)zQ6y@L;Zs#1my zhy`V=lQWyP788&5lrVa6NVBxC2OXegh&Q&eVdx`fnV4j;@@kdc_8*|V_jpHAE_@s! zM5y81zJSE@9J((fBO@v#yCtIRi(V`u3KnK0Ay}gr0OYR>CMIfip)b)5DqY zeotWYIWye1xxS!93Q0_T@y6~zzT1qLbyvPN;3?R^fb@Rq=Co1Y`aFZV@FWuxFK9K` zNGa05=DhPQ`o;?IV)s&T-#oTS{ z>JK*-mKJi$#IMEmPKucS0bVJg?TxvS2;mF`_MlqzOjFVAwdgI8$J>IxL6(Wk$}}9U zPo5v0FvHaUeh{;fir>p9^hYP*$Cc6C?Xl9~*Gap%-*d%Ivu>Oth*G`rNM%g%f^Fg%k1LR?b+77E{mH{#DWt-_2XjB2^qzl?D z2NH8SG6rJ}oab764jy;>Ho{4JcH18+t)SCqOlyK=`=w@aEr?g0-LL;8@9M|yPjKyZ zvhFU>lTJ`MKh8IiVAC&T7rwmp{rd{k!G7x1pQj073I^24%lajoduMkC{gI|@BL)Cqz_CuZB={87!M`b?MIO zB+&na7B;Tp+G2BZjcZex`=#1&1WO@{T6I~~;RAc9n!#*eF}`KEYO;2)b7XWftpvcN zQE7p)btgbK>L4wjy>d?=MFATT5dq!G(*f>SLfRI?HLIskI1-tsLpn2a>feFToxO9v z{P-}BKARSK^Yxwc7>NZXI3|`Bv3~6Sk4I_V`<;YG7U(V+ZqPSl^LO5VI8Aqa_u;Ri z-6206<}_s`|50`RPm22g00t=W;f9Q7ne95dy}!o7l3R$sx%;+{L=JQR7ck+!6#2hb zu|7IN&zE*K&y>UIC%u3E<25!Xn(vsoZO^SM58?~v*0e=FehlU}k%I_TqaU$#8)EgV zkd7LMc5{-4un$8XEM#4<2XH0G3=mL(aq#!kXfe z684+d2amBN0~IuhKYz<8WAOLb3Lv)Eu?hKu z`0Q;`&|}Yk}w8 zc_wY(FjY^`L7LQsBeCkxcZrT$=d&sYD7!s{mMN@hc6szU1&hc*DGJV(rEZ)yLYOKb z5ph3_jJrH>VnM3va*-U6yG z0<<_3fb7)O(;XpyyWsQTUuF?9K0ZSC=kL)n2{xlkCtPpbK=%~o9H!&B1bz2L8-ipY z>>+(isx!=2$yBi5)w^PV*1mAC`ZVh)$HlM{&RxjL8#30M13h3LN7?U@IH3`m$8 z<%2mstem$fOH|AROGE{Fu^X5V?v9JT-pgp3ZCk@mePH&u-V-zTZcd>qaU_N-E`PvS zgOVfA+b2zhJ@EE&r?{DinLc2RuUtzVAMbB3vj`gZ6^e6lb*-(dtHbfgkJUqPDE;cG z;Ey+WBvNhpj#I*p9Ngo)eLXI&GoyB}HcS}{7~e+Iee1xn(1f6b3UaS_$1p&;mX@#g z@wq^Q_=5XMMMOeEH>D}ErN0#VOMGD7zq5*9lCr;Tn%AxBG`r~v_zkYIY|m6B3u0RG<^xUts4G{ct%#c?D44aK z9ZcfiFxEiKm3Sj=f}!D3&lz5|{F;SzI|V3v05o2fdHbr-pZB(>9JM%hkTYB5lgsj< zA7k{p#oZ;|*x^qQ?+GL;w64hpkrJG3fdxW=i-k+2>@9SN<~x1*w5%%BAI#9$7Z@X` zJ$nr!O7DD6g9Ku%;C^YGB~= zf>u+jtKoF30pOBajZDhT9xp7+u;VA$aiQLxbbKg%!se7^ebINfwfP5SJ?;?Exh+p+ ztyuS7+CkZBGnE1qmi7>@ZPg_8U<4mdT!B^6H0!0`vE|zONU9u}Y1Qo#6Z5aDB9{Hq zJ}&~Zxfr(_?h2W=@^ny|7#(zBzstAnRGNW8`G9En(qoqgIg_gn*KuTI8={+`Yzsf!R0B9SX za;K;&&z-21t%bZQFA+KD>eKeLMh)}<7)r;TZBK$ViN7l6-yfGVv*RZHF&seRWc3x> zS8Z%|UNxm|%}mh8u(+h?b7|mHQ%eb`8yIx8Cl!3|D_ctX$HvCJqEETt#5zkN{?gu&6T^Osh)I!Ei= zQnpKV%B|8mG7Yj!xHZT+=BZRbrec85=@xZa`;}L_K({r4*MkK`L_bT4srKqAl9k+8 zBE|060`5kyG;6Z!`!JVwd=dC{&db(Dai_@9b0S^$)UHc<6D+zv+8?(rMwC;Q!&sA`UoY5s zlE7WUg~A29dA8gF&&kdTUG*Z!P=Dfp_MLxlKpFK{O2KGTUY74VCEQ}V{UeO{su-5Y&l7lBhQVK2wzXEsQqZneeR*m*35sLJdzBO0 z!Hp(pJ;qZfO1+n$R#a>|1qU2BX!)`0YEQ*?j^*j?;v}UxJ~#9GJ6>|3Jnu}zdp%2? zxT0bVo*3L}Ad{e%rKA|Whwh(j89ARS(J>%r261a6lWiqlTP3w&yeFr=1D9_BS*37N zHmXwlM1tGJYQ4;~->u=TB*~MZVZo%BxdwFUvlqA z*Qqd%3;m^`^YP$ui@wB~P_Y6D>g3h#X~K#+q<4me=dX{`T8{$o?cs1L)kGu2dAF%Y+p<3{zW0zKCPKe zlQ5XR1>Dg^(H7mUNx@*+frr_|b;Ec4%Yg(~r@n z!QR_@ZKO{W5HS{bkCgg%{Ib}whT~Z8C};H&by-nyalHA^N4G*RNnV5ETCy$+i*1yY zgY=-M*cs33C z#AuP;oj#QaE)e{^4^KeYr#`nmS6hwhxlyg|*a+e>k%M&`#_F0Zm*k8lwXZNCc6!5T zo9gagkyzYDguL@iy<9BvPtkVR6M;mgiYc_90}tgh}p>3_kuFd zK*9qlvOYRCmb@zN^xB;V?a4tn9iL$b+5_5 z(!1xr>uNr1+ToZf(v{!e=kj-k<~*R@bZ}m(fN96q`5zgMhQ>Dc_TXN(KOrBtM*iJb zxd6*8W+*Wr_fiQD{v^3H?->2i;c-4)_4}WmYmAT7ztM9T+e(E7x}suIax`M%rK##Q zSbNz=tI6!j(Ym5R9D%Fr0;WwZ6k0|h?-hm7nkW>IUn+t6<)gE+nLv(qlR7d`g~1o@ z*wqrsAXbt8Js<`KoKTMg^@7o=pX68|GADhlYv>7HZ} zpa?V$-9s1{YgZt&7E-2Rq#!`;_Nb7A&=}Xq)T1q>93d(XAG!eV&#k$m zv5elyT<{9M*T9?CZH45s>`2#GZT|GBe|9Q&^j!st7!7J)W{mgx9pF|vR$`pVv;CzS zJHMSjUtMJcTp{QVeAC)LPEb}1z|;WV97{X{2?rRa(f;9;FMv50yfkI6o-o@9MYwfN zcn7IjsHOxspF3pH5m`)bk1JmwE{C#mf=L!liho!2D&jr}>)4~d?sxFEJ^6>{v+&>t z)X6~;4jg5MGX4>q0ATsgoNHfPv_TK#zwGzi3G+!61;Kus(IxV`ZkbCBSBv{(7I9>1YNKiL&}Ny{ z_rq*w+mdB7@XG1WLyZY0n47uF?}Xh}^=2rTLZui{(4D4LhM&`ZBLzqTK?(JX*_4%= zTnc~OuX%R&jFa!}ELzIaG_p`sJXCAUKly%+Xt`dCgqu;^Zw*7C0@%lCmV%^CNfvKr z61X1>+c!o#`_Wzb#o%K6E$b~_6OW6%3Q| zKi1UP+I&duF$8=EJ^E!m0Gz_l1tr(a|M+@%Dlb`U--A zCibA3%nhR{0Z;vc&$`g0Ib*O$Hf);$iT|`B2f<`_>L)nMFv+5num zi~#&q;S5~zsy^ma2pagYKam3@Kn&D|b&{bSXarOuO#B|ij+dDmD8Wn>P|fl2E?0q8 z7+jDVfs91%=`mh^CWMOTfumGo#6TKSpvtKohU{CVFPXADh10J#Fb$GLa%rmec4UAZ z%vC?cvZFxmRlrYyN=qhhr9p=Pfo%*n3o5D+vhCLZ41{hidIKhzBzGtoS2|cl%V@rk zKN&7LCnMV$&oa&hm{b^{27xzh_MO3#0!!;0EY(`itp6H7y;84`!a~hg_Wz6>O;^>Q zc@&`F(QzWB8fzg5!XwjprD9^`Uv25di-OKxuc2YkKHTVt`9F_Au8iUT7PI%C zW2m2Ws{fo>f_2BNtVd(`>aG75@eJxE{> zv5os?Qin&+h*3=2ok5F{o6e5*qq@{*deQ*r)mHazJX5sQIz@yebCWfWX zXnG!<^yaBfs(E|Q@+K+Sh(RI;)X?R>)f(00?)O8oiyL^ zgNGush=#a#cM|N*ywzs)(Fuw$T$IE^J;l~gXi8Wk%WI$GXzgIAn39r`4&2Aq^>vH& z`29Kszu%?$yJMgEP2=+ZExMk+)0b?#NJrK%sfk3`*e9>X{o(;eGw0k2@EbD?aLJF=ye#D5^_yzE?J#x%G*1WHy=uFge37?SLiS-P6q9e58JIJL0J9gL{?#An zBF$ZimlgY$w)Y*h#%V%~QAcDUc8YLu@i#8!nl_-pcz67`GlXY#3+rGKqdfR{n@MWn z#cV&3!-u($$%Lc?!fl3BX>qYO!AL#|s+gL89N`SV;_8c({7d61EqtsLEm3UGu58mO zx?^(XOfUfDa@ar0R9$PWymYs98+Hfp?k_LHX}tK;ECbuq)an4ULUK~oHG23gnwpw* zX4TiQIEZW2j7RoCan>7N6%`<9bLr+fzH&Qu%+BogFwSU~2)x$o)&j&U6+&lcN8t4G zp38-CamwnHdX?+y>5UEzz4`Ln?mK69S{Comhc$ed?ow<)@sG(u^Ky>Ruz;EDla&eFZ`s^D1Yjt=-+hIg&?Kcz;C)st_P3G|=k@K^&s zgTEM~y;2ln8kcjDy-H{Buk&Bk7>LG%cw?_J#3I6L9aG34Y@L}Q?(~^Ex}zb&vb#1! zC&$e{b91XOG*c@MoU?#X=)a?PNwn+V(C` zQ!ktd0T?0xQ@((^mhXIrZ7pWOLgmt*ArH7y$=84S8TdW2IG;uQMIyq%$|Lt zGo#diNgcBF4FS45*rymRtd}>wbxBrdX;>G62=Nc!I1Mt>Da+fc;FE9v`ruQ@c~3Xm z=}t}9v}CiTc89Y};`zqv$A%9w$8dMk7VI$7Z|JJofvRRs8Hc zp-KP$hXov3if0HY@}9dR^iyu*AN&80g{23YdyBMMF7R;$XzGJ8MR2@&{cjfV!zZFf zA)h-UPYG_a95UT-UAw{_Sq8qdES=q?()V>Mtr|3get$Hl<{&Qpp8bwXJu3;^ah6?Q zRH#~ZzxAElw^PaD+kC&*rcCt@JA`l|xdz5LZr59jmuLN!AdL#~+}JuJWZ8Ckm$ox% zQ|`%`I;BcvF?B-!ImL*`7L+_u>8@c>#F8I)~*q&H99&Uve^u?Gf~(x{Vo3OWL#@ z4PvF^nJ=I$lc_HGbn<>)op`o7%?T>bl#kGWhAkR??2qjqy5wuQeok$n##V_ksB?${ zk0B4Bv4U7>&q%0asm?W6b`#KBgG;ZhNNQ)vsJOUztGDxGrB0d0Gn=Q+IHR(%prlmB z*kgZQ&nPiZos1sm-1a$e-02jW+tAHD`fw|xzF%)3I=ZJ|*sQlVfXdAsxGbF@W(e7Py3=;1O@79`K{mj zDDW1Bh6}9H$e;rvQ{QpRrwOt>oO>1@_xrEoo7-2KvQEBz@y0_`ilq~i>DS~*M9ej9 zNWIt@==Ai?x87Kllao`8(BQ?0&jwu9ElPc9B(44J9Md_%CC9YT%RAD}CrdaNU87Am*CrZp>y8G9w>JQepcK?Y~)PY$gAfir2o8Zf*U zvH5J?EiyRKKH0W#Szm(mw#}?@;;gMLq_8TvE^cqUDIKWvv+NCsLf<1s1A=Fj4dGhD zsqQzBudm`o5w5V_Hj+Ntx!5F$ALdY+@OgY|Wa4WoAaPl&l*gQ&pyH zzNzVvr5!(e$M%9bXWUMy=h7^y95dB~Amy#1!bP2Qh15T(2#cHwJOPVb_vLD@67Tq8K)IIwGNv*(uAsH+TL=eU*4h4ldLbh^{45TPhqr{v0kD#|ez+6>gsq?fX_Qu|Pb{JP;AQZ8% zyB)2CIn#_cOl8`e)J+6_xn$jwD@1Byd85LmSx|p5f`}FTy0seI)UOv+x_l41@Ca?! z$%-v)%xPSe*Yrqm%Gih8el`x~Xk>{xCk8#Jf$z*De7<(=+LxM^8?9TzUMwEluDW)! zDKQ$?^OFC}JD5s(PD`*{jo$I@Sg>t}wma*{j0+qb9LOc!tk8xCi+*QD-m3?EhISUG zhN{n>=Fw`l@5?PMjo{L_lx0*Mm{M$tN{6Ijp(&Ak4-K!<4)j21xbw^a{s3P`XWnQn zr%b>(^17VJsDg=R7Jf?2ZE?9Xm_wOa5|tT=)5wVbl4@juTU8;ZwV12>nJWGXFiV}4 zTep~MLD53u=^J=U-L2Pu(gUL%ciet)gwT^!)<9uf4zxCOBLpggrJm|pN=!`G&P0D3 z7_SYFA9= ztciPzDzvIV@am^)YD$(8t%{?GE-wR^5bu)Dg=Oit_$^QAWX_4Gd#Y|$>FZZ!RNU%} z6m2WJjPh!uj_>S~J3V=GG9>(PmY(~{gvB9rvP43XWfSS8wl_y*(Kf z85#Wct+~+H>qCl2xY)e_RbjGb^cF!1%bMkkGb0Etilb1!R?+px{C&g`rN~s_h9SfK z*xB*wAc}Zf+f9EyR8E45{Rvqp|1fR`@38qO@j^CP@Jjps@hac9__vb@Yill3PleA# zd2D~YgO{Fr=64zyd;KxdM&t|LTqWunrj{J7mZF3=e{*3{EPVaVwqN9A@ZiN5@Ft*z zoTj_JzqoDoDuZY-^@yHgx1%iPN)BGx+04_~F>)?w*x98x-=C}>x2x<_%%yq{ zI=8lOh9nx^@tpv*oMCD5%9hiF^Boz3&Nm6ag7mptCy2gD$tEkNEY-w zSkNAM)+((yIVJ}3J>sAr6Yx|PDif;x6+x5$YZQ>-Q=8BHc6O$T;d2Gs$po~zroOYY z;#`Z_VCe9aVGtMD0hKmud{mJ0e{fB&^=r^i$Mk zZESF=Y4J}E9(xUxA#2)@-!h%UVra6rBK{7C=HA1}=ff*R?AbE8 zV?fMg&LVPx+Dx$Smm}ekq^P1IM?@h8Pt%~6eHUs(g|YI63&dx*r%#-zPuSvmH#`oZVatG>k;`I9n%^|=h& zomAR|-I5r10B8Q2a{1tDKTrDMAH#Xw?e@9Dn#BttR*1a}E5op9lPvqx#hqs~?Co>C zjveoOJpYiCsV#v2$(8nckA)aKInL@`tJ{Wur;uyUQBVF&0lG`C4d&qN+QezpkXwJe z1C8+`J?5WlVJM#nc=S`T4Dnn0ezFuz|3Z6z_gAw|3jX9-darbCTk~a1?Ih*P+kFQ= zAz?Gw!R^|zXk6m6|NAf1{8LO$BYtDw+99W}_lrF66i#7bVZ8kA7{5N0i`lnU>d#0j zPm;Nvfe~GuUNhM4L9es<(%K<%cfyN~42pEQdwH^*fkztegWlYdU1P|5 z8{^&s^V68#&o=9wYg zpp$x&K&F^C-^c6vg+5Ky#&_T}N(1)xI=62E4I#?P>C*MEeD%W%jn!X{GiFL{QBau& z4|vBv{t#pSF|h{SAD6%&Y?&GNNoE4%1`GWN?~ERujecirq7c+>a1%n{*MWz$SStFC zZq6^pkrp(kjxiC~czEta-;uxB9*;q-uXiG0fR{#tv$hHATjMb~@S;J_kFh);?9;|V z7YwdJoR?0XmuGnAMcMQ6VQG3(*Y_XDe{B|b6Wx-Y){~D;W0Ma{;{L(@(>w~!HQ9F! z?vhG%Kw3!*#wZ`^6EiHXoAEQu@!{_deY9_Qrv@D|o8SELt0E+Q588Go?qMmjEjDYQ zKvu<+<^JQFTJBC(YV)yyLhb_|4bdV9MImsppf^^Yjs%^bXC1z z)?5;lSI6`^RaRC`l>$W+>m`X_FFrz#o`NJ+0i$rInXY6drXEZ1KS_gd2H((I2K<1v43B*0g z$|AY!H!xuG|05e+u#0H#60N#vDt>dgWtykWyuKjGFKkdz1`htG>;Y#pRHfLc`W2jr zsIk*8o^r*y3XoD~0W@(SGqqHp2wXcE0wP^a(3$DF_^)F}LOBA%gGOeVjpzxZNcVmpvpIPJs zKhda;y?C_dqO85Wy#@w>fYT-=d=(02lbZ`NNN<&EPUsZ5y!;0e>40StO>igxB8+OE zZCKsWyY4p@E`bSmEAP2j*#~r`ph=G-9J1N7pZnk3I9?HWTV5eZnwDVKW?Fu#S(VPy z6WN~?9xiB;_%S+~bHV?dfD&SQAc1$P6Le-ylAHZ#EnAFkaVsJ8;L!sZD z&MYE%%|z^^2;=SzRLClsYNzk4&I!C6p3-i&UT@kUa*|uw=GN zNGZo#x&D&xmzT#yp`ZbWu=OL{x>Jm0UES#A)R-ZI6;h3v5OyEYn?SZNaB0-%Msxzp z1Cd?2;r4b#|G~y6aa)qQl=sk+!}L6Flf#+6M0DywlOow1(2Ejpy*Q4yz8`ydD`W$_ zA)K>-O?(3bHH&jime8v<;Q|gNd-v_lzr-qdXZp*77zg?_ITreUAe*nBj_%|mZkq~iFMfEq9_l@0z&NveCRv49i`&{(49Lq|ydY>ea*4@* z4fJ$oQipXfo9uHz!BeHbU|IpuVqo8fu|2lE;HQzLjSnmRMcYsgbohyuUn@Uv^8_?3*ba0HFRbOeUlgW2>k+=bh>oIbm4g1lG97l851f zwCSo1gx%J7R266ehr=6Aw5I4c(vX6G^8*^u(49YLj`gPZr4=RoR)8LB@}%^MAHRt2 z$Z9F3bE&r5iKw+z>H7IaJaIDIeLG$)0NpDk$k2t@oR)vp4fBC$2IQQ zzWaE6FIMg5EF>3gjC~(i=U-Pp5FH)WK{MXH&64=SBW-zgKyba*LIhQ%zohaV*Vl2o z8nWY?zO2rN&TH@_vd10-VkrV&A9edp?*H{P<$s(i{d0cvA20ksoXeg}D_!77ru72= zNlCL}Y3Gb%NFN8n(oLro72Owal+A=aexzS$x~X{cgmSp(*S+Z%+o2sd$3Xdiz5ND0 z_p)_uhE8qMM3Vv>(GEr&qDd(mNI^l#9QdGyt{H$$Jl$gK`5yT+dVeJz5{2|J(LP3R zJ~>tNLQHE4rJU--H9v8puhc{69OLUCT@>z-yTjV;@|{_Rr!dZQ!0p5#Ud{t^7m{JO zZrj*n*7q>DEl0(lF^?xq)Kl=BO~sz?(ElJw^t9E*e zQ>cl>D~!wKi|LUPRxg}dTIR(JFT%`euXN(z3}N~Y09`iOJYuUAFiRM9(wD2L(zYgawLsMmIMS<1oP-L9Ov2kbeVU{heRry{t+mZ@x zCZx2qC-hSm+Y&8S9mo>w%y~^&a_{!tjJ^jUu`<`<Sg1Do7mY1vjrmrMS`!>cjFXCP1u9tT>#%$cz? z2!`SdVB_WP?)ikDB+Zu!H`ugP=GX1Jf4)7;e8M$$VOTFXJ>44IY(&gbwKEqqG&B%y z28+c$d-CM@Bqg&Bf90e`E07QRQ?3#gna_ah`q^tLI#C=Y*EadJcuDj$>EEJy^F(iI ziAaQK$|0xr=Xd*WPaBaKNPDW+RLRI{LsvfHp#)tEwq7`p>O*j8tYxz*b2ow0U0#}V zhp0!08<+y3B+}H=)06pdYBE;3?}8+tNm{C_KSC*Rg)0A1Qywz-g*m#AhaL#2_zuL=mnvUf6N( z^0t0D;khmktVj*1m2s z7MPkE5y&VF=Ao>8*IOYhFJG)(_l1z(@f3`Vv$=~?5DQ!5*eytp}Q>b)N1*BLd3b=KB3Flj2pl=wC4vi{{&!U=pgneuUWVRj{#^0EKqP zCJuxAHnw2Z4#<#261s~G_Hl5CUnw|s&i7!D1kw(l1Ap`v_w^IS#>&*ijvJ-v-NTa(+NGSAmjMV} zd=-OiQFvqMbD{&poi-F zm(~7?TBY^Zn|^u;`5;)s`n2RSuU)NhTC%5*gzmB0Py?79Ktengu$%eqes>uyUE0yJvyk!~Gx!bk1DSOH>x-!(n5L3IDmuhBH9w3+%t9j<=<3dv?6! zN5%uTW6PNqgSojmzj7Zp{K||)`3>9g{k|NM5*+sS(C48Kdl5j!&-f$6E}wI!P&FF_4dwRndbNQ@iDl5 z{ZH3{s_;a@*IBeh>~MbB9Y|VT9^v&%^&g#tgaO@5S7EHp9t+`FiE$qdsCV3i6V_v; zT*?BKJg@i+&bv)PpZ+5mWSV8#&)UF`)5V%kP{!;UVGF~dndr8^#f zSFQQlyTRxN%${hlszwrPb?g@I&Fx9~nik*5a@&&D(W$c=HX8wr^T7voPRjqwTC*Vv zMxd^)4!UD2yl48eISmByOeOl{VoW|Pc3l;3zB`|_0J+;v9djRT$bfu)9BK_&bVjbD zAv2sGU3@Fa+#Gt!CSx7Q-|SB|^&J;Zv$(%038Ak{L(zM^#OSeEqB0m>5Q=`kpYhWE z2yA&7f1ZDIW1%B^es=Z={Lpv~&JXLDJ0SEkH+S5qAC&0z#Muhuge3eNj_m=dczw4N z*Yr}_*O8x%?sTEHfSmvI=K7;m{DeO&yHsu55s-I4Y;t%gYLfSRM-EGu!SW-xS+Keh zd8B59*wdq9pb%~cJh6!08QjpSb+$@~_M^%j9*!-?Yd z9NLYpgxrVHAhU2iKm91uuHeYme%{Dw1%IY(6p^y--+v%KE3sieA0NS#!JWLe`Csk2 z1#ekzJ9iz(<|<}Wb=ctZxaHlU=kGlMNijGK^s~}UzkKA=?hTXQO`Y#t;#s|Y@#Sxd zUhT7bsx5_1R2lc)Qx=upIzt!2>MyzGcY!vGf9KAiJ@1lHZ~2}gD}}iMn|24}t~aLM zK3u)tdxf-IB3WfPH)uK8`vPtEYz23GW+a8vN~efR`u&qpWypSGjK1#svzNNOcfQO% zD8B15M1_$tD^y5qVRMv0m1awW`Oo_^NAJ2AW`jwhBlXO}K?y7v47YhyB`WC^I<`08 zVXooLyQDIQtjv{y0Fk$Wl)wubRROT)OW_VYEDd?eHkq z)?(W|HN@=}yacriXPPw_T=FeD`PkYMN=|7t!1pOb4Edj-L85T?5>x|n7zWv4(*MV^TjI9 zk5DR;lV#p@=1IWCU_N-BuY!sKpqUcFebSMtaPb8xl2*t|xQN|>UVLGBd(#*2PD`cg zqAT-CN(w0s7AXOmnPySHz<1A(A`P40=MMeco_nu8LfracO!?IpMXm!c6r@w@+S&{T zax9=KKgW3EK_<23KMBx$)(rZ0gGS8!u*S;rGi?K%n2(obdnRvf5WZ})ud2EL9zn=F z;GJE4!3PEEq2@UV{>%1DM;)ZAD7B zu-8wPte^NZ{d7PTF3B9vfJD8rue_KNz|65Ikn((35zG+-^v{6ReS;(pw_|aY0!4uoQ{<(fP&UC%S^tpuzT`9e`ed2eILFA z*;o&GhxSX{@-7`(Tg8o9s3^)tW`@R)y2#Xo!$Sp+r7S4ZQXHi1P5NlV!~Af9mqOz5 zfbxw8f`Wp{r8VuR6cq4fI9xnyq%t?`QLdK-xSLf z{l>6R(-wFg-IPrTp{N}5dBAti$>Nl~G+{S03#pv#^@vq+dZ6>4Zmq5Fwo@u078>p+ zxV)f%z~}z{`}Yc)g=R)&16$$&NIkmwM|;)A8X`z&T=pvxe9S8VRa@!Mb{S{@N6Ie=TSN@pcFPN=J>8Bq6j$0ApNvLw4w)mJf z_Cr5DKY!#!-GK zt2`(28p#^L4OgPhD~9^(ivHhEKxWF~ z4YJ^BgltMs_A&(z_Vx#jaTGu{v>M)OgfqMc0w0BjM)D)HGx*eGdQRv^M$cH>EAVxS zhBQc6SXcu(iaNO6m@Uwu-DXHObF0V!34qM{!ezz%KfOS!%}|v#`lGB09sO#0%&>+) zcU=fmKQ1V!jxBIv@}7qd-q&tNyXORN<70SH#JLz5-`C&^JpVQS$h(Zk!j1Mwny5}p40+h1V|(FkLHI~Jm( zusgbhegYHAk3Q-92wqadoBv+cL-=eXOi*N)4M^G_bXIA1|WH%6qC&&hvKD4{|l@eCVev!+kzQ0 z@^@&IIchQ?)ij4s-pp~c9D0;*j7}+-zP$aA-(dxN9^kV@>kX?fN!* z&e5c7@U2y|9p0nIRQiTV-@?4lD$yFLN7Yw-Th?yLYWXKt{6C_`SHH}@E&tD@X@B8yywv~7k z&&&hd4ZDdU;no2LnzfBbaos<%0~R-u#dZFy^cQwbQ_DH-@jIk{kuE)WQ*n%}6UDU6 zt_eweP&ws3bjPc+LI|>|!ko&VsK1;Fy$DeFJqS?LhVD@f)V?oo{T1)0vQXA(9@`O+ zZ+rFZe$kja!zSY320q^Tnqi;Gz6^hOG4>o zTQEXYrv~(b&z+U^dHu$7#H?(P8#v{Oh^TVA^qg-X?q^@|ua`nyWn1HcnDb>9O+`SV z;9J&4pezen8{AC_n0@)tkA9CWduA7}oH-hJL~|=}c>?)@C$!b*;24EmvQ964pk&gL z&fF6}`L7Fgqkh1(@wGI!<9H zlg&BVQ2gnKm=y5;DsEaFfx}qHI(D43(p?duq?Up3N3qb20N{UXCHGfw;H5mE|EKL7 z*_+MjORUp~P1}p#w!4(2eboRwd3~d~pe}s)sU%@WK)~essKk|YXg|Ku^w%9BmWu?(6lx`P^f z!mc3GFemvOk(E#7^-zK zL8TrTu9)jnP#ufRj6{r@U#<;9+S))jlB!7jTRYH*Q_&Jg^i>C>(g?>~tpk_7yTG%* zU881Z2)CuuK-dGKMdw)BSL|X+r>KAHx?bXaWdRMiOs5SEVlWDnzcRNPZXkdlw8a2? zI3P6u>`3+=i=z-10maMe5H}Nn!12@5r;&%CaSS@wFJgiA7w5b8`szdQ{LKhu^$k9` zv|Yw6m5Z_g^E|npT{P1RA8A@?H?7uTdJ#w+4eb1=7ivV4ot=|C;;7X(xtGpBUA35b z#jj6mLnkPxvXqtnZ{c-BumaF`?|g{{_J@PVpR*s)(s$X^WO7deNdHLDTdcA0%Oc#e zbe!Y^c0Pw(4a@;F1^NghZ;2hgs%>V~Q&(p~*;ofV`S{88f9Ca>am4^rI{1Z(gAa4s zK9?TTkQH#&hvv!S3JNCs_A%ockZkQy-Cx<7_EX8f)2nJPh)g=rrK@!|Y7*Anh(*Qi zSdZQcUjse01XA7u3}SdM=h~eW^_^+vp9DhYnvp?xO^9J6M2z1P3^$QK(@zY6P> zALdnTb=I<`8godPKjUX#5?gPM4yu%tBL>x;qjR1(b!FEsA6fqz8DyD28qB`oXp_}x z7~MD;^mqU>mObYd;*xZd!Sj}NALiO#UPWyppTeKM`Suyw8Tu1VjOV_IvF4@j4owXc zkCu<=uPxE3iIr;QwCf8mE zm4OHPS&k<1r3LC&q@UsoPYdaNgs7Q1!JD3{Ae|-(eJjCd&l-ft1I7;QIgm9IIbU@l zpZ37R92{u~4l=fFC*$L**Q+DcfyF>#RpiS4{(eP2nX@NPp2Q|wlrxV@h0ud#xk5yq zs1g6%bX*0$sBGaj`}Y)z?7Nm~5YGt1HdnqhMTx+pyq1~AkV4j06_B%C7&HjJu-I^M zDx0^U&#uX2I8k!))$5xei{x1!;mEekH#OR`sd_nC7)Ee@*ab$R{L)@}VFdu2;3;{W8^Wx7w`y z)XbTCt<4T`(^2qHP2ouNx6Dt|GXixGDS&1y&b~<_IJRZv18bf%8Axd_GtvSgKuU+q zsd)Q|dMHoY4xKns4@v8u8t-#Bs^Ug{y8|{E1ngQ^IFcs|%3H*-2t-|Jh?3R~S3GQ# zmCm1m@Dp5V|IAOc=lOc{mHd=+1h=mZa%k92aUh{N2OE;qqD2Z7)CZ!FK{lEe7I9%F zGtw5}^qe$dlrRnpKwL94Eijx*^MpqSW;PzwU=%cXtY&gxh>-UCz1$V)KK*{ulmP#w zDWm?18AyFRV<*3i!I7PQJhoS8u0QZ9BwT8H&K(w(O&fD`y`6s!e#4p>1tu~A@;5;XqG3U==F{swvb z|AJU}zBPVtfXt0QRJmmM@ub`7Pyi!{JZNQ1EFMLu9X*C1G=})VZALklwGi|MAwWKELx$*-*6`L;>HrA@} z0DCIeJaXnJ3%GwaQi75~LK(h1z13@`^NfX(kLFLzx>+*pJ6TZse407JkA2 z^dV?E%)dn>PRBIB&ECg2l6gtgIHxsw?!aP`nOmlJb}S;!j-4Ih=iIZyYofhne^h*Y z7Bmq!!N~-`Eof(!IzRjxUGu`7d$w$Xst>x{&jC#;se$kh?qhY??~c4cz%uvs!eEaU z?2icCI8LJ)b+#EGN`!2!Cm{QIgIO`c1um63QnFq^6P#$e^-e?p2ic)vslEUL>g$}u zBK)4tvO5+*b2`;nhTz)E7^PLVms~ri)d(5a<)(Or2x^}N2u6>CiUm2Vs#J-77ijG2vRdgO6#Tjta<#|DDmRAVnAy~zI2H_X^Sm#zCyVE?vcY- zP9JMde)VQi7L5*|$D%Ha$${? z8AqlPa0-D|lq890tb@M=$_T`T9-2=zu}f2(sR`H2_%#fK&@XUHQ>}m@J22)`BK`lv zT#G~Go`FLTARx_;ky(UWP&2SMSHvJx34Dp5veG-UO&uArg^|$!hapPKca;_YtWaQxAWj;}a4_raW0H&7q;8h*Jmvdw`9IF+2BvRVCTT zBW(KJdmrWA!I4CD^UQk|DGwj+V)>C{^2gd=xR~Kj;;;e*H|O>P6>nC9h{wz@o5FBl zfM^sfIQWH~5rEc`m8Y<<;J9<07&!|fk75=mux*;}aUIA`v`W5t4Nn6i3Rz4f2KdVW z)qNQsJ!tt(-@+=#ymUBRlu8WT$!SX~<98U2zO{Od$?IZ5lskWn@r`yvcIGcYu%*8*Z_BEkqjRn zyY#J_@R;dE^e>95n@u0S+rEjuaO%rWewCg#@>BDF0fd=CNCd?mqA)gUco;4;Vw80y zb>^UKj5ZwQ_p*D(j!Y2MVv~`MD^V02423u=lL~=eg>$#{Ln_g0MF!T7JgBdn(KzXrJqmCx74+HoT=0X2y0d@O56S*P&qoWwR2=YS_E$ndw%DAM z_suY#FLK_Z)%w3cwAuyI!#i9$k+=qyJf5>rRt0{Irj@+{#|@lLe#r_$zh;G$$=mj3 z6<3Ifa?ZWU17U|RgA&oMDKTleq&+UVoH=9AqkE>i%ESHv^-1hv7ODL9E%f^9wNEZ6 z3zh!VUtIdRQh||xu+>^;kjAL#LCg4dFUnBb^Dp?g^yHq)A8jxi&b2lLVR98TE4eLB z)qNv1xIJjQqI9PXIr`(j#-+L{N?m*ZXnCR+G=cPu0xnl+IsF1sxEM?Fe-AtLrz)$I zRU%svA};0EnC>aMyy52sDj`;*!~n~hUR90HBD3xi_3qrca%9hT<_%ib^=$tafBf;o zAKO4YZG>lMI`-vzuNcuf%7k2L#&1>3e9jwQa$vVT{^-v8i@JfBD+}SqBj2}e2_sOw zd{>5tRdH(z@lS{A8yA|U8tQx*bgI0b%0ltsTkS2J0h9=bwp5*>&VYr0#ksff=>F{r zqrZo@=AH|3ruYZ%>9(hydp-u}-`)E*!1+w+n&@!2O?>ZkwSPELq!yZYC4cYZtZ84)}F zkAHs?&M^3Yc|a`Om!Zu0%NGE-CI7w)$SC~3eHZ@gk^dvo>Ttzu!;pu^=Z>2It&|L$ zZU(L=4!H`xKJ*;;-2QdxM{&J!bbD|A=eVu&iv8862W{E1z2$n_^v;B6{tM3e&Z;~c z*UOCcw5gL1yIk;N!o`Q^0lEnO3*c#YyGePAu&9cR9z#ap($j+)ZF0M z_Ti}M^XcC_{68;vmVpo(Z{)DnPtE5ZsLD{~&MmAmT53nB7xLvdwD#` z)5PZKVxJ>vY9$z#29h2Re}CkD%_g*-*fK-Ozi{5?kbGc(CHKjrqJ)y`ilA%_6AX+; z1rZ~SvoqTYNkGB}^I}Y;@!9HyqzII2yDvM#Ny}r&v58tE5M`10TE6d@f$k|) zwe{C*^Bv$wo<5{^F*q~xu=|Itd%YSNfy~M~EAKAX^5T3hgf;D|pC3T!p6CJ>HN75n94QX$SVQ4VpW~jHL&l>Bx3bcSNQ1_ zYlH_coIN^ZB4|0pBTCLaY8z`y8ra1yM^c59^Oc;+%?A&x@=*HH1V${J^tS0)6XW~g zb&$T5ggs5%Kdq`NUO&SzvJ6k)j;Fa*2=fIjUpqM6ExWPCXq+jZ!DeP=5=>U`{t267 z`SY*NvDwcjYnTMgz00Hb6SWD6D%_PgO+%x?_xCxhur#+a1;&WGpL3Z;sPf=!!Dg$G zpUy4Ih^+jM9o)9yVxfUO7rN|Ky%A}C{b8?Hk2pIsW7wB4e9JY{Qiq|K^>&tOMOJho zMDPW$)-=6F*6$5on;MwwXNMY;Z)_bTR6U}groH8Fo_$UufXDNHTViBKY@W@;P3 zUw1uNBK9MpGO?By^JdE@9xB!(Da|!pP)Ap1ZC`$`aa)xjPCGV!b#>K_;P3;3!Qhk& zxNS`DaEa_e(L2Pet^CDD+)MwA_02}juY7QS-@Mb~qk_HddletsI)SJLc(s#SjTX$_ zI8si9R9d3v*+k!|r|Wa=22#rpxILx;*60<|kSL$eRGw|FE?s=Hu&`XsHkzii@BQW4 zgeNDhYvdf&BK&4n#-y!l9{1i_&{3dgNiWPMg^lEi<~YwUWZEPM@dcg;+sA|D|MVlK z5FdE}7oc3+cN_fXNoLitAY{=Kp=uia*l5q`&ECWgVx&pr4^=jiuaoA#XOww0Lv)CpWw&b8xJg!zV< z87iu*tjrKqTpyh~g-Rj~*Mur1!?Sb+WSEuao z9(tK5k7BzG%%d2ok+w&UqlK0l?yPO+^1bNdd4oTQCVP2#?U*_BOR#v^xR@+;4{tnf z-=QazaxX0-BO_So_oozR(@G~I&A1t-W~yisrM9z$CUz4kZDnPOJxjEx?(Xi2r5f&# zZoL%KBIlS5=7v#sq1ka{o-FqG;rDPV7SxgPSTESg4Ei$5u32T{@o=qQBvDUMDMFPV zq1>d?f)}<5iWna#)>~zCDQ@%WQGq-#aK}`IR?$?UZ&6>WRT3{Q^%Rrk*~gHFN+on6 zw@xrbG40Pl7l#?Do=2rz)l`9WQ-dP67a7?j&h6_|+gNq(n=KvnBg!-VU!JsUijUC< z7qT5~h;bZkt|%IFGcYhRlGR#OFEwv68B4$+yWY-yt8_djZt~Dcpd{*+`R zFH(UMRf3XjIwDGV=h74_s4o)HT4hn5RnhurqPa@Y%G}0WL^j9Ha#Gg2w?wS;?f(4U zqD6hJrujKyzpv1k)nZgUrp3yxw9*J~EF48*uNBZ+9}bWA*xc^B!bccx%fO{RBg?Oz zH*`!{%V>8ei7S!%BTe1w9`nfOd>l$u1bR75Ki~h;oXkX}$m)E|6D8DFGTG|9P?m}8 zkyfh51_DGc9QN|;E^>)Gvo_T?yq4jaKvrE7qgrCl?NeEP^=x!DIe7WgnrAv4(^y(w znVIOrqZ0i;-d~@1ENx(B7ROW6Q#7}`v8G~?*ZJ|Sq1v(1F&v3YZSxkq6=7MAtCl?; z29$<)1?io<-nRvH#(9F+< zxFeu3Nn?ph_>VSp2dt3eA~lUx8m|S}jp1etSIVqD%GVx_nrKTK!R{vOLFCq#h*}u( z>Tp^N6k%AQdf++M#Xo);J$r8vUl)FM)h3;0yoOZ}d&yuXKA&mnZx<1P(#2*K4F0gv zC9%z;qjb!^S=Ar4zAW>u6w@`Nry2q2gbtaPeRErk`cf<(u}FqVODnbHwt|?AM0zBs z8es}FyNfJ-v5)uiE}{{2!Z+@P`Oc~QL+9gY&t`0Kzn@ZZpXa%Q&OP3)ZlB2Ds&Fp$ ztqJ_H)7>LO4-eRvP7j3%rlwkyKfn*g=BElj(@e=KnhcvC)RH8-ujP}8Ph9cYu@F-J zTD+~cPtVWK_|s(r%&n|&#ViHK$XMG*OGQG}Qny&K4&(&^OI`NtOqggZwW0(YD8V4s zk74L;!*Om+Eig7sN_3c-<@Bn!^X1+y%O%21cwuTxf@UI9lW#0_obrrNt;sap47AL2^D`2;iGi`S!pram`q9O8>?jKr(sZ_Ik*3Rm&91vP z{Q+!pHT#1l6E`=CR%-C_M3QVoGtBrz&nC$sL6`h5Bnm})0v&OO+t z!^+!NZo2ALIGg!cA@diR`(Z+X@pa*84bv{4)|-#%L^91XLl5h^J2Z1y#h_oDY_pT9NOhYG<5Ow*JRa))w;@lR} zlF&|%_6uaDwavdz3KP)S;Y_qpjX!Y&LV6P?X7${;z7O1P^@aBcWF$lVrpRZ7nr!aS zQ`yk|bZu9GgNa>6ihILTVjXg$%-q(T_I0{lf(Y|`aHFB=axD)|Uvr|j)L^D$EwLRI zQK2t+eYN?VU6+SJJSv5zov>=sL95b`P^8Cal`nONn6gNA%nH=}DS0D;rc>;j#fdhL z7J;bI&7BWMt9Kpvgg4~Eb4*70r|87V!60i=O^9o$3Rjz}I7`|>Jpx?MuXne`L%Lmf zSt81#ZUi;lR8DUyxdk7?x)!o7<`fH$wtHyqn#1%rNZ^o(+F(rJNQt-8Y`ySZ{ikrF>hF@)K zg^jVa@>84EogE%OZ&mSI99Q0K5BL4>B4~cuKo806#?l2fAWfeimL?D1kFty%veX$F zHFMJ%m@Xp6(%hm|dt7ncZdJCSj*G2MffJ1TfZy2CHhWThh%TFBm0LzHSl5P-2uo@w z%osBXO0K2CNHV18IsW8Wn2M{b+b8z>>OpaPD=9n@9twXtmN)-MG}WLsbmhiit*_Uz zlLIAF(V|>T|CWx6g@s_?`YrVehB4G!k=wfqVv;O%Y{$>Bjj^$O{6bYG-$Q~M>$`cM z!=p8WsHLN$6P8{4W&cO{=-+d3x5TPe#}zaKneZM3d>_gkAXSTz z@zsaSZ6zU{b7eHnmYCP&S=Mp z`_oYUjxa?8vNO#fu6k#@?bcY~TUQQ@7&Uk5=g z`5jRWB@$t0X3$b2GEVs)CPm2GOhG{a(^VcRR+vdYMqE;jnA-WF{KLXOHf=^9$2bgB zp|ND^7dvMPtos9K*#Vd?+p+n5pZ1(hqWbyMLK-^d-Yd5U-SPAy0#_c-EHVY&VJ*UH@M&!9XGSjax?J9TTKKOa@b zoQm*}EIBb$6QTz;7W1tnNqG~==eMEeo?E0B>p$TaCsrGm_yruXs#+HoD(P(K*e zgmOn+X06&Ao8G!#;L=kx7z;(wGZI5Minj1Nh%Y%OMCD%@uH^{69eI?&7r zWk`L2*4Yf0DFw{rd@JBMP?*R>Sq9od&HYlWa2U%@9Z#XIH{-z!tm|62i}cmCX|7Et zNJ#sA$NKhEt=1zCasVxKcmgaN=hkEmWbaxC?hA}m!+E$0>LkV|_;jDN$N2~PxwVMW z2<0=MmkqL%HS`rH%21Qx$vwX%iEJvpqKEjZxrK2d7`px4?cFw|wo#4OEjU~Z8xMdW zNl$HKQIu+B&2Tj-S$MgTJyvkyAhxgqX6AIx3QA0ok#0dc;TS`j|xL)QxfrS^WbFhz(+1p_zspMQd-P`|3 z;BvZpOUVd_{LiAOHQNp9c8SkN+Oizh>IE zY1+T1)L&!r?-ljA!~EYH%HJM4|K3mk{~b;s;l0^CoisYC`ged{j{F*6e|4BlGngD( z`jp|p&zG0XKie=|`f!0=d)$(wjoNo*`b?q!R*#l1Y z-V9QE@4c});yGsEp}CD(s7X|cP48t?o!Ir{tw7kc}w2bmZ>aM&k6HAd#7)20n$Pw%brse*GX zPcxSY?daKSfgZhczB4Nv+=X^7Xmo0^Yg9&dPntz-(+Zj%(VDUFCgu`Fb;zucoIuxZ z9A^%Yww>ds?zkb#N+;uAqBe=gaLXEuv2y??{Qx;lh@ad`M z5@URX3hWU!8TK0fi%(FtO`LXgDdwJe7cNOY>CfxbaKl7XjeJkGZdynZuDy?+KfvKBY0Hn$fxz4 z-EB+i2Q^=+9uAl;;*l`r)hh^4-CyZv7gvSm<{cpD*P;6Z{SBKG=@n2OtCrLAJRte@ zsUCwuYkI1z*0OZ!%|hP1rl@T1W}OgjxR}VY4qa{hX`=H*wKJLTYT!I^y>vJqM&5{O z9=FDOD3&jfwbo_L>~kMg7_*DQ%{&W{?>=!rQ2TvH&))Yd7*!Rg_2nK6!Mu2l#p0y^ z<+&@UQr?eUyx&$#Rq!lNX|Vd3roP(A_|nXWLd1qI@3hwy>wP7P!Wc0Iu~ z?HDYHP^!r7@>2JpiOt7nnVGGCbHHDl8+&t~>1R9x13&vlSrmDMYMvl$kZM@q4VcXY z^WHwrlIykYhEe)fR!w)+W}a1|Xwhf-6zzr%T1!pU1%>lNwba;o(mLNSMRU;h==qp=B9awmrmIQG=vtoH(&Aq`b^x{Bvv1ZUd*%(!8bw`?G+*PWT zOWO46SbUoviLR%5lH0B1+U8R40%u7Nc>xFkR0Tp<{d9pReEY#GzX^XEICHvVD4=@&#hL;OGMd215glMguSdw+~0gNMw-5Sa?Jqk++Bq9_vt=dGD%3 zSMDj~a5SnH++D0O|Bjd?V!I%a8P3n_!(c#WF#4c-hT9F>=(yZ? zsxjZYKpx8=s>SuVl~sLwlpV^WI9y_&t3!lgA8SfrIfZD>EKJT)rR;i_*F9^unq0lR zy%l|rG5^*;&z8cMjYV1UB)(u%i+zotE-O>ojgh>stF!YjeqjfvHk|HdppOvX*o{moG+{IWD)yU zLzJq2Kvq~hW-f(ubCuhq*gp*TkF9%g{&Oi&9?Nc_nmCtY@-88k96gj5nA3*`ghnfN zOVsO@&milV{L=;Agcf>bRFviBg&jNrSFkpu%^v%{85LNP8Y~JEA;@{xrsB%!rSfZh z0XKH+>6JwY3_yS7wEDvJR1&HK(KbcnJ!(7o0)MsSTyjKkVPgNS3;RwDH#@f!V**R* zft>TNn$-yi()?=OzK~+9gRNlpuK`xe<}opXvLRUQtv`rf8=D)@Aa{5e zF5obs*?=)25tMmwJ>VbAnm?X)-P~ZDkg`F$M69$6K@}_J&ZZK zI{ngm$IQ}#dEkgq3}}{AHx}ERTb)R~OVl3w47DM${aEw?UG4dm2pxovTI!wwxDDznv!}egd z>E~ZGk?&Q#AY(IrZ-QbSZZfDyHS!ADaC7A!)@c2*siQ!)VUD{BL|je7F7pX;J95-WpiT}Y?h0X+rT4e!p} zBP!Q`jT|jGpr?yWyK6*MDY7avpqeYrtxw*qV#TMP zv#3gR8=8yHMeO?It8;m-Ox{xOSq8$gtWPXTOWiESrO~~&%?Gt@N)FXxnqT(UrEN-J zl&2R>{M;y*^4`GO1gla4VXc%#GYr^18+1T0dE@Nv6!RRdjA!lCC8X?(R z4neQ9Dt>ADU_CqxVksJ1=5AqHa3jXOG=4%FjPr^IfV}UcUTQ1W!!6Q!gsOvxmnR}c zJ3R7veP^FukX~oVyTd7pGVAy}xLv{*czpu)^<#)rFZ+so$Ddxv8+aWwI!ThySo5BC zucvQ5i7Ax!XbxNx)IeBG1c3ulgUe$9>SCgJhv3OJ7i@f zk@&uVoH|kwEYY+)$2S~(Of4*l7nWZO@5Bo zZ#ehx&GC<=A1Cn;F%qO*x(oIwet0g%s^%Mpb4RoQ&{p4AzyZGcPRuFvkb=~O!)Wb& zTm^1^t{ull+!bo$Z(euoE++pJbod#w-(L-7?%@K-9D>ri&u(3_|1jfk)0Glczv}{D zMC?9X?c64DX+~M1-VyU*+5YN-^IJK*f3`XBtGE0s^!9?qOfONUfV56yN_%@=Szpf} zR@|=7%)~M0xU0iV$w=(kneTo(`{y4&@aK^m=wWvszTT?t#vGeoLbHzT8B_4!O?*05 z(Zp03aMN+JD6TLpitgga+Mc6bEhX-}_Je9--SW5ZwwaW=s`&b{MLsp-BR(C$ z8!t>gi^?s`9Whnk5t7*+j_xcZwzrEeuP*<1pf7lBJJY0KS-Ra()>=L6`c-0;b5Cyg z)(zHogzt5j!G`C-lkcVV*?{3c=l@T zFf}u4Yp4{Kce>c)QV$)I@@KP+J=U6d;t%s^5dA`CUtZD`5_BIlJld9~Yui|MYH(1# z~1kbo9h4ob*^zOOn5QlJ$>g|6s8jjR{1gR>}qCCaE{4dYyu^?2?m{ zrTo^l9&?_ISFN;Oo@jRvtdvU7JJ^t9(IY2jS@7g1$;ZcJjz7r=7rgFZYcjUhA&U|| ze*A;GX6EJ@8H3VON<))VzpB66n1JTU$6QK@XE5kTo#WI~n2$%pEn}LfILVsYo;bFC z-?$jJin`#W3;WrBI+IirQgVB4KZ8aYTX|Z2StKIMG(VI&;1`DpTyn@}OgQPcRp15L zBJP+bwTyeOeP_q+g*g+fjz4dfD|&^hUaZw=W7_N}b6dvgQA~^ihomLH%7#c^nYX|- zR$yY_r0^D0NN&fS<|jWU9$uDtsG;FG z*?c!iNKeXT@cZry=eLftw%=gq-9a+`u`@5__1lJMJr42YTSGN>W`y(V+U%vScm4=% ztIOm00u9lMCTbcts6`>0B~*K2DoasB3i~_EO)tgEd|Ps|>8!%`(`Wy2je}$?wvLbYHO(8mZD$x7?n2Bv%I#puo0SIu3PmuDQTOM5B-ICwq28xG=nj& zh|0QeRNt_EpEG}Kmb!JrcXT3c%SCO?!DYR)&W(+OlT?Bp&^E%NlJ<-;#yTV8?rmSc zRzFb3UsT;-dV z>kJqJVLYOszHBj%w>YFSBaoM$pIcaH zTNhXWmH8;-Vd}Y{C3S-%aegWy7R1EuSD$n!Y)s0o{;GCG zuDQSJqz@>vZ}+uZo9_7j!{G4y)JC%#oXx-C$O-nXT0_g@ZRA6;o+qLttn5Z&y(&&R z3HJC*4OjZ^zH*9=Mq4#1E0^4!U=B3%|NbLO{Bfh!{DK#otO|4S^gVy_+QPxvhC7>& z56S)9oDy2)L+3>%mbXjK2l`kLE=)c_$-9{>T2FE!Y?x8a-}R7!6U^kUFZ?58x$pm| z>bk?4%D!#X84*En072=Z^d?dQ(gZ0YRiuOt(m}d(LQq6HQGo;uO^_1lRXWlYLRF-M zCM`f{(gLBqGw;p&&3pO2{F$5FlfBp3Yu)dxg^?PXp#@%$I$;PO;IVn}KKSeEp39^* z9qrSPRd6f45IzSM{5x`L{9AO1#XaItWmTCSaD!Z!Y>rRNnSX%n9&0R=;-Ls*TU)R5 zfih7EYT~L)B`&L~9vJ(-5Daf;jt3d{+U}OkO60f+XAm8J+e#XQoJ?aw;PLF}L>~;WyYvQeLGBJST=Qt+ z9~Z0cU_B@yRR7xn%Ew-76X-7`a7yxKgYW8BmP&_}3-##4Cu5%xiuv@3wJ0pUSl`_I z{1@QnF994aKg6p z(@2NM_#?2#ryQJ8*f10Qb&3T-%11yX+yPKkg2vA zbhwQ#WI!D_>Z2uWl+-TK z`dF0@cODdD<%iFL^|1ypQ`nZuGl>TjSMDC9rl$)-a@45vbvI$5a^A0M58`oW%k&B+ z(zFQ07WGgoRn@7>R8*Sa?JLTh+P$>6DDKy$k{!HnJ$&$6!f7mw@H(E8doM-<{%@H^ zDWY_l`qgvw=jPVpfSF1td;=ICYo(Ddi|6U&>1t6kq8O93&dxxD|)_Z?@sVJ81phJi5EXG z(V960w{yq;C+;4eb*!m<*4NclQ2Ax}b@`{#o5HZKAJJX5&dwSJc7Hs6`xn_SnV;Rl z!kqoOnKs^F`ULbvr&c3JP)eWl4|+rx!^k#wm5mffBWu}{#Sq%bQ)8Bv86OP`Ma@CSRL4=9 z3%o2JNezlCMbc)%8!rv6tFY{~#WqR@-M+#iwcw(5@vrrX^ULAQb%BB8(%#+?NrjJ` zudX7VOZI8T0Uyz%WHixr$4LLv^Ko&U;D!w?@i9Tb_gk}XY!PNWATaP;eQg#dRy>vo z?lf3ZBQfH|GG_v;$e!?O+l{?xHLrWu6*o%A#(G-~GE+fLb+l+8v}>ctR1aL|NFOBy zh5Dz{=_*qTZEH8xRYDqG#(bb>Y4JC`7JHOYVb?^@c}-1C6nsZCMpIL-uvZK!uAWKhb8uQb3pN#r|1MY4Wg2F(9Q50G7dtN6f{O!4!-NX zy;l@*5fKrza*HXcsT#du5XkWG@E!3dvm4V%J^orc+O6&FQ?n^aKN#HYX691!Glz9& zsbXUe>$2bYS^U3BOaMVbae0u7(t)cC86bK1i?xZ#4M>ElDkLB{u)_cBTu*PW>*Swu zHpkL2F)=BJWQ@?socMbQ=j?$lbYf!8jF(xu@`u3|D?`cRRA&ihaPG3aMylwHg_WTL zjAl{+>sv_mgQW&Tp#TyaGt@mUyy-!@n}GN=Bi&AOJx?Ua{La$H7U&d?R0I-@>isvO zA|sC;kZj!D`-2f@n4uY6H+_A5lWLI!QH8_WE~!1@Xucz9V|Q(uEq?%2#^c_k1R48L z-Ew>!Lc=c813>qdG~cDc-1{Y;o zGh5sCf)DMmYoAerZy>B|2u*0Vs+pMBu=kN44WXY`+-12ea>A(ir&BGQAi~JVDB)zS z>F(YfT9KR%r+^ysPEMWqILA&+VZ0YtCF%A#vXm~!nnpy5yBv6`HEwZn2*Dn1rZ_QN z76-)O?m?ZS7!*o4IizyN*I(=J*O{yfMi-{0Qtu3}Gy3_C$fwu%9{EkYikx3x?|hrn z9YCBS!M2CE+LRHR@^^i^gg^vmSZ*@6=IJ4KPUMx5{DDwEYlOnFotRT=Ute-uoR%C$ z^1JS%W{c*RI&AmeSpKdu-L#aa?k}uqj`*LU7fik#9GSt1Jb>=B)z|KU9ERYB0aUI*Q`%r!LIyYaDY*<1;%@;bw*;vtQm3J?>h`;YPN9O?9wC}o?jreM-T z4Q>0wRY@&o64%XEB{jV0nLBrll$3<)k`n0Ie1yUS&o{qlCMSNLGE2IAT})yS;pLqt z&W9IP(!0*h4Z27&Fia8#hiosx$MJZ2d?khJJoI-LZiW1(i=dz&Pm4qb0%sK#5%H(Z zhYyo2R$r>)v@+PGA+oZv85{~pBqovUf`Us-#!Mhs$99lZG8G(G5_{g)61mDMxXLr# zgo#Dx^Fr2hxF{*9?Usl>HxvS{0ynjg#s#Xb_#4`P_tEHxVsIYo6GJZz%1p$edW->>5D*Pw{b%p z0hfA$if%EUoHQ@@up6jhYv2-PpZZ2X%{4E?mTK8hHuN zGXpNr(K()4l77#7^*I-aZJ$5*mMDGi-c{D?QOX}U1qEX{{MH?&UI&2v;fCZhKKN7H z$9xQ96JxXExO88ZIT^2Q-Zte2dIHISr(b3H9;(g*y zq3|2KYvN>D!@{jQIEKqWf^Z<#tS2R*=opz_B0oc$6)e}r>M-(@l+t5(de$*IA0HpU zcHjT_{GUF3b+G9_J9txqSs3tWF|yBqLtmMmKs3HKy5nPJw9_bR_#NGO(*uAN5$Wk{ zY+SdbL+5Vy8khb$j-aZAx+#0=T^%iTEcY3(d`)}f6@Wir&Fru^Aw4|&eR&vri)}*P z=7WCudL&zs3CBRX>}Sen<8&a4z8?qZgNCN2W9RCK(+?K_W+U|usJE#aN=oONY@h03 zU1l)e^Gn(_>tmf|tpg)4rm`TGWc}#aSaA)<{xTnuB@OH8U%A6+Aj@H}+J_giy}d2{ zeBlyt4*Krx+vrjxw9*RmK^OY$5eMxp8SaP&itIK={Zr3o>S96bOFSD4GtSq_b4B-) z{Zv&|i%eO%>C44x>+8LD9HfqDSgkFN)&34vWy|IY2;Joq5*E@jJ@WCGc%^OD%)3Gm zS0Sxx*frFo9j`DDp>7|Fs$QKtA$x=GxoODQ(c9ZQj#$20nb-0%dBmfk>f}AN8(>uv zC1nw;`u#$Zf!ri!HGC9w*4U3qhaD1L+Gn5jts7q7(o&5|QL(Y9>1*!*6vkBisf74A zJwE;laUMF{HlS-o8Uv;0ZgI~?mN#+_BbDeo&(lt!*d2C?7~(KKsdVfSGdwA?@%x1a0LHbquLZ@j(-iDi3W4bm>T8eA7c%q zWhN@0j=1zZ>Wu=mQt&m3nIK@FYoDg0mHhxP$NTHXGa~aK$P>tCv9`QASt6qOaJxTB zwNjsL-FfpUH`^Gw0r){87~qe~T%ngaC2psv#WSxA9DbWFudQ8X|Kl?CB~D`3m7hg3 z_tn&RY}Ju-3)m;r%cD#H0N+cw#UwV#n<2(pS2}JVhrr7#sy?PhQV1WPczKrbDk>rk zUFWwvJz|ibNf&lX3ZkMY`myh_w?bPV@TQ3H@o|RsGQNAFUk4}9Kxp>&4N<4D{ep4HYGW|K`^= z^#IUnPRkPi+&n!2SJf#WYJ$~xOEQUCaAm2+cFVKo0LKm$E#wB>@ihS3u!kbn{~_i; zq&i5H-JM7NsI1|VlUw`y^OhMI)PYOR3GA|`wUJMM0L59_YgJ+Z$@;rCk_-Gq-PO1H z_x~kLf9qUGO)oDo6beNiesSgoC>Peabb6lsHBcTE`TwsD@Q)_eadt`LobW&L*uRT* bcI%Y(E81!}YYznjw+PWt)ln%?wto3vgzRqy literal 0 HcmV?d00001 diff --git a/wireshark/README.md b/wireshark/README.md new file mode 100644 index 000000000..07bf20fb6 --- /dev/null +++ b/wireshark/README.md @@ -0,0 +1,56 @@ +# How to analyze CFLIB CRTP traffic using Wireshark + +Wireshark is a free and open-source packet analyzer. It is used for network troubleshooting, analysis, software and communications protocol development, and education. It makes analyzing what is going on with packet based protocols easier. + +We have written a plugin to Wireshark that can display [CRTP](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/) packets (what this library uses to communicate with Crazyflies). + +![Image of CRTP dissector in Wireshark](../docs/images/wireshark-dissector.png?raw=true "CRTP Wireshark Dissector") + +In order for Wireshark (and our dissector plugin) to be able to decode CRTP you need to do *two* things: + +## 1. Install the plugin [crtp-dissector.lua](crtp-dissector.lua) in Wireshark + +Wireshark looks for plugins in both a personal plugin folder and a global plugin folder. Lua plugins are stored in the plugin folders; compiled plugins are stored in subfolders of the plugin folders, with the subfolder name being the Wireshark minor version number (X.Y). There is another hierarchical level for each Wireshark plugin type (libwireshark, libwiretap and codecs). So for example the location for a libwireshark plugin foo.so (foo.dll on Windows) would be PLUGINDIR/X.Y/epan (libwireshark used to be called libepan; the other folder names are codecs and wiretap). + +On Windows: + +- The personal plugin folder is %APPDATA%\Wireshark\plugins. +- The global plugin folder is WIRESHARK\plugins. + +On Unix-like systems: + +- The personal plugin folder is ~/.local/lib/wireshark/plugins. + +Copy the crtp-dissector.lua file into the personal plugin folder and restart Wireshark or hit `CTRL+SHIFT+L` + +## 2. Generate a [PCAP](https://en.wikipedia.org/wiki/Pcap) file + +The de facto standard network packet capture format is libpcap (pcap), which is used in packet analyzers such as tcpdump/WinDump and Wireshark. The pcap file format is a binary format, with support for nanosecond-precision timestamps. + +To tell the CFLIB to generate a PCAP file of what it thinks the CRTP traffic looks like you can go: + +```bash +$ CRTP_PCAP_LOG=filename.pcap python3 examples/swarm/hl-commander-swarm.py +$ wireshark filename.pcap +``` +To generate a PCAP file and open it with Wireshark. You can also use the text based `tshark` tool, and you can add a filter, for instance, only shoow CRTP port 8 (Highlevel setpoint): + +```bash +$ tshark -r test.pcap "crtp.port == 8" + 6 0.003333 00:DE:AD:BE:EF (10) → Radio #0 CRTP 13 Setpoint Highlevel +13158 5.176752 Radio #0 → 00:DE:AD:BE:EF (10) CRTP 24 Setpoint Highlevel +13163 5.178626 00:DE:AD:BE:EF (10) → Radio #0 CRTP 13 Setpoint Highlevel +17768 8.179085 Radio #0 → 00:DE:AD:BE:EF (10) CRTP 32 Setpoint Highlevel +17773 8.181294 00:DE:AD:BE:EF (10) → Radio #0 CRTP 13 Setpoint Highlevel +21223 10.181543 Radio #0 → 00:DE:AD:BE:EF (10) CRTP 32 Setpoint Highlevel +21228 10.183293 00:DE:AD:BE:EF (10) → Radio #0 CRTP 13 Setpoint Highlevel +24030 12.182588 Radio #0 → 00:DE:AD:BE:EF (10) CRTP 32 Setpoint Highlevel +24035 12.185723 00:DE:AD:BE:EF (10) → Radio #0 CRTP 13 Setpoint Highlevel +26449 14.184183 Radio #0 → 00:DE:AD:BE:EF (10) CRTP 32 Setpoint Highlevel +26454 14.185862 00:DE:AD:BE:EF (10) → Radio #0 CRTP 13 Setpoint Highlevel +28388 16.187258 Radio #0 → 00:DE:AD:BE:EF (10) CRTP 24 Setpoint Highlevel +28393 16.188963 00:DE:AD:BE:EF (10) → Radio #0 CRTP 13 Setpoint Highlevel +30533 18.190823 Radio #0 → 00:DE:AD:BE:EF (10) CRTP 11 Setpoint Highlevel +``` + +Happy hacking! diff --git a/wireshark/crtp-dissector.lua b/wireshark/crtp-dissector.lua new file mode 100644 index 000000000..34bebd622 --- /dev/null +++ b/wireshark/crtp-dissector.lua @@ -0,0 +1,402 @@ +-- declare our protocol +crtp = Proto("CrazyRealTimeTProtocol","Crazy Real Time Protocol") + +-- General CRTP packet fields +local f_crtp_port = ProtoField.uint8("crtp.port", "Port") +local f_crtp_channel = ProtoField.uint8("crtp.channel", "Channel") +local f_crtp_size = ProtoField.uint8("crtp.size", "Size") + +-- Specialized CRTP service fields +local f_crtp_console_text = ProtoField.string("crtp.console_text", "Text", base.ASCII) +local f_crtp_parameter_varid = ProtoField.uint16("crtp.parameter_varid", "Variable Id") + +local f_crtp_parameter_val_uint = ProtoField.uint32("crtp.parameter_val_uint", "Value uint") +local f_crtp_parameter_val_int = ProtoField.int32("crtp.parameter_val_int", "Value int") +local f_crtp_parameter_val_float = ProtoField.float("crtp.parameter_val_float", "Value float") + +local f_crtp_setpoint_hl_command = ProtoField.string("crtp.setpoint_hl_command", "Command") +local f_crtp_setpoint_hl_retval = ProtoField.uint8("crtp.setpoint_hl_retval", "Return Value") + +local f_crtp_setpoint_hl_groupmask = ProtoField.uint8("crtp.setpoint_hl_groupmask", "Group Mask") +local f_crtp_setpoint_hl_id = ProtoField.uint8("crtp.setpoint_hl_id", "Trajectory Id") + +local f_crtp_setpoint_hl_height = ProtoField.float("crtp.setpoint_hl_height", "Height") +local f_crtp_setpoint_hl_yaw = ProtoField.float("crtp.setpoint_hl_yaw", "Yaw") +local f_crtp_setpoint_hl_use_yaw = ProtoField.bool("crtp.setpoint_hl_use_yaw", "Use Current Yaw") +local f_crtp_setpoint_hl_relative = ProtoField.bool("crtp.setpoint_hl_relative", "Relative") +local f_crtp_setpoint_hl_duration = ProtoField.float("crtp.setpoint_hl_duration", "Duration") +local f_crtp_setpoint_hl_timescale = ProtoField.float("crtp.setpoint_hl_timescale", "Timescale") + +local f_crtp_setpoint_hl_x = ProtoField.float("crtp.setpoint_hl_x", "X") +local f_crtp_setpoint_hl_y = ProtoField.float("crtp.setpoint_hl_y", "Y") +local f_crtp_setpoint_hl_z = ProtoField.float("crtp.setpoint_hl_z", "Z") + + +-- All possible fields registred +crtp.fields = { + f_crtp_port, f_crtp_channel, f_crtp_console_text, f_crtp_size, + f_crtp_parameter_varid, f_crtp_parameter_val_uint, + f_crtp_parameter_val_int, f_crtp_parameter_val_float, + f_crtp_setpoint_hl_command, f_crtp_setpoint_hl_retval, + f_crtp_setpoint_hl_use_yaw, f_crtp_setpoint_hl_yaw, + f_crtp_setpoint_hl_groupmask, f_crtp_setpoint_hl_duration, + f_crtp_setpoint_hl_height, f_crtp_setpoint_hl_relative, + f_crtp_setpoint_hl_x, f_crtp_setpoint_hl_y, f_crtp_setpoint_hl_z, + f_crtp_setpoint_hl_id, f_crtp_setpoint_hl_timescale +} + + +-- Analye port and channel and figure what service (port name / channel name) +-- we are dealing with + +local Ports = { + Console = 0x0, + Parameters = 0x2, + Commander = 0x3, + Memory = 0x4, + Logging = 0x5, + Localization = 0x6, + Commander_Generic = 0x7, + Setpoint_Highlevel = 0x8, + Platform = 0xD, + All = 0xF +} +function get_crtp_port_channel_names(port, channel) + local port_name = "Unknown" + local channel_name = nil + + if port == Ports.Console and channel == 0 then port_name = "Console" + + elseif port == 0x02 then + port_name = "Parameters" + if channel == 0 then channel_name = "Table Of Contents" + elseif channel == 1 then channel_name = "Read" + elseif channel == 2 then channel_name = "Write" + elseif channel == 3 then channel_name = "Misc" + end + + elseif port == 0x03 then port_name = "Commander" + + elseif port == 0x04 then + port_name = "Memory" + if channel == 0 then channel_name = "Information" + elseif channel == 1 then channel_name = "Read" + elseif channel == 2 then channel_name = "Write" + end + + elseif port == 0x05 then + port_name = "Logging" + if channel == 0 then channel_name = "Table Of Contents" + elseif channel == 1 then channel_name = "Settings" + elseif channel == 2 then channel_name = "Log data" + end + + elseif port == 0x06 then port_name = "Localization" + if channel == 0 then channel_name = "Position" + elseif channel == 1 then channel_name = "Generic" + end + + elseif port == 0x07 then port_name = "Commander Generic" + elseif port == 0x08 then port_name = "Setpoint Highlevel" + elseif port == 0x0D then + port_name = "Platform" + if channel == 0 then channel_name = "Platform Command" + elseif channel == 1 then channel_name = "Version Command" + elseif channel == 2 then channel_name = "App Layer" + end + + elseif port == 0x0F then + port_name = "Link Control" + if channel == 1 then channel_name = "Link Service Source" + end + + elseif port == 0xFF then port_name = "ALL" end + + return port_name, channel_name +end + +function format_address(buffer) + addr = buffer(1, 5):bytes():tohex() + port = buffer(6, 1):uint() + + addr = addr:gsub("..", ":%0"):sub(2) + port = tostring(port) + + return addr .. " (" .. port .. ")" +end + +function format_radio(buffer) + devid = buffer(7, 1):uint() + + return "Radio #" .. tostring(devid) +end + +function handle_setpoint_highlevel(tree, receive, buffer, channel, size) + local Commands = { + COMMAND_SET_GROUP_MASK = 0, + COMMAND_TAKEOFF = 1, + COMMAND_LAND = 2, + COMMAND_STOP = 3, + COMMAND_GO_TO = 4, + COMMAND_START_TRAJECTORY = 5, + COMMAND_DEFINE_TRAJECTORY = 6, + COMMAND_TAKEOFF_2 = 7, + COMMAND_LAND_2 = 8, + COMMAND_TAKEOFF_WITH_VELOCITY = 9, + COMMAND_LAND_WITH_VELOCITY = 10, + } + + local height = nil + local duration = nil + local yaw = nil + local group_mask = nil + local use_yaw = nil + local relative = nil + local x = nil + local y = nil + local z = nil + local id = nil + local timescale = nil + + local port_tree = tree:add(crtp, port_name) + + local cmd = buffer(9, 1):uint() + local cmd_str = "Unknown" + + if cmd == Commands.COMMAND_SET_GROUP_MASK then + cmd_str = "Set Group Mask" + + -- struct data_set_group_mask { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(10, 1):uint() + end + elseif cmd == Commands.COMMAND_TAKEOFF then cmd_str = "Take Off (deprecated)" + elseif cmd == Commands.COMMAND_LAND then cmd_str = "Land (deprecated)" + elseif cmd == Commands.COMMAND_STOP then + cmd_str = "Stop" + + -- struct data_stop { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(10, 1):uint() + end + elseif cmd == Commands.COMMAND_GO_TO then + cmd_str = "Go To" + + -- struct data_go_to { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- uint8_t relative; // set to true, if position/yaw are relative to current setpoint + -- float x; // m + -- float y; // m + -- float z; // m + -- float yaw; // rad + -- float duration; // sec + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(10, 1):uint() + relative = buffer(11, 1):uint() + x = buffer(12, 4):le_float() + y = buffer(16, 4):le_float() + z = buffer(20, 4):le_float() + yaw = buffer(24, 4):le_float() + duration = buffer(28, 4):le_float() + end + elseif cmd == Commands.COMMAND_START_TRAJECTORY then + cmd_str = "Start Trajectory" + + -- struct data_start_trajectory { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- uint8_t relative; // set to true, if trajectory should be shifted to current setpoint + -- uint8_t reversed; // set to true, if trajectory should be executed in reverse + -- uint8_t trajectoryId; // id of the trajectory (previously defined by COMMAND_DEFINE_TRAJECTORY) + -- float timescale; // time factor; 1 = original speed; >1: slower; <1: faster + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(10, 1):uint() + relative = buffer(11, 1):uint() + reversed = buffer(12, 1):uint() + id = buffer(13, 1):uint() + timescale = buffer(14):float() + end + elseif cmd == Commands.COMMAND_DEFINE_TRAJECTORY then + cmd_str = "Define Trajectory" + + -- struct data_define_trajectory { + -- uint8_t trajectoryId; + -- struct trajectoryDescription description; + -- } __attribute__((packed)); + if receive == 0 then + id = buffer(10, 1):uint() + end + elseif cmd == Commands.COMMAND_TAKEOFF_2 then + cmd_str = "Take Off" + + -- struct data_takeoff_2 { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- float height; // m (absolute) + -- float yaw; // rad + -- bool useCurrentYaw; // If true, use the current yaw (ignore the yaw parameter) + -- float duration; // s (time it should take until target height is reached) + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(10, 1):uint() + height = buffer(11, 4):le_float() + yaw = buffer(15, 4):le_float() + use_yaw = buffer(19, 1):uint() + duration = buffer(20, 4):le_float() + end + + elseif cmd == Commands.COMMAND_LAND_2 then + cmd_str = "Land" + -- struct data_land_2 { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- float height; // m (absolute) + -- float yaw; // rad + -- bool useCurrentYaw; // If true, use the current yaw (ignore the yaw parameter) + -- float duration; // s (time it should take until target height is reached) + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(10, 1):uint() + height = buffer(11, 4):le_float() + yaw = buffer(15, 4):le_float() + use_yaw = buffer(19, 1):uint() + duration = buffer(20, 4):le_float() + end + + elseif cmd == Commands.COMMAND_TAKEOFF_WITH_VELOCITY then cmd_str = "Take Off With Velocity" + elseif cmd == Commands.COMMAND_LAND_WITH_VELOCITY then cmd_str = "Land With Velocity" end + + port_tree:add_le(f_crtp_setpoint_hl_command, cmd_str) + local success = true + if receive == 1 then + retval = buffer(12):uint() + local success = (retval == 0) and "Success" or "Failure" + port_tree:add_le(f_crtp_setpoint_hl_retval, retval):append_text(" (" .. success .. ")") + end + + if id then + port_tree:add_le(f_crtp_setpoint_hl_id, id) + end + if timescale then + port_tree:add_le(f_crtp_setpoint_hl_timescale, timescale) + end + if group_mask then + port_tree:add_le(f_crtp_setpoint_hl_groupmask, group_mask) + end + if relative then + port_tree:add_le(f_crtp_setpoint_hl_relative, relative) + end + if height then + port_tree:add_le(f_crtp_setpoint_hl_height, height):append_text(" (m)") + end + if x then + port_tree:add_le(f_crtp_setpoint_hl_x, x) + end + if y then + port_tree:add_le(f_crtp_setpoint_hl_y, y) + end + if z then + port_tree:add_le(f_crtp_setpoint_hl_z, z) + end + if yaw then + port_tree:add_le(f_crtp_setpoint_hl_yaw, yaw) + end + if use_yaw then + port_tree:add_le(f_crtp_setpoint_hl_use_yaw, use_yaw) + end + if duration then + port_tree:add_le(f_crtp_setpoint_hl_duration, duration):append_text(" (s)") + end +end + +function handle_parameter_port(tree, buffer, channel, size) + -- Read or Write + if (channel == 1 or channel == 2) and size > 2 then + + -- Add variable id + local var_id = buffer(9, 2):le_uint() + local port_tree = tree:add(crtp, port_name) + port_tree:add_le(f_crtp_parameter_varid, var_id) + + -- Add value + if size > 3 then + port_tree:add_le(f_crtp_parameter_val_uint, buffer(12):le_uint()) + port_tree:add_le(f_crtp_parameter_val_int, buffer(12):le_int()) + end + if size >= 7 then + port_tree:add_le(f_crtp_parameter_val_float, buffer(12):le_float()) + end + end +end + +-- create a function to dissect it, layout: +-- | receive| address | channel | radio devid | crtp header | crtp data | +-- | 1 byte | 5 bytes | 1 byte | 1 byte | 1 byte | n bytes | +function crtp.dissector(buffer, pinfo, tree) + pinfo.cols.protocol = "CRTP" + + if buffer:len() <= 8 then return end + + local receive = buffer(0, 1):uint() + if receive == 1 then + pinfo.cols.dst = format_radio(buffer) + pinfo.cols.src = format_address(buffer) + else + pinfo.cols.dst = format_address(buffer) + pinfo.cols.src = format_radio(buffer) + end + + local subtree = tree:add(crtp, "CRTP Packet") + local header = bit.band(buffer(8, 1):uint(), 0xF3) + local crtp_port = bit.rshift(bit.band(header, 0xF0), 4) + local crtp_channel = bit.band(header, 0x03) + + -- Add CRTP packet size: + -- receive_byte + address + channel + devid = 8 + -- Rest is CRTP packet + local crtp_size = buffer:len() - 8 + subtree:add_le(f_crtp_size, crtp_size) + + -- Check for safelink packet + if crtp_size == 3 and header == 0xF3 and buffer(9, 1):uint() == 0x05 then + pinfo.cols.info = "SafeLink" + return + end + + -- Get port and channel name + port_name, channel_name = get_crtp_port_channel_names(crtp_port, crtp_channel) + + -- Display port in info column + pinfo.cols.info = port_name + + -- Add to CRTP tree + subtree:add_le(f_crtp_port, crtp_port):append_text(" (" .. port_name .. ")") + if channel_name then + subtree:add_le(f_crtp_channel, crtp_channel):append_text(" (" .. channel_name .. ")") + else + subtree:add_le(f_crtp_channel, crtp_channel) + end + + -- Check for special handling + + -- Console, we can add text + if crtp_port == Ports.Console then + local port_tree = tree:add(crtp, port_name) + port_tree:add_le(f_crtp_console_text, buffer(9):string()) + end + + if crtp_port == Ports.Parameters then + handle_parameter_port(tree, buffer, crtp_channel, crtp_size) + end + + if crtp_port == Ports.Setpoint_Highlevel then + handle_setpoint_highlevel(tree, receive, buffer, crtp_channel, crtp_size) + end + +end + +wtap_encap = DissectorTable.get("wtap_encap") +wtap_encap:add(wtap.USER15, crtp) From 510d1126e82f9ef8630f94e9e22cdf1c0914cb71 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 12:55:00 +0100 Subject: [PATCH 279/861] Wait for parameters to be updated before setinng new value --- cflib/crazyflie/param.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 8fdca1151..b4ec37415 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -275,6 +275,9 @@ def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ + if not self._initialized.wait(timeout=60): + raise Exception('Connection timed out') + element = self.toc.get_element_by_complete_name(complete_name) if not element: From e902522099831f40498156dc15fcd3e525e89ff4 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:30:40 +0100 Subject: [PATCH 280/861] Added function to SyncCrazyflie to wait for param value download --- cflib/crazyflie/syncCrazyflie.py | 61 +++++++++++++++++++++-- test/crazyflie/test_syncCrazyflie.py | 74 +++++++++++++++++++++++++++- 2 files changed, 130 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index 7330158bb..ffeae9b45 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -26,7 +26,7 @@ """ The synchronous Crazyflie class is a wrapper around the "normal" Crazyflie class. It handles the asynchronous nature of the Crazyflie API and turns it -into blocking function. It is useful for simple scripts that performs tasks +into blocking functions. It is useful for simple scripts that performs tasks as a sequence of events. Example: @@ -49,9 +49,19 @@ class SyncCrazyflie: def __init__(self, link_uri, cf=None): - """ Create a synchronous Crazyflie instance with the specified - link_uri """ - + """ + Create a synchronous Crazyflie instance with the specified link_uri + + Parameters + ---------- + link_uri: string + The uri to use when connecting to the Crazyflie + cf: Crazyflie + Optional Crazyflie instance to use, None by default. If no object is supplied a Crazyflie instance + is created. + This parameters is useful if you want to use a Crazyflie instance with log/param + caching. + """ if cf: self.cf = cf else: @@ -60,10 +70,20 @@ def __init__(self, link_uri, cf=None): self._link_uri = link_uri self._connect_event = None self._disconnect_event = None + self._params_updated_event = Event() self._is_link_open = False self._error_message = None def open_link(self): + """ + Open a link to a Crazyflie on the underlying Crazyflie instance. + + This function is blocking and will return when the connection is established and TOCs for log and + parameters have been downloaded or fetched from the cache. + + Note: Parameter values have not been updated when this function returns. See the wait_for_params() + method. + """ if (self.is_link_open()): raise Exception('Link already open') @@ -72,14 +92,38 @@ def open_link(self): logger.debug('Connecting to %s' % self._link_uri) self._connect_event = Event() + self._params_updated_event.clear() self.cf.open_link(self._link_uri) self._connect_event.wait() self._connect_event = None if not self._is_link_open: self._remove_callbacks() + self._params_updated_event.clear() raise Exception(self._error_message) + def wait_for_params(self): + """ + Wait for parameter values to be updated. + + During the connection sequence, parameter values are downloaded after the TOCs have been received. The + open_link() method will return after the TOCs have been received but before the parameter values + are downloaded. + This method will block until the parameter values are received and can be used + to make sure the connection sequence has terminated. In most cases this is not important, but + radio bandwidth will be limited while parameters are downloaded due to the communication that is going on. + + Example + ------- + ```python + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + scf.wait_for_params() + # At this point the connection sequence is finished + ``` + + """ + self._params_updated_event.wait() + def __enter__(self): self.open_link() return self @@ -90,6 +134,7 @@ def close_link(self): self.cf.close_link() self._disconnect_event.wait() self._disconnect_event = None + self._params_updated_event.clear() def __exit__(self, exc_type, exc_val, exc_tb): self.close_link() @@ -97,6 +142,9 @@ def __exit__(self, exc_type, exc_val, exc_tb): def is_link_open(self): return self._is_link_open + def is_params_updated(self): + return self._params_updated_event.is_set() + 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.""" @@ -120,10 +168,14 @@ def _disconnected(self, link_uri): if self._disconnect_event: self._disconnect_event.set() + def _all_params_updated(self): + self._params_updated_event.set() + def _add_callbacks(self): self.cf.connected.add_callback(self._connected) self.cf.connection_failed.add_callback(self._connection_failed) self.cf.disconnected.add_callback(self._disconnected) + self.cf.param.all_updated.add_callback(self._all_params_updated) def _remove_callbacks(self): def remove_callback(container, callback): @@ -135,3 +187,4 @@ def remove_callback(container, callback): remove_callback(self.cf.connected, self._connected) remove_callback(self.cf.connection_failed, self._connection_failed) remove_callback(self.cf.disconnected, self._disconnected) + remove_callback(self.cf.param.all_updated, self._all_params_updated) diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index 957b1e274..d9ef77aee 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -28,6 +28,7 @@ from unittest.mock import MagicMock from cflib.crazyflie import Crazyflie +from cflib.crazyflie import Param from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper from cflib.utils.callbacks import Caller @@ -56,7 +57,16 @@ def setUp(self): ) self.cf_mock.close_link = self.close_link_mock.trigger - self.sut = SyncCrazyflie(self.uri, self.cf_mock) + # Mock the behaviour that param values are updated(downloaded) after connection + self.param_mock = MagicMock(spec=Param) + self.param_mock.all_updated = Caller() + self.cf_mock.param = self.param_mock + + # Register a callback to be called when connected. Use it to trigger a callback + # to trigger the call to the param.all_updated() callback + self.cf_mock.connected.add_callback(self._connected_callback) + + self.sut = SyncCrazyflie(self.uri, cf=self.cf_mock) def test_different_underlying_cf_instances(self): # Fixture @@ -107,6 +117,26 @@ def test_open_link_of_already_open_link_raises_exception(self): with self.assertRaises(Exception): self.sut.open_link() + def test_wait_for_params(self): + # Fixture + + self.sut.open_link() + + # Test + self.sut.wait_for_params() + + # Assert + self.assertTrue(self.sut.is_params_updated()) + + def test_do_not_wait_for_params(self): + # Fixture + + # Test + self.sut.open_link() + + # Assert + self.assertFalse(self.sut.is_params_updated()) + def test_close_link(self): # Fixture self.sut.open_link() @@ -152,6 +182,17 @@ def test_open_close_with_context_management(self): self.assertEqual(1, self.close_link_mock.call_count) self._assertAllCallbacksAreRemoved() + def test_wait_for_params_with_context_management(self): + # Fixture + + # Test + with SyncCrazyflie(self.uri, cf=self.cf_mock) as sut: + sut.wait_for_params() + self.assertTrue(sut.is_params_updated()) + + # Assert + self._assertAllCallbacksAreRemoved() + def test_multiple_open_close_of_link(self): # Fixture @@ -166,7 +207,38 @@ def test_multiple_open_close_of_link(self): self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() + def test_wait_for_params_with_multiple_open_close_of_link(self): + # Fixture + + # Test + self.sut.open_link() + self.assertFalse(self.sut.is_params_updated()) + self.sut.wait_for_params() + self.assertTrue(self.sut.is_params_updated()) + self.sut.close_link() + self.assertFalse(self.sut.is_params_updated()) + + self.sut.open_link() + self.assertFalse(self.sut.is_params_updated()) + self.sut.wait_for_params() + self.assertTrue(self.sut.is_params_updated()) + self.sut.close_link() + + # Assert + self.assertFalse(self.sut.is_params_updated()) + self._assertAllCallbacksAreRemoved() + def _assertAllCallbacksAreRemoved(self): + # Remove our probe callback + self.cf_mock.connected.remove_callback(self._connected_callback) + self.assertEqual(0, len(self.cf_mock.connected.callbacks)) self.assertEqual(0, len(self.cf_mock.connection_failed.callbacks)) self.assertEqual(0, len(self.cf_mock.disconnected.callbacks)) + self.assertEqual(0, len(self.param_mock.all_updated.callbacks)) + + def _connected_callback(self, uri): + AsyncCallbackCaller( + cb=self.param_mock.all_updated, + delay=0.2 + ).trigger() From 3ba784b930ba10615615fc9e9c00a083ebd94096 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:45:53 +0100 Subject: [PATCH 281/861] Updated documentation --- cflib/crazyflie/syncCrazyflie.py | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index ffeae9b45..3245f0674 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -52,15 +52,9 @@ def __init__(self, link_uri, cf=None): """ Create a synchronous Crazyflie instance with the specified link_uri - Parameters - ---------- - link_uri: string - The uri to use when connecting to the Crazyflie - cf: Crazyflie - Optional Crazyflie instance to use, None by default. If no object is supplied a Crazyflie instance - is created. - This parameters is useful if you want to use a Crazyflie instance with log/param - caching. + :param link_uri: The uri to use when connecting to the Crazyflie + :param cf: Optional Crazyflie instance to use, None by default. If no object is supplied, a Crazyflie instance + is created. This parameters is useful if you want to use a Crazyflie instance with log/param caching. """ if cf: self.cf = cf @@ -113,8 +107,7 @@ def wait_for_params(self): to make sure the connection sequence has terminated. In most cases this is not important, but radio bandwidth will be limited while parameters are downloaded due to the communication that is going on. - Example - ------- + Example: ```python with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: scf.wait_for_params() From df855b0aeae8894e60cef416f862f6abef58eb11 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 282/861] Param: fetch extended types at connection --- cflib/crazyflie/param.py | 112 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 111 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index b4ec37415..61c2e25d4 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -61,6 +61,9 @@ WRITE_CHANNEL = 2 MISC_CHANNEL = 3 +MISC_SETBYNAME = 0 +MISC_GET_EXTENDED_TYPE = 2 + # One element entry in the TOC @@ -70,6 +73,8 @@ class ParamTocElement: RW_ACCESS = 0 RO_ACCESS = 1 + EXTENDED_PERSISTENT = 1 + types = {0x08: ('uint8_t', ' 0: + self.extended_type_fetcher.set_callback(refresh_done_callback) + self.extended_type_fetcher.request_extended_types(extended_elements) + else: + refresh_done_callback() + self._useV2 = self.cf.platform.get_protocol_version() >= 4 toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, - refresh_done_callback, toc_cache) + refresh_done, toc_cache) toc_fetcher.start() def _disconnected(self, uri): @@ -317,6 +354,79 @@ def get_value(self, complete_name, timeout=60): return self.values[group][name] +class _ExtendedTypeFetcher(Thread): + + def __init__(self, cf, toc): + Thread.__init__(self) + self.setDaemon(True) + self._lock = Lock() + + self._cf = cf + self._toc = toc + self._done_callback = None + + self.request_queue = Queue() + self._cf.add_port_callback(CRTPPort.PARAM, self._new_packet_cb) + self._should_close = False + self._req_param = -1 + self._count = -1 + + def _new_packet_cb(self, pk): + """Callback for newly arrived packets""" + if pk.channel == MISC_CHANNEL: + var_id = struct.unpack(' Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 283/861] Param: add persistent_get_state method Get the state of the specified persistent parameter. The state will be returned in the supplied callback. The state is represented as a namedtuple with members: `is_stored`, `default_value` and `stored_value`. The state is `None` if the parameter is not persistent or if something goes wrong. | Member | Description | | ----------------- | ----------------------------------------------- | | `is_stored` | `True` if the value is stored to eeprom | | `default_value` | The default value supplied by the firmware | | `stored_value` | Value stored in eeprom, None if `not is_stored` | --- cflib/crazyflie/param.py | 51 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 61c2e25d4..c56986119 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -31,8 +31,10 @@ the parameters that can be written/read. """ +import errno import logging import struct +from collections import namedtuple from queue import Queue from threading import Event from threading import Lock @@ -64,6 +66,10 @@ MISC_SETBYNAME = 0 MISC_GET_EXTENDED_TYPE = 2 +PersistentParamState = namedtuple('PersistentParamState', 'is_stored default_value stored_value') + +MISC_PERSISTENT_GET_STATE = 4 + # One element entry in the TOC @@ -353,6 +359,51 @@ def get_value(self, complete_name, timeout=60): [group, name] = complete_name.split('.') return self.values[group][name] + def persistent_get_state(self, complete_name, callback): + """ + Get the state of the specified persistent parameter. The state will be + returned in the supplied callback. The state is represented as a + namedtuple with members: `is_stored`, `default_value` and + `stored_value`. The state is `None` if the parameter is not persistent + or if something goes wrong. + + | Member | Description | + | ----------------- | ----------------------------------------------- | + | `is_stored` | `True` if the value is stored to eeprom | + | `default_value` | The default value supplied by the firmware | + | `stored_value` | Value stored in eeprom, None if `not is_stored` | + + @param complete_name The 'group.name' name of the parameter to store + @param callback Callback, takes PersistentParamState namedtuple as arg + """ + element = self.toc.get_element_by_complete_name(complete_name) + + def new_packet_cb(pk): + if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_GET_STATE: + if pk.data[3] == errno.ENOENT: + callback(None) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + return + + is_stored = pk.data[3] == 1 + if not is_stored: + default_value = struct.unpack(f'<{element.pytype}') + else: + default_value, stored_value = struct.unpack(f'<{element.pytype}*2') + + callback(PersistentParamState( + is_stored, + default_value, + None if not is_stored else stored_value + )) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + + self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) + pk = CRTPPacket() + pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) + pk.data = struct.pack(' Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 284/861] Param: add persistent_store method Store the current value of the specified persistent parameter to eeprom. The supplied callback will be called with `True` as an argument on success, and with `False` as an argument on failure. --- cflib/crazyflie/param.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index c56986119..0a48e8d30 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -68,6 +68,7 @@ PersistentParamState = namedtuple('PersistentParamState', 'is_stored default_value stored_value') +MISC_PERSISTENT_STORE = 3 MISC_PERSISTENT_GET_STATE = 4 # One element entry in the TOC @@ -359,6 +360,30 @@ def get_value(self, complete_name, timeout=60): [group, name] = complete_name.split('.') return self.values[group][name] + def persistent_store(self, complete_name, callback=None): + """ + Store the current value of the specified persistent parameter to + eeprom. The supplied callback will be called with `True` as an + argument on success, and with `False` as an argument on failure. + + @param complete_name The 'group.name' name of the parameter to store + @param callback Optional callback should take boolean status as arg + """ + element = self.toc.get_element_by_complete_name(complete_name) + + def new_packet_cb(pk): + if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_STORE: + callback(pk.data[3] == 0) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + + if callback is not None: + self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) + + pk = CRTPPacket() + pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) + pk.data = struct.pack(' Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 285/861] Param: add persistent_clear method Clear the current value of the specified persistent parameter from eeprom. The supplied callback will be called with `True` as an argument on success, and with `False` as an argument on failure. --- cflib/crazyflie/param.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 0a48e8d30..0095f9d25 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -70,6 +70,7 @@ MISC_PERSISTENT_STORE = 3 MISC_PERSISTENT_GET_STATE = 4 +MISC_PERSISTENT_CLEAR = 5 # One element entry in the TOC @@ -360,6 +361,30 @@ def get_value(self, complete_name, timeout=60): [group, name] = complete_name.split('.') return self.values[group][name] + def persistent_clear(self, complete_name, callback=None): + """ + Clear the current value of the specified persistent parameter from + eeprom. The supplied callback will be called with `True` as an + argument on success and with `False` as an argument on failure. + + @param complete_name The 'group.name' name of the parameter to store + @param callback Optional callback should take boolean status as arg + """ + element = self.toc.get_element_by_complete_name(complete_name) + + def new_packet_cb(pk): + if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_CLEAR: + callback(pk.data[3] == 0) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + + if callback is not None: + self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) + + pk = CRTPPacket() + pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) + pk.data = struct.pack(' Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 286/861] param.py: Add complete name to callbacks So that the same callback can multiplex for a lot of parameters. --- cflib/crazyflie/param.py | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 0095f9d25..7c69104f8 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -368,13 +368,13 @@ def persistent_clear(self, complete_name, callback=None): argument on success and with `False` as an argument on failure. @param complete_name The 'group.name' name of the parameter to store - @param callback Optional callback should take boolean status as arg + @param callback Optional callback should take `complete_name` and boolean status as arguments """ element = self.toc.get_element_by_complete_name(complete_name) def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_CLEAR: - callback(pk.data[3] == 0) + callback(complete_name, pk.data[3] == 0) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) if callback is not None: @@ -392,13 +392,13 @@ def persistent_store(self, complete_name, callback=None): argument on success, and with `False` as an argument on failure. @param complete_name The 'group.name' name of the parameter to store - @param callback Optional callback should take boolean status as arg + @param callback Optional callback should take `complete_name` and boolean status as arguments """ element = self.toc.get_element_by_complete_name(complete_name) def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_STORE: - callback(pk.data[3] == 0) + callback(complete_name, pk.data[3] == 0) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) if callback is not None: @@ -424,14 +424,14 @@ def persistent_get_state(self, complete_name, callback): | `stored_value` | Value stored in eeprom, None if `not is_stored` | @param complete_name The 'group.name' name of the parameter to store - @param callback Callback, takes PersistentParamState namedtuple as arg + @param callback Callback, takes `complete_name` and PersistentParamState namedtuple as arg """ element = self.toc.get_element_by_complete_name(complete_name) def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_GET_STATE: if pk.data[3] == errno.ENOENT: - callback(None) + callback(complete_name, None) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) return @@ -441,11 +441,13 @@ def new_packet_cb(pk): else: default_value, stored_value = struct.unpack(f'<{element.pytype}*2') - callback(PersistentParamState( - is_stored, - default_value, - None if not is_stored else stored_value - )) + callback(complete_name, + PersistentParamState( + is_stored, + default_value, + None if not is_stored else stored_value + ) + ) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) From 4f45a73604ba745a40dded12ef7b3d9db342d2aa Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 287/861] param.py: Fix pep8 --- cflib/crazyflie/param.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 7c69104f8..b5f83f970 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -443,11 +443,11 @@ def new_packet_cb(pk): callback(complete_name, PersistentParamState( - is_stored, - default_value, - None if not is_stored else stored_value + is_stored, + default_value, + None if not is_stored else stored_value + ) ) - ) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) From a3599beced26698278f8331a668b66b50613ee0b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 288/861] Added example for persistent parameters --- examples/parameters/persistent_params.py | 96 ++++++++++++++++++++++++ 1 file changed, 96 insertions(+) create mode 100644 examples/parameters/persistent_params.py diff --git a/examples/parameters/persistent_params.py b/examples/parameters/persistent_params.py new file mode 100644 index 000000000..fa4014804 --- /dev/null +++ b/examples/parameters/persistent_params.py @@ -0,0 +1,96 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 to show the use of persistent parameters. + +All persistent parameters are fectched and the current state is printed out. +Finallay the LED ring effect is set to a new value and stored. + +Note: this script will change the value of the LED ring.effect parameter +""" +import logging +import sys +from threading import Event + +import cflib.crtp +from cflib.crazyflie import Crazyflie +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() + + cf = Crazyflie(rw_cache='./cache') + with SyncCrazyflie(uri, cf=cf) as scf: + wait_for_callback_event = Event() + + # Collect the names of all persistent parameters + persistent_params = [] + for group_name, params in scf.cf.param.toc.toc.items(): + for param_name, element in params.items(): + if element.is_persistent(): + complete_name = group_name + '.' + param_name + persistent_params.append(complete_name) + + if len(persistent_params) == 0: + print('No persistent parameters') + sys.exit(0) + + # Get state for the persistent parameters + print('-- All persistent parameters --') + + def state_callback(complete_name, state): + print(complete_name, state) + + persistent_params.pop(0) + if len(persistent_params) > 0: + scf.cf.param.persistent_get_state(persistent_params[0], state_callback) + else: + wait_for_callback_event.set() + + wait_for_callback_event.clear() + # Request the state for the first parameter. The callback will pop the name from the list + # and request the next + scf.cf.param.persistent_get_state(persistent_params[0], state_callback) + # Wait for all parameters to be done + wait_for_callback_event.wait() + + print() + print('-- Set the default LED ring effect --') + led_ring_param_name = 'ring.effect' + scf.cf.param.set_value(led_ring_param_name, 7) + + def is_stored_callback(): + print('New LED ring effect is persisted') + wait_for_callback_event.set() + + wait_for_callback_event.clear() + scf.cf.param.persistent_store(led_ring_param_name, callback=is_stored_callback) + wait_for_callback_event.wait() From 7d50f339cdfba1553d86077f60b8e1b49e42db91 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 289/861] param: Correct struct.unpack() usage --- cflib/crazyflie/param.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index b5f83f970..d424cd3b1 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -437,9 +437,11 @@ def new_packet_cb(pk): is_stored = pk.data[3] == 1 if not is_stored: - default_value = struct.unpack(f'<{element.pytype}') + default_value, = struct.unpack(element.pytype, pk.data[4:]) else: - default_value, stored_value = struct.unpack(f'<{element.pytype}*2') + # Remove little-endian indicator ('<') + just_type = element.pytype[1:] + default_value, stored_value = struct.unpack(f'<{just_type * 2}', pk.data[4:]) callback(complete_name, PersistentParamState( From cf747b15ebfd9d6e92bb4b6619b65db06e83c104 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 290/861] toccache: Add parameter extended and persistent --- cflib/crazyflie/toccache.py | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/cflib/crazyflie/toccache.py b/cflib/crazyflie/toccache.py index 2bd8602dd..5b9258bc7 100644 --- a/cflib/crazyflie/toccache.py +++ b/cflib/crazyflie/toccache.py @@ -100,13 +100,20 @@ def insert(self, crc, toc): def _encoder(self, obj): """ Encode a toc element leaf-node """ - return {'__class__': obj.__class__.__name__, - 'ident': obj.ident, - 'group': obj.group, - 'name': obj.name, - 'ctype': obj.ctype, - 'pytype': obj.pytype, - 'access': obj.access} + encoded = { + '__class__': obj.__class__.__name__, + 'ident': obj.ident, + 'group': obj.group, + 'name': obj.name, + 'ctype': obj.ctype, + 'pytype': obj.pytype, + 'access': obj.access, + } + + if isinstance(obj, ParamTocElement): + encoded['extended'] = obj.extended + + return encoded raise TypeError(repr(obj) + ' is not JSON serializable') def _decoder(self, obj): @@ -119,5 +126,7 @@ def _decoder(self, obj): elem.ctype = str(obj['ctype']) elem.pytype = str(obj['pytype']) elem.access = obj['access'] + if isinstance(obj, ParamTocElement): + elem.extended = obj['extended'] return elem return obj From 914f8e4a1470be519e41681e1e71054d05cddbfe Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 291/861] Check type of elem when decoding param TOC --- cflib/crazyflie/toccache.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/toccache.py b/cflib/crazyflie/toccache.py index 5b9258bc7..4576f92b4 100644 --- a/cflib/crazyflie/toccache.py +++ b/cflib/crazyflie/toccache.py @@ -126,7 +126,7 @@ def _decoder(self, obj): elem.ctype = str(obj['ctype']) elem.pytype = str(obj['pytype']) elem.access = obj['access'] - if isinstance(obj, ParamTocElement): + if isinstance(elem, ParamTocElement): elem.extended = obj['extended'] return elem return obj From b81a968bc60c4d330d42641afd761665ec768b50 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 292/861] Reversed order of default and stored values --- cflib/crazyflie/param.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index d424cd3b1..a1c724fd8 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -441,13 +441,13 @@ def new_packet_cb(pk): else: # Remove little-endian indicator ('<') just_type = element.pytype[1:] - default_value, stored_value = struct.unpack(f'<{just_type * 2}', pk.data[4:]) + stored_value, default_value = struct.unpack(f'<{just_type * 2}', pk.data[4:]) callback(complete_name, PersistentParamState( is_stored, default_value, - None if not is_stored else stored_value + stored_value if is_stored else None ) ) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) From 14fa3858d874fc6354c60b47c1faf6b6c860a571 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 293/861] param: Raise AttributeError if not persistent When doing persitent param operations, check that the parameter is persistent. Otherwise throw exception. --- cflib/crazyflie/param.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index a1c724fd8..a2873d3f9 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -371,6 +371,8 @@ def persistent_clear(self, complete_name, callback=None): @param callback Optional callback should take `complete_name` and boolean status as arguments """ element = self.toc.get_element_by_complete_name(complete_name) + if not element.is_persistent(): + raise AttributeError(f"Param '{complete_name}' is not persistent") def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_CLEAR: @@ -395,6 +397,8 @@ def persistent_store(self, complete_name, callback=None): @param callback Optional callback should take `complete_name` and boolean status as arguments """ element = self.toc.get_element_by_complete_name(complete_name) + if not element.is_persistent(): + raise AttributeError(f"Param '{complete_name}' is not persistent") def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_STORE: @@ -427,6 +431,8 @@ def persistent_get_state(self, complete_name, callback): @param callback Callback, takes `complete_name` and PersistentParamState namedtuple as arg """ element = self.toc.get_element_by_complete_name(complete_name) + if not element.is_persistent(): + raise AttributeError(f"Param '{complete_name}' is not persistent") def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_GET_STATE: From c2047c2b99d03980eeac437d2929fa268f3d8b6d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 294/861] param: Do not have type_fetcher as member --- cflib/crazyflie/param.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index a2873d3f9..08bf36123 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -158,9 +158,6 @@ def __init__(self, crazyflie): self.cf, self._useV2, self._param_updated) self.param_updater.start() - self.extended_type_fetcher = _ExtendedTypeFetcher(self.cf, self.toc) - self.extended_type_fetcher.start() - self.cf.disconnected.add_callback(self._disconnected) self.all_updated = Caller() @@ -270,8 +267,10 @@ def refresh_done(): extended_elements.append(element) if len(extended_elements) > 0: - self.extended_type_fetcher.set_callback(refresh_done_callback) - self.extended_type_fetcher.request_extended_types(extended_elements) + extended_type_fetcher = _ExtendedTypeFetcher(self.cf, self.toc) + extended_type_fetcher.start() + extended_type_fetcher.set_callback(refresh_done_callback) + extended_type_fetcher.request_extended_types(extended_elements) else: refresh_done_callback() From b2378dec4ac22275494b19b2b059242077476ffc Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 295/861] Swapped back stored and default values --- cflib/crazyflie/param.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 08bf36123..457a4caca 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -446,7 +446,7 @@ def new_packet_cb(pk): else: # Remove little-endian indicator ('<') just_type = element.pytype[1:] - stored_value, default_value = struct.unpack(f'<{just_type * 2}', pk.data[4:]) + default_value, stored_value = struct.unpack(f'<{just_type * 2}', pk.data[4:]) callback(complete_name, PersistentParamState( From 04bff62bc02c1d45099b7196669419be503b22ff Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 296/861] Updated example for persistent parameters --- examples/parameters/persistent_params.py | 123 ++++++++++++++++------- 1 file changed, 87 insertions(+), 36 deletions(-) diff --git a/examples/parameters/persistent_params.py b/examples/parameters/persistent_params.py index fa4014804..7376d273f 100644 --- a/examples/parameters/persistent_params.py +++ b/examples/parameters/persistent_params.py @@ -22,8 +22,9 @@ """ Example to show the use of persistent parameters. -All persistent parameters are fectched and the current state is printed out. -Finallay the LED ring effect is set to a new value and stored. +* All persistent parameters are fectched and the current state is printed out. +* The LED ring effect is set to a new value and stored. +* The LED ring effect persisted value is cleared. Note: this script will change the value of the LED ring.effect parameter """ @@ -35,62 +36,112 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper +import time - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') +# uri = uri_helper.uri_from_env(default='usb://0') +uri = uri_helper.uri_from_env(default='radio://0/30/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) +def get_persistent_state(cf, complete_param_name): + wait_for_callback_event = Event() + + def state_callback(complete_name, state): + print(f'{complete_name}: {state}') + wait_for_callback_event.set() + + cf.param.persistent_get_state(complete_param_name, state_callback) + wait_for_callback_event.wait() + + +def persist_parameter(cf, complete_param_name): + wait_for_callback_event = Event() + + def is_stored_callback(complete_name, success): + if success: + print(f'Persisted {complete_name}!') + else: + print(f'Failed to persist {complete_name}!') + wait_for_callback_event.set() + + cf.param.persistent_store(complete_param_name, callback=is_stored_callback) + wait_for_callback_event.wait() + + +def clear_persistent_parameter(cf, complete_param_name): + wait_for_callback_event = Event() + + def is_stored_cleared(complete_name, success): + if success: + print(f'Cleared {complete_name}!') + else: + print(f'Failed to clear {complete_name}!') + wait_for_callback_event.set() + + cf.param.persistent_clear(complete_param_name, callback=is_stored_cleared) + wait_for_callback_event.wait() + + +def get_all_persistent_param_names(cf): + persistent_params = [] + for group_name, params in cf.param.toc.toc.items(): + for param_name, element in params.items(): + if element.is_persistent(): + complete_name = group_name + '.' + param_name + persistent_params.append(complete_name) + + return persistent_params + +def all_params_are_fetched(): + print("Got all!") + if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() cf = Crazyflie(rw_cache='./cache') + + fetch_all_params = Event() + cf.param.all_updated.add_callback(lambda: fetch_all_params.set()) + with SyncCrazyflie(uri, cf=cf) as scf: - wait_for_callback_event = Event() + # Wait for all parameters to be fetched + fetch_all_params.wait() - # Collect the names of all persistent parameters - persistent_params = [] - for group_name, params in scf.cf.param.toc.toc.items(): - for param_name, element in params.items(): - if element.is_persistent(): - complete_name = group_name + '.' + param_name - persistent_params.append(complete_name) + # Get the names of all parameters that can be persisted + persistent_params = get_all_persistent_param_names(scf.cf) - if len(persistent_params) == 0: + if not persistent_params: print('No persistent parameters') sys.exit(0) - # Get state for the persistent parameters + # Get the state of all persistent parameters print('-- All persistent parameters --') + for complete_name in persistent_params: + get_persistent_state(scf.cf, complete_name) - def state_callback(complete_name, state): - print(complete_name, state) + print() + param_name = 'ring.effect' + param_value = 10 - persistent_params.pop(0) - if len(persistent_params) > 0: - scf.cf.param.persistent_get_state(persistent_params[0], state_callback) - else: - wait_for_callback_event.set() + print(f'Set parameter {param_name} to {param_value}') + scf.cf.param.set_value(param_name, param_value) - wait_for_callback_event.clear() - # Request the state for the first parameter. The callback will pop the name from the list - # and request the next - scf.cf.param.persistent_get_state(persistent_params[0], state_callback) - # Wait for all parameters to be done - wait_for_callback_event.wait() + # Wait a bit to make sure the parameter has been set + time.sleep(0.1) print() - print('-- Set the default LED ring effect --') - led_ring_param_name = 'ring.effect' - scf.cf.param.set_value(led_ring_param_name, 7) + print("Store the new value in persistent memory") + persist_parameter(scf.cf, param_name) - def is_stored_callback(): - print('New LED ring effect is persisted') - wait_for_callback_event.set() + print("The new state is:") + get_persistent_state(scf.cf, param_name) + + print() + print("Clear the persisted parameter") + clear_persistent_parameter(scf.cf, param_name) - wait_for_callback_event.clear() - scf.cf.param.persistent_store(led_ring_param_name, callback=is_stored_callback) - wait_for_callback_event.wait() + print("The new state is:") + get_persistent_state(scf.cf, param_name) From 10b2276aacf11dcd2ec8a224dc5fc0a42b4141f9 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 13:58:38 +0100 Subject: [PATCH 297/861] pep8 and clean up --- examples/parameters/persistent_params.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/examples/parameters/persistent_params.py b/examples/parameters/persistent_params.py index 7376d273f..2bf5be97c 100644 --- a/examples/parameters/persistent_params.py +++ b/examples/parameters/persistent_params.py @@ -30,13 +30,13 @@ """ import logging import sys +import time from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -import time # uri = uri_helper.uri_from_env(default='usb://0') uri = uri_helper.uri_from_env(default='radio://0/30/2M/E7E7E7E7E7') @@ -94,8 +94,6 @@ def get_all_persistent_param_names(cf): return persistent_params -def all_params_are_fetched(): - print("Got all!") if __name__ == '__main__': # Initialize the low-level drivers @@ -133,15 +131,15 @@ def all_params_are_fetched(): time.sleep(0.1) print() - print("Store the new value in persistent memory") + print('Store the new value in persistent memory') persist_parameter(scf.cf, param_name) - print("The new state is:") + print('The new state is:') get_persistent_state(scf.cf, param_name) print() - print("Clear the persisted parameter") + print('Clear the persisted parameter') clear_persistent_parameter(scf.cf, param_name) - print("The new state is:") + print('The new state is:') get_persistent_state(scf.cf, param_name) From 99cc45726efc675dd9786c0a574061edeb0c62b6 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Dec 2021 14:14:19 +0100 Subject: [PATCH 298/861] No need to check that the param values are downloaded --- examples/parameters/persistent_params.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/examples/parameters/persistent_params.py b/examples/parameters/persistent_params.py index 2bf5be97c..677724820 100644 --- a/examples/parameters/persistent_params.py +++ b/examples/parameters/persistent_params.py @@ -101,13 +101,7 @@ def get_all_persistent_param_names(cf): cf = Crazyflie(rw_cache='./cache') - fetch_all_params = Event() - cf.param.all_updated.add_callback(lambda: fetch_all_params.set()) - with SyncCrazyflie(uri, cf=cf) as scf: - # Wait for all parameters to be fetched - fetch_all_params.wait() - # Get the names of all parameters that can be persisted persistent_params = get_all_persistent_param_names(scf.cf) From bada71bb7a18126e7e1929d42d3bd58cbd5bd57d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 2 Dec 2021 13:11:34 +0000 Subject: [PATCH 299/861] param: Send persistent param commands with param_updater To make sure they get treated in the same queue and do not race against each other. --- cflib/crazyflie/param.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 457a4caca..515083142 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -384,7 +384,7 @@ def new_packet_cb(pk): pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack(' Date: Thu, 2 Dec 2021 14:16:07 +0100 Subject: [PATCH 300/861] persistent_params.py: No need to sleep after set --- examples/parameters/persistent_params.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/examples/parameters/persistent_params.py b/examples/parameters/persistent_params.py index 677724820..b0780ce19 100644 --- a/examples/parameters/persistent_params.py +++ b/examples/parameters/persistent_params.py @@ -121,9 +121,6 @@ def get_all_persistent_param_names(cf): print(f'Set parameter {param_name} to {param_value}') scf.cf.param.set_value(param_name, param_value) - # Wait a bit to make sure the parameter has been set - time.sleep(0.1) - print() print('Store the new value in persistent memory') persist_parameter(scf.cf, param_name) From 81184b4e65fb185e2676df10f067332d6928902e Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 3 Dec 2021 09:24:20 +0100 Subject: [PATCH 301/861] param: Fix pep8/flake8 issues --- cflib/crazyflie/param.py | 2 -- examples/parameters/persistent_params.py | 1 - 2 files changed, 3 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 515083142..7e6b72f07 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -570,13 +570,11 @@ def request_param_setvalue(self, pk): the Crazyflie it will answer with the update param value. """ self.request_queue.put(pk) - def send_param_misc(self, pk): """Place a param misc request on the queue. When this is sent to the Crazyflie it will answer with the same var_id and command. """ self.request_queue.put(pk) - def _new_packet_cb(self, pk): """Callback for newly arrived packets""" if pk.channel == READ_CHANNEL or pk.channel == WRITE_CHANNEL: diff --git a/examples/parameters/persistent_params.py b/examples/parameters/persistent_params.py index b0780ce19..338557b7b 100644 --- a/examples/parameters/persistent_params.py +++ b/examples/parameters/persistent_params.py @@ -30,7 +30,6 @@ """ import logging import sys -import time from threading import Event import cflib.crtp From 9b91bda117e9a728fe3afafe0f2e4f4aacfb65aa Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Dec 2021 06:39:24 +0100 Subject: [PATCH 302/861] set_value_raw: wait for initialization --- cflib/crazyflie/param.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 7e6b72f07..d10860be1 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -301,6 +301,9 @@ def set_value_raw(self, complete_name, type, value): Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ + if not self._initialized.wait(timeout=60): + raise Exception('Connection timed out') + char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) From 4517bc2c4bdb253eb9a3d2b594bbdc833dbde994 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 6 Dec 2021 12:57:39 +0100 Subject: [PATCH 303/861] param: Use send_param_misc for set_value_raw --- cflib/crazyflie/param.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index d10860be1..ff3cc2f10 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -314,9 +314,7 @@ def set_value_raw(self, complete_name, type, value): pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack(f' Date: Mon, 6 Dec 2021 14:55:44 +0100 Subject: [PATCH 304/861] param: Change order of "all updated" check If we call all callbacks first we end up with "deadlock" if a callback tries to set parameters, since that waits for self._initialized. --- cflib/crazyflie/param.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index ff3cc2f10..04f566eb9 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +# !/usr/bin/env python # -*- coding: utf-8 -*- # # || ____ _ __ @@ -46,7 +46,6 @@ from cflib.crtp.crtpstack import CRTPPort from cflib.utils.callbacks import Caller - __author__ = 'Bitcraze AB' __all__ = ['Param', 'ParamTocElement'] @@ -205,6 +204,13 @@ def _param_updated(self, pk): self.values[element.group] = {} self.values[element.group][element.name] = s + # Once all the parameters are updated call the + # callback for "everything updated" + if self._check_if_all_updated() and not self.is_updated: + self.is_updated = True + self._initialized.set() + self.all_updated.call() + logger.debug('Updated parameter [%s]' % complete_name) if complete_name in self.param_update_callbacks: self.param_update_callbacks[complete_name].call( @@ -213,14 +219,6 @@ def _param_updated(self, pk): self.group_update_callbacks[element.group].call( complete_name, s) self.all_update_callback.call(complete_name, s) - - # Once all the parameters are updated call the - # callback for "everything updated" (after all the param - # updated callbacks) - if self._check_if_all_updated() and not self.is_updated: - self.is_updated = True - self._initialized.set() - self.all_updated.call() else: logger.debug('Variable id [%d] not found in TOC', var_id) From f8cf5285c1bdccc63de15c0e3e2f5ad981e52b49 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 7 Dec 2021 06:55:16 +0100 Subject: [PATCH 305/861] Revert "param: Use send_param_misc for set_value_raw" This reverts commit 4517bc2c4bdb253eb9a3d2b594bbdc833dbde994. --- cflib/crazyflie/param.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 04f566eb9..da97e2c54 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -312,7 +312,9 @@ def set_value_raw(self, complete_name, type, value): pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack(f' Date: Tue, 7 Dec 2021 06:55:26 +0100 Subject: [PATCH 306/861] Revert "set_value_raw: wait for initialization" This reverts commit 9b91bda117e9a728fe3afafe0f2e4f4aacfb65aa. --- cflib/crazyflie/param.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index da97e2c54..f721fd805 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -299,9 +299,6 @@ def set_value_raw(self, complete_name, type, value): Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ - if not self._initialized.wait(timeout=60): - raise Exception('Connection timed out') - char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) From b768e2d8765ebaa7b6f712498fe8a2c11faf6214 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 14 Dec 2021 13:13:32 +0100 Subject: [PATCH 307/861] bootloader: Remove spurious print --- cflib/bootloader/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 7cd791da2..0d33d58d7 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -420,7 +420,6 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): if self.progress_cb: self.progress_cb(f'Updating deck {deck.name}', int(50)) - print(f'Handling {deck.name}') # Test and wait for the deck to be started while not deck.is_started: From eca8b7e62b514dc680f23868a5564bf0bbe01bb0 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 14 Dec 2021 10:53:41 +0100 Subject: [PATCH 308/861] Add progress callback to deck memory writes --- cflib/bootloader/__init__.py | 16 +++++++++++----- cflib/crazyflie/mem/__init__.py | 16 +++++++++++++--- cflib/crazyflie/mem/deck_memory.py | 13 +++++++------ 3 files changed, 31 insertions(+), 14 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 0d33d58d7..e1ec85304 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -390,7 +390,7 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): flash_all_targets = len(targets) == 0 if self.progress_cb: - self.progress_cb('Detecting deck to be updated', int(25)) + self.progress_cb('Detecting deck to be updated', 0) with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) @@ -419,7 +419,7 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): deck_artifact = deck_artifacts[0] if self.progress_cb: - self.progress_cb(f'Updating deck {deck.name}', int(50)) + self.progress_cb(f'Updating deck {deck.name}', 0) # Test and wait for the deck to be started while not deck.is_started: @@ -440,11 +440,17 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): print(f'Error: Deck {deck.name} bootloader not active, skipping!') continue - # ToDo, white the correct file there ... - result = deck.write_sync(0, deck_artifact.content) + progress_cb = self.progress_cb + if not progress_cb: + def progress_cb(msg: str, percent: int): + frames = ['◢', '◣', '◤', '◥'] + frame = frames[int(percent) % 4] + print('{} {}% {}'.format(frame, percent, msg)) + + result = deck.write_sync(0, deck_artifact.content, progress_cb) if result: if self.progress_cb: - self.progress_cb(f'Deck {deck.name} updated succesfully!', int(75)) + self.progress_cb(f'Deck {deck.name} updated succesfully!', 100) else: if self.progress_cb: self.progress_cb(f'Failed to update deck {deck.name}', int(0)) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 03fcef6a3..7cf8f06d8 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -143,14 +143,17 @@ class _WriteRequest: """ MAX_DATA_LENGTH = 25 - def __init__(self, mem, addr, data, cf): + def __init__(self, mem, addr, data, cf, progress_cb=None): """Initialize the object with good defaults""" self.mem = mem self.addr = addr self._bytes_left = len(data) + self._write_len = self._bytes_left self._data = data self.data = bytearray() self.cf = cf + self._progress_cb = progress_cb + self._progress = -1 self._current_addr = addr @@ -195,6 +198,7 @@ def _write_new_chunk(self): self.cf.send_packet(pk, expected_reply=reply, timeout=1) self._addr_add = len(data) + self._bytes_left -= self._addr_add def write_done(self, addr): """Callback when data is received from the Crazyflie""" @@ -203,6 +207,12 @@ def write_done(self, addr): 'Address did not match when adding data to read request!') return + if self._progress_cb is not None: + new_progress = 100 * (self._write_len - self._bytes_left) / self._write_len + if int(new_progress) > self._progress: + self._progress = int(new_progress) + self._progress_cb(f'Writing to memory (id: {self.mem.id})', + self._progress) if len(self._data) > 0: self._current_addr += self._addr_add self._write_new_chunk() @@ -296,9 +306,9 @@ def ow_search(self, vid=0xBC, pid=None, name=None): return None - def write(self, memory, addr, data, flush_queue=False): + def write(self, memory, addr, data, flush_queue=False, progress_cb=None): """Write the specified data to the given memory at the given address""" - wreq = _WriteRequest(memory, addr, data, self.cf) + wreq = _WriteRequest(memory, addr, data, self.cf, progress_cb) if memory.id not in self._write_requests: self._write_requests[memory.id] = [] diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 93febc393..72ecd90ef 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -51,19 +51,20 @@ def __init__(self, deck_memory_manager): self._base_address = None self._bit_field = 0 - def write(self, address, data, write_complete_cb, write_failed_cb=None): + def write(self, address, data, write_complete_cb, write_failed_cb=None, progress_cb=None): """Write a block of binary data to the deck""" if not self.supports_write: raise Exception('Deck does not support write operations') if not self.is_started: raise Exception('Deck not ready') - self._deck_memory_manager._write(self._base_address, address, data, write_complete_cb, write_failed_cb) + self._deck_memory_manager._write(self._base_address, address, data, + write_complete_cb, write_failed_cb, progress_cb) - def write_sync(self, address, data): + def write_sync(self, address, data, progress_cb=None): """Write a block of binary data to the deck, block until done""" syncer = Syncer() - self.write(address, data, syncer.success_cb, write_failed_cb=syncer.failure_cb) + self.write(address, data, syncer.success_cb, write_failed_cb=syncer.failure_cb, progress_cb=progress_cb) syncer.wait() return syncer.is_success @@ -225,7 +226,7 @@ def _parse_info_section(self, data): return result - def _write(self, base_address, address, data, read_complete_cb, read_failed_cb): + def _write(self, base_address, address, data, read_complete_cb, read_failed_cb, progress_cb): """Called from deck memory to write data""" if self._write_complete_cb is not None: raise Exception('Write operation ongoing') @@ -235,7 +236,7 @@ def _write(self, base_address, address, data, read_complete_cb, read_failed_cb): self._write_failed_cb = read_failed_cb mapped_address = address + self._write_base_address - self.mem_handler.write(self, mapped_address, data, flush_queue=True) + self.mem_handler.write(self, mapped_address, data, flush_queue=True, progress_cb=progress_cb) def _write_done(self, mem, addr): if mem.id == self.id: From c9572bbd39217d944fd541487aed1f87ccf34373 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 15 Dec 2021 09:45:03 +0100 Subject: [PATCH 309/861] Memory: Special case progress message MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ''' $ python3 -m cfloader flash -w $URI aideck-NINA-firmware.bin deck-bcAI-fw Reset to bootloader mode ... ◢ 0% Writing to bcAI deck memory ◣ 1% Writing to bcAI deck memory ◤ 2% Writing to bcAI deck memory ◥ 3% Writing to bcAI deck memory ◢ 4% Writing to bcAI deck memory ◣ 5% Writing to bcAI deck memory ◤ 6% Writing to bcAI deck memory ◥ 7% Writing to bcAI deck memory ◢ 8% Writing to bcAI deck memory ◣ 9% Writing to bcAI deck memory ◤ 10% Writing to bcAI deck memory ``` --- cflib/crazyflie/mem/__init__.py | 18 +++++++++++++----- cflib/crazyflie/mem/deck_memory.py | 6 ++++++ 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 7cf8f06d8..b232b6e75 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -200,6 +200,14 @@ def _write_new_chunk(self): self._addr_add = len(data) self._bytes_left -= self._addr_add + def _get_progress_message(self): + if isinstance(self.mem, DeckMemoryManager): + for deck_memory in self.mem.deck_memories.values(): + if deck_memory.contains(self._current_addr): + return f'Writing to {deck_memory.name} deck memory' + + return 'Writing to memory' + def write_done(self, addr): """Callback when data is received from the Crazyflie""" if not addr == self._current_addr: @@ -208,11 +216,11 @@ def write_done(self, addr): return if self._progress_cb is not None: - new_progress = 100 * (self._write_len - self._bytes_left) / self._write_len - if int(new_progress) > self._progress: - self._progress = int(new_progress) - self._progress_cb(f'Writing to memory (id: {self.mem.id})', - self._progress) + new_progress = int(100 * (self._write_len - self._bytes_left) / self._write_len) + if new_progress > self._progress: + self._progress = new_progress + self._progress_cb(self._get_progress_message(), self._progress) + if len(self._data) > 0: self._current_addr += self._addr_add self._write_new_chunk() diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 72ecd90ef..dd777c57a 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -42,6 +42,8 @@ class DeckMemory: MASK_UPGRADE_REQUIRED = 32 MASK_BOOTLOADER_ACTIVE = 64 + MEMORY_MAX_SIZE = 0x10000000 + def __init__(self, deck_memory_manager): self._deck_memory_manager = deck_memory_manager self.required_hash = None @@ -51,6 +53,10 @@ def __init__(self, deck_memory_manager): self._base_address = None self._bit_field = 0 + def contains(self, address): + max = self._base_address + self.MEMORY_MAX_SIZE + return address >= self._base_address and address <= max + def write(self, address, data, write_complete_cb, write_failed_cb=None, progress_cb=None): """Write a block of binary data to the deck""" if not self.supports_write: From 5f3417c34e99620c8266c05bddb4eedd595255cf Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 20 Dec 2021 07:13:18 +0100 Subject: [PATCH 310/861] param: Add get_default_value method --- cflib/crazyflie/param.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index f721fd805..f45ff78fc 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -70,6 +70,7 @@ MISC_PERSISTENT_STORE = 3 MISC_PERSISTENT_GET_STATE = 4 MISC_PERSISTENT_CLEAR = 5 +MISC_GET_DEFAULT_VALUE = 6 # One element entry in the TOC @@ -358,6 +359,35 @@ def get_value(self, complete_name, timeout=60): [group, name] = complete_name.split('.') return self.values[group][name] + def get_default_value(self, complete_name, callback): + """ + Get the default value of the specified parameter. + The supplied callback will be called with the name of the parameter + as well as the default value. None if there is an error. + + @param complete_name The 'group.name' name of the parameter to store + @param callback The callback should take `complete_name` and default value as argument + """ + element = self.toc.get_element_by_complete_name(complete_name) + + def new_packet_cb(pk): + if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_GET_DEFAULT_VALUE: + if pk.data[3] == errno.ENOENT: + callback(complete_name, None) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + return + + default_value, = struct.unpack(element.pytype, pk.data[3:]) + callback(complete_name, default_value) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + + self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) + + pk = CRTPPacket() + pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) + pk.data = struct.pack(' Date: Thu, 23 Dec 2021 11:10:41 +0100 Subject: [PATCH 311/861] Examples: Add console.py This adds a simple example of using the console class to receive DEBUG_PRINTS from the Crazyflie. Sample output: ``` $ CFLIB_URI=radio://0/10/2M/DEADBEEF python3 examples/console/console.py SYS: ---------------------------- SYS: Crazyflie 2.1 is up and running! SYS: Production release 2021.06 SYS: I am 0x203937434848500D00470039 and I have 1024KB of flash! CFGBLK: v1, verification [OK] DECK_CORE: 1 deck(s) found DECK_CORE: Calling INIT on driver bcFlow2 for deck 0 ZR2: Z-down sensor [OK] PMW: Motion chip id: 0x49:0xB6 IMU: BMI088: Using I2C interface. IMU: BMI088 Gyro connection [OK]. IMU: BMI088 Accel connection [OK] IMU: BMP388 I2C connection [OK] ESTIMATOR: Using Kalman (2) estimator CONTROLLER: Using PID (1) controller MTR-DRV: Using brushed motor driver SYS: About to run tests in system.c. EEPROM: I2C connection [OK]. STORAGE: Storage check [OK]. IMU: BMI088 gyro self-test [OK] DECK_CORE: Deck 0 test [OK]. SYS: Self test passed! STAB: Wait for sensor calibration... SYS: Free heap: 17672 bytes [host] Connected, use ctrl-c to quit. ``` --- examples/console/console.py | 61 +++++++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 examples/console/console.py diff --git a/examples/console/console.py b/examples/console/console.py new file mode 100644 index 000000000..0b9ef16de --- /dev/null +++ b/examples/console/console.py @@ -0,0 +1,61 @@ +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 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) From 5b64572a1b5c52dbe0b500536c5a77807ac3e6e6 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 11 Jan 2022 06:44:02 +0100 Subject: [PATCH 312/861] Update GNU copying permission statement We do not want to keep drack of the Free Software Foundation office. Instead we use the recommened lines from https://www.gnu.org/licenses/gpl-howto.en.html --- cflib/__init__.py | 6 ++---- cflib/bootloader/__init__.py | 6 ++---- cflib/bootloader/boottypes.py | 6 ++---- cflib/bootloader/cloader.py | 6 ++---- cflib/crazyflie/__init__.py | 6 ++---- cflib/crazyflie/appchannel.py | 6 ++---- cflib/crazyflie/commander.py | 6 ++---- cflib/crazyflie/console.py | 6 ++---- cflib/crazyflie/extpos.py | 6 ++---- cflib/crazyflie/high_level_commander.py | 6 ++---- cflib/crazyflie/localization.py | 6 ++---- cflib/crazyflie/log.py | 6 ++---- cflib/crazyflie/mem/__init__.py | 6 ++---- cflib/crazyflie/param.py | 6 ++---- cflib/crazyflie/platformservice.py | 6 ++---- cflib/crazyflie/swarm.py | 6 ++---- cflib/crazyflie/syncCrazyflie.py | 6 ++---- cflib/crazyflie/syncLogger.py | 6 ++---- cflib/crazyflie/toc.py | 6 ++---- cflib/crazyflie/toccache.py | 6 ++---- cflib/crtp/__init__.py | 6 ++---- cflib/crtp/cflinkcppdriver.py | 6 ++---- cflib/crtp/crtpdriver.py | 6 ++---- cflib/crtp/exceptions.py | 6 ++---- cflib/crtp/radiodriver.py | 6 ++---- cflib/crtp/serialdriver.py | 6 ++---- cflib/crtp/udpdriver.py | 6 ++---- cflib/crtp/usbdriver.py | 6 ++---- cflib/drivers/__init__.py | 6 ++---- cflib/drivers/cfusb.py | 6 ++---- cflib/drivers/crazyradio.py | 6 ++---- cflib/localization/__init__.py | 6 ++---- cflib/positioning/__init__.py | 6 ++---- cflib/positioning/motion_commander.py | 6 ++---- cflib/positioning/position_hl_commander.py | 6 ++---- cflib/utils/__init__.py | 6 ++---- cflib/utils/callbacks.py | 6 ++---- cflib/utils/fp16.py | 6 ++---- cflib/utils/multiranger.py | 6 ++---- examples/autonomy/autonomousSequence.py | 6 ++---- examples/autonomy/autonomous_sequence_high_level.py | 6 ++---- .../autonomy/autonomous_sequence_high_level_compressed.py | 6 ++---- examples/autonomy/motion_commander_demo.py | 6 ++---- examples/autonomy/position_commander_demo.py | 6 ++---- examples/basicLedTiming.py | 6 ++---- examples/basicLedmemSync.py | 6 ++---- examples/basicLedparamSync.py | 6 ++---- examples/lighthouse/read_lighthouse_mem.py | 6 ++---- examples/lighthouse/write_lighthouse_mem.py | 6 ++---- examples/loco_nodes/lps_reboot_to_bootloader.py | 6 ++---- examples/logging/basiclog.py | 6 ++---- examples/logging/basiclogSync.py | 6 ++---- examples/memory/erase-ow.py | 6 ++---- examples/memory/flash-memory.py | 6 ++---- examples/memory/read-eeprom.py | 6 ++---- examples/memory/read-ow.py | 6 ++---- examples/memory/read_deck_mem.py | 6 ++---- examples/memory/write-eeprom.py | 6 ++---- examples/memory/write-ow.py | 6 ++---- examples/motors/multiramp.py | 6 ++---- examples/motors/ramp.py | 6 ++---- examples/multiranger/multiranger_pointcloud.py | 6 ++---- examples/multiranger/multiranger_push.py | 6 ++---- examples/parameters/basicparam.py | 6 ++---- examples/positioning/bezier_trajectory.py | 6 ++---- examples/positioning/flowsequenceSync.py | 6 ++---- examples/positioning/initial_position.py | 6 ++---- examples/positioning/matrix_light_printer.py | 6 ++---- examples/radio/scan.py | 6 ++---- examples/step-by-step/sbs_connect_log_param.py | 6 ++---- examples/step-by-step/sbs_motion_commander.py | 6 ++---- examples/swarm/hl-commander-swarm.py | 6 ++---- examples/swarm/swarmSequence.py | 6 ++---- examples/swarm/swarmSequenceCircle.py | 6 ++---- examples/swarm/synchronizedSequence.py | 6 ++---- examples/tuning/PID_controller_tuner.py | 6 ++---- lpslib/__init__.py | 6 ++---- lpslib/lopoanchor.py | 6 ++---- sys_test/single_cf_grounded/single_cf_grounded.py | 6 ++---- sys_test/single_cf_grounded/test_bootloader.py | 6 ++---- sys_test/single_cf_grounded/test_link.py | 6 ++---- sys_test/single_cf_grounded/test_power_switch.py | 6 ++---- sys_test/swarm_test_rig/__init__.py | 6 ++---- sys_test/swarm_test_rig/rig_support.py | 6 ++---- sys_test/swarm_test_rig/test_connection.py | 6 ++---- sys_test/swarm_test_rig/test_logging.py | 6 ++---- sys_test/swarm_test_rig/test_memory_map.py | 6 ++---- sys_test/swarm_test_rig/test_response_time.py | 6 ++---- test/crazyflie/test_localization.py | 6 ++---- test/crazyflie/test_swarm.py | 6 ++---- test/crazyflie/test_syncCrazyflie.py | 6 ++---- test/crazyflie/test_syncLogger.py | 6 ++---- test/crtp/test_crtpstack.py | 6 ++---- test/positioning/test_motion_commander.py | 6 ++---- test/positioning/test_position_hl_commander.py | 6 ++---- test/support/asyncCallbackCaller.py | 6 ++---- test/utils/test_callbacks.py | 6 ++---- test/utils/test_multiranger.py | 6 ++---- 98 files changed, 196 insertions(+), 392 deletions(-) diff --git a/cflib/__init__.py b/cflib/__init__.py index df7f0083e..ac9d3653d 100644 --- a/cflib/__init__.py +++ b/cflib/__init__.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ The Crazyflie Micro Quadcopter library API used to communicate with the Crazyflie Micro Quadcopter via a communication link. diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index e1ec85304..be93fc837 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Bootloading utilities for the Crazyflie. """ diff --git a/cflib/bootloader/boottypes.py b/cflib/bootloader/boottypes.py index 8a73f2543..0c8239ac6 100644 --- a/cflib/bootloader/boottypes.py +++ b/cflib/bootloader/boottypes.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Bootloading utilities for the Crazyflie. """ diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index e2ee99629..f0401b2bf 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Crazyflie radio bootloader for flashing firmware. """ diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 56771bea7..2e09afdf2 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ The Crazyflie module is used to easily connect/send/receive data from a Crazyflie. diff --git a/cflib/crazyflie/appchannel.py b/cflib/crazyflie/appchannel.py index 9a8206a90..e47ca5a1d 100644 --- a/cflib/crazyflie/appchannel.py +++ b/cflib/crazyflie/appchannel.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Data channel to communicate with an application running in the Crazyflie """ diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 405af8e3b..d7d86f703 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Used for sending control setpoints to the Crazyflie """ diff --git a/cflib/crazyflie/console.py b/cflib/crazyflie/console.py index 42cb84735..02df8fa6d 100644 --- a/cflib/crazyflie/console.py +++ b/cflib/crazyflie/console.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Crazyflie console is used to receive characters printed using printf from the firmware. diff --git a/cflib/crazyflie/extpos.py b/cflib/crazyflie/extpos.py index cf4ec4900..724fc6ea0 100644 --- a/cflib/crazyflie/extpos.py +++ b/cflib/crazyflie/extpos.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Used for sending external position to the Crazyflie """ diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 64cd2a11d..7966193e3 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Used for sending high level setpoints to the Crazyflie """ diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 9d0ce8367..072c6be95 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Subsytem handling localization-related data communication """ diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index f48dd0071..47e146a7a 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . # flake8: noqa """ Enables logging of variables from the Crazyflie. diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index b232b6e75..9dcd65753 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Enables access to the Crazyflie memory subsystem. diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index f45ff78fc..37650f31b 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Enables reading/writing of parameter values to/from the Crazyflie. diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index db0dbd219..b4f2c43ae 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Used for sending control setpoints to the Crazyflie """ diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index 96b196ba1..9bf8cd6d8 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time from collections import namedtuple from threading import Thread diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index 3245f0674..779e934c7 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ The synchronous Crazyflie class is a wrapper around the "normal" Crazyflie class. It handles the asynchronous nature of the Crazyflie API and turns it diff --git a/cflib/crazyflie/syncLogger.py b/cflib/crazyflie/syncLogger.py index 483e169a6..3735cc1f0 100644 --- a/cflib/crazyflie/syncLogger.py +++ b/cflib/crazyflie/syncLogger.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ This class provides synchronous access to log data from the Crazyflie. diff --git a/cflib/crazyflie/toc.py b/cflib/crazyflie/toc.py index e430cc49a..ea583dc23 100644 --- a/cflib/crazyflie/toc.py +++ b/cflib/crazyflie/toc.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ A generic TableOfContents module that is used to fetch, store and manipulate a TOC for logging or parameters. diff --git a/cflib/crazyflie/toccache.py b/cflib/crazyflie/toccache.py index 4576f92b4..3314b0818 100644 --- a/cflib/crazyflie/toccache.py +++ b/cflib/crazyflie/toccache.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Access the TOC cache for reading/writing. It supports both user cache and dist cache. diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index c52bcf1a3..b3fe1b77a 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """Scans and creates communication interfaces.""" import logging import os diff --git a/cflib/crtp/cflinkcppdriver.py b/cflib/crtp/cflinkcppdriver.py index ff8cccb1d..ab69f0773 100644 --- a/cflib/crtp/cflinkcppdriver.py +++ b/cflib/crtp/cflinkcppdriver.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Crazyflie driver using the cflinkcpp implementation. diff --git a/cflib/crtp/crtpdriver.py b/cflib/crtp/crtpdriver.py index 7d9f7d8cb..2f620fd30 100644 --- a/cflib/crtp/crtpdriver.py +++ b/cflib/crtp/crtpdriver.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ CRTP Driver main class. """ diff --git a/cflib/crtp/exceptions.py b/cflib/crtp/exceptions.py index 9ccb6af62..756a879a4 100644 --- a/cflib/crtp/exceptions.py +++ b/cflib/crtp/exceptions.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Exception used when the URI is not for the current driver (ie. radio:// for the serial driver ...) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index f67d3bc0a..a8e3f1707 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Crazyradio CRTP link driver. diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index ba78a059f..20b4b86fc 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ An early serial link driver. This could still be used (after some fixing) to run high-speed CRTP with the Crazyflie. The UART can be run at 2Mbit. diff --git a/cflib/crtp/udpdriver.py b/cflib/crtp/udpdriver.py index 17b66f6b5..949c88ec6 100644 --- a/cflib/crtp/udpdriver.py +++ b/cflib/crtp/udpdriver.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ CRTP UDP Driver. Work either with the UDP server or with an UDP device See udpserver.py for the protocol""" import queue diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index 2e29b2b1e..4e1e26ab8 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Crazyflie USB driver. diff --git a/cflib/drivers/__init__.py b/cflib/drivers/__init__.py index f0fd2d73c..fde700bec 100644 --- a/cflib/drivers/__init__.py +++ b/cflib/drivers/__init__.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Drivers for the link interfaces that can be used by CRTP. """ diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index f17b9eeca..9d9de54be 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ USB driver for the Crazyflie. """ diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 0d57c11d4..a87b96930 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ USB driver for the Crazyradio USB dongle. """ diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py index b39ec338f..7a9cc98d2 100644 --- a/cflib/localization/__init__.py +++ b/cflib/localization/__init__.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . from .lighthouse_bs_geo import LighthouseBsGeoEstimator from .lighthouse_bs_vector import LighthouseBsVector from .lighthouse_config_manager import LighthouseConfigFileManager diff --git a/cflib/positioning/__init__.py b/cflib/positioning/__init__.py index d776760fa..d00f05735 100644 --- a/cflib/positioning/__init__.py +++ b/cflib/positioning/__init__.py @@ -17,7 +17,5 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index c449405a2..40bc92f04 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ The MotionCommander is used to make it easy to write scripts that moves the Crazyflie around. Some sort of positioning support is required, for instance diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index def2d5675..75e9bd94a 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ The PositionHlCommander is used to make it easy to write scripts that moves the Crazyflie around. Some sort of positioning support is required, for diff --git a/cflib/utils/__init__.py b/cflib/utils/__init__.py index fd35e3624..726974d1c 100644 --- a/cflib/utils/__init__.py +++ b/cflib/utils/__init__.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Various utilities that is needed by the cflib. """ diff --git a/cflib/utils/callbacks.py b/cflib/utils/callbacks.py index e6babb31f..4efdf97ac 100644 --- a/cflib/utils/callbacks.py +++ b/cflib/utils/callbacks.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Callback objects used in the Crazyflie library """ diff --git a/cflib/utils/fp16.py b/cflib/utils/fp16.py index 83394ecf1..5f02649db 100644 --- a/cflib/utils/fp16.py +++ b/cflib/utils/fp16.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import struct diff --git a/cflib/utils/multiranger.py b/cflib/utils/multiranger.py index 84d254565..84ce5e152 100644 --- a/cflib/utils/multiranger.py +++ b/cflib/utils/multiranger.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index 2c9423575..a4e03e96d 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 (check the address at the top and update it to your crazyflie address) and send a sequence of setpoints, diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 45e04ee16..5c05f6a49 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 (check the address at the top and update it to your crazyflie address) and uses the high level commander diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index f44c3efde..f45b95b4d 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 (check the address at the top and update it to your crazyflie address) and uses the high level commander diff --git a/examples/autonomy/motion_commander_demo.py b/examples/autonomy/motion_commander_demo.py index ccc0e90d1..1f34e178a 100644 --- a/examples/autonomy/motion_commander_demo.py +++ b/examples/autonomy/motion_commander_demo.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 MotionCommander class. diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py index fb31cd47d..6f751a2c6 100644 --- a/examples/autonomy/position_commander_demo.py +++ b/examples/autonomy/position_commander_demo.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py index 144f5aad2..94d428e15 100644 --- a/examples/basicLedTiming.py +++ b/examples/basicLedTiming.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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, diff --git a/examples/basicLedmemSync.py b/examples/basicLedmemSync.py index 5676ad897..dec847798 100644 --- a/examples/basicLedmemSync.py +++ b/examples/basicLedmemSync.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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, diff --git a/examples/basicLedparamSync.py b/examples/basicLedparamSync.py index 15b078652..a205a2812 100644 --- a/examples/basicLedparamSync.py +++ b/examples/basicLedparamSync.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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, diff --git a/examples/lighthouse/read_lighthouse_mem.py b/examples/lighthouse/read_lighthouse_mem.py index 23b881964..7f46d160f 100644 --- a/examples/lighthouse/read_lighthouse_mem.py +++ b/examples/lighthouse/read_lighthouse_mem.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 diff --git a/examples/lighthouse/write_lighthouse_mem.py b/examples/lighthouse/write_lighthouse_mem.py index 1db49669f..5bc331e5c 100644 --- a/examples/lighthouse/write_lighthouse_mem.py +++ b/examples/lighthouse/write_lighthouse_mem.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 diff --git a/examples/loco_nodes/lps_reboot_to_bootloader.py b/examples/loco_nodes/lps_reboot_to_bootloader.py index 5a0c66339..10a07066f 100644 --- a/examples/loco_nodes/lps_reboot_to_bootloader.py +++ b/examples/loco_nodes/lps_reboot_to_bootloader.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 diff --git a/examples/logging/basiclog.py b/examples/logging/basiclog.py index 3031231d3..bf2a22b68 100644 --- a/examples/logging/basiclog.py +++ b/examples/logging/basiclog.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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, logs the Stabilizer and prints it to the console. After 10s the application disconnects and exits. diff --git a/examples/logging/basiclogSync.py b/examples/logging/basiclogSync.py index 9ad3d199c..529709c40 100644 --- a/examples/logging/basiclogSync.py +++ b/examples/logging/basiclogSync.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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, logs the Stabilizer and prints it to the console. After 10s the application disconnects and exits. diff --git a/examples/memory/erase-ow.py b/examples/memory/erase-ow.py index d672491a5..05070035e 100644 --- a/examples/memory/erase-ow.py +++ b/examples/memory/erase-ow.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/memory/flash-memory.py b/examples/memory/flash-memory.py index f0b761d8e..f79a4844d 100644 --- a/examples/memory/flash-memory.py +++ b/examples/memory/flash-memory.py @@ -11,10 +11,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. """ diff --git a/examples/memory/read-eeprom.py b/examples/memory/read-eeprom.py index 75eb5b944..e3239d6cf 100644 --- a/examples/memory/read-eeprom.py +++ b/examples/memory/read-eeprom.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/memory/read-ow.py b/examples/memory/read-ow.py index ae6c10f3e..785c14d1c 100644 --- a/examples/memory/read-ow.py +++ b/examples/memory/read-ow.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 1-wire memories and lists its contents. diff --git a/examples/memory/read_deck_mem.py b/examples/memory/read_deck_mem.py index 35dda99a8..4969bfe87 100644 --- a/examples/memory/read_deck_mem.py +++ b/examples/memory/read_deck_mem.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 """ diff --git a/examples/memory/write-eeprom.py b/examples/memory/write-eeprom.py index 2313cac8c..9dc0a7c8f 100644 --- a/examples/memory/write-eeprom.py +++ b/examples/memory/write-eeprom.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/memory/write-ow.py b/examples/memory/write-ow.py index 301f7c18e..c7b27ef93 100644 --- a/examples/memory/write-ow.py +++ b/examples/memory/write-ow.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/motors/multiramp.py b/examples/motors/multiramp.py index c1f29a9e0..404841e77 100644 --- a/examples/motors/multiramp.py +++ b/examples/motors/multiramp.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index d216755ed..d17261d4a 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/multiranger/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py index 3592999a0..1555b7a7e 100644 --- a/examples/multiranger/multiranger_pointcloud.py +++ b/examples/multiranger/multiranger_pointcloud.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/multiranger/multiranger_push.py b/examples/multiranger/multiranger_push.py index 66f4671de..9905d6472 100755 --- a/examples/multiranger/multiranger_push.py +++ b/examples/multiranger/multiranger_push.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/parameters/basicparam.py b/examples/parameters/basicparam.py index abc2f03ac..6dc18b713 100644 --- a/examples/parameters/basicparam.py +++ b/examples/parameters/basicparam.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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, triggers reading of all the parameters and displays their values. It then modifies diff --git a/examples/positioning/bezier_trajectory.py b/examples/positioning/bezier_trajectory.py index 9b26ad943..e2b8ca0cf 100644 --- a/examples/positioning/bezier_trajectory.py +++ b/examples/positioning/bezier_trajectory.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Example of how to generate trajectories for the High Level commander using Bezier curves. The output from this script is intended to be pasted into the diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py index 00d8a7449..1c7abd661 100644 --- a/examples/positioning/flowsequenceSync.py +++ b/examples/positioning/flowsequenceSync.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index 800f7dd01..cbe324abe 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index a53563c1f..a2b0207d8 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/radio/scan.py b/examples/radio/scan.py index fe7220790..6dd200d13 100644 --- a/examples/radio/scan.py +++ b/examples/radio/scan.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 and lists them. """ diff --git a/examples/step-by-step/sbs_connect_log_param.py b/examples/step-by-step/sbs_connect_log_param.py index 1bba06e89..c46b0ddc6 100644 --- a/examples/step-by-step/sbs_connect_log_param.py +++ b/examples/step-by-step/sbs_connect_log_param.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import logging import time diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index 63a51e245..d93507f37 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import logging import time diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index d1319c388..0adc53a08 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py index 91ca0cacb..5c715d610 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index a9c30d9e3..31475d77a 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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 diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index 9e462c3c9..1bf3d8556 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -18,10 +18,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# 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. diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 3d70bc101..5e575402f 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -20,10 +20,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ Gui une the PID controller of the crazyflie diff --git a/lpslib/__init__.py b/lpslib/__init__.py index 82be6a10e..8423e2bd7 100644 --- a/lpslib/__init__.py +++ b/lpslib/__init__.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ The LPS lib is an API that is used to communicate with Loco Positioning anchors. diff --git a/lpslib/lopoanchor.py b/lpslib/lopoanchor.py index e07d7f121..58432f5fa 100644 --- a/lpslib/lopoanchor.py +++ b/lpslib/lopoanchor.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . """ This class represents the connection to one or more Loco Positioning Anchors """ diff --git a/sys_test/single_cf_grounded/single_cf_grounded.py b/sys_test/single_cf_grounded/single_cf_grounded.py index 622117993..b11234630 100644 --- a/sys_test/single_cf_grounded/single_cf_grounded.py +++ b/sys_test/single_cf_grounded/single_cf_grounded.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest import cflib.crtp diff --git a/sys_test/single_cf_grounded/test_bootloader.py b/sys_test/single_cf_grounded/test_bootloader.py index 07649e61b..a3115251e 100644 --- a/sys_test/single_cf_grounded/test_bootloader.py +++ b/sys_test/single_cf_grounded/test_bootloader.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import unittest diff --git a/sys_test/single_cf_grounded/test_link.py b/sys_test/single_cf_grounded/test_link.py index 321e9369f..da3cefc7d 100644 --- a/sys_test/single_cf_grounded/test_link.py +++ b/sys_test/single_cf_grounded/test_link.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import struct import time import unittest diff --git a/sys_test/single_cf_grounded/test_power_switch.py b/sys_test/single_cf_grounded/test_power_switch.py index 3671f6186..9c0d82125 100644 --- a/sys_test/single_cf_grounded/test_power_switch.py +++ b/sys_test/single_cf_grounded/test_power_switch.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import unittest diff --git a/sys_test/swarm_test_rig/__init__.py b/sys_test/swarm_test_rig/__init__.py index c68f60fff..34118da25 100644 --- a/sys_test/swarm_test_rig/__init__.py +++ b/sys_test/swarm_test_rig/__init__.py @@ -17,7 +17,5 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . diff --git a/sys_test/swarm_test_rig/rig_support.py b/sys_test/swarm_test_rig/rig_support.py index 2755df777..7940373bc 100644 --- a/sys_test/swarm_test_rig/rig_support.py +++ b/sys_test/swarm_test_rig/rig_support.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time from cflib.utils.power_switch import PowerSwitch diff --git a/sys_test/swarm_test_rig/test_connection.py b/sys_test/swarm_test_rig/test_connection.py index 0df6dce98..e172c0340 100644 --- a/sys_test/swarm_test_rig/test_connection.py +++ b/sys_test/swarm_test_rig/test_connection.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import unittest diff --git a/sys_test/swarm_test_rig/test_logging.py b/sys_test/swarm_test_rig/test_logging.py index 2226f45b2..6b9f43ece 100644 --- a/sys_test/swarm_test_rig/test_logging.py +++ b/sys_test/swarm_test_rig/test_logging.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest import cflib.crtp diff --git a/sys_test/swarm_test_rig/test_memory_map.py b/sys_test/swarm_test_rig/test_memory_map.py index 5150c76c5..68c81733e 100644 --- a/sys_test/swarm_test_rig/test_memory_map.py +++ b/sys_test/swarm_test_rig/test_memory_map.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import unittest diff --git a/sys_test/swarm_test_rig/test_response_time.py b/sys_test/swarm_test_rig/test_response_time.py index ef9bdbe13..da9222fce 100644 --- a/sys_test/swarm_test_rig/test_response_time.py +++ b/sys_test/swarm_test_rig/test_response_time.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import unittest diff --git a/test/crazyflie/test_localization.py b/test/crazyflie/test_localization.py index 731865a89..7306f4b6a 100644 --- a/test/crazyflie/test_localization.py +++ b/test/crazyflie/test_localization.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import struct import unittest from unittest.mock import MagicMock diff --git a/test/crazyflie/test_swarm.py b/test/crazyflie/test_swarm.py index ca125f1fa..75fbeea98 100644 --- a/test/crazyflie/test_swarm.py +++ b/test/crazyflie/test_swarm.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest from unittest.mock import MagicMock diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index d9ef77aee..0a88b0f6d 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest from test.support.asyncCallbackCaller import AsyncCallbackCaller from unittest.mock import MagicMock diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index 3417fe397..c791df7ad 100644 --- a/test/crazyflie/test_syncLogger.py +++ b/test/crazyflie/test_syncLogger.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest from test.support.asyncCallbackCaller import AsyncCallbackCaller from unittest.mock import call diff --git a/test/crtp/test_crtpstack.py b/test/crtp/test_crtpstack.py index 903032da4..d52831c2e 100644 --- a/test/crtp/test_crtpstack.py +++ b/test/crtp/test_crtpstack.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest from cflib.crtp.crtpstack import CRTPPacket diff --git a/test/positioning/test_motion_commander.py b/test/positioning/test_motion_commander.py index 044b29ae6..879ec8e6f 100644 --- a/test/positioning/test_motion_commander.py +++ b/test/positioning/test_motion_commander.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import math import unittest from unittest.mock import call diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index 545f193e1..4d40ae7de 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import math import unittest from unittest.mock import call diff --git a/test/support/asyncCallbackCaller.py b/test/support/asyncCallbackCaller.py index bca42ccaf..cad18b5ec 100644 --- a/test/support/asyncCallbackCaller.py +++ b/test/support/asyncCallbackCaller.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time from threading import Thread diff --git a/test/utils/test_callbacks.py b/test/utils/test_callbacks.py index bd00a0800..5eff1ee4e 100644 --- a/test/utils/test_callbacks.py +++ b/test/utils/test_callbacks.py @@ -19,10 +19,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest from cflib.utils.callbacks import Caller diff --git a/test/utils/test_multiranger.py b/test/utils/test_multiranger.py index f79b45015..60bfcc505 100644 --- a/test/utils/test_multiranger.py +++ b/test/utils/test_multiranger.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import unittest from unittest.mock import call from unittest.mock import MagicMock From 2f26a27c7757d78313d28b923d19c7fadfa2eb14 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 11 Jan 2022 10:05:06 +0100 Subject: [PATCH 313/861] Use https:// instead of git:// in precommit-config The unauthenticated git protocol on port 9418 is no longer supported. --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e0c3343e9..b4c4462ea 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,4 +1,4 @@ -- repo: git://github.com/pre-commit/pre-commit-hooks +- repo: https://github.com/pre-commit/pre-commit-hooks rev: 7192665e31cea6ace58a71e086c7248d7e5610c2 hooks: - id: autopep8-wrapper @@ -15,7 +15,7 @@ - id: end-of-file-fixer - id: flake8 - id: requirements-txt-fixer -- repo: git://github.com/asottile/reorder_python_imports +- repo: https://github.com/asottile/reorder_python_imports rev: ab609b9b982729dfc287b4e75963c0c4de254a31 hooks: - id: reorder-python-imports From 896e490a1168666a45e2be34585ee909754f0758 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 3 Jan 2022 15:08:25 +0100 Subject: [PATCH 314/861] Added IPPE --- cflib/localization/_ippe.py | 408 ++++++++++++++++++++++++++++++++++ cflib/localization/ippe_cf.py | 121 ++++++++++ 2 files changed, 529 insertions(+) create mode 100644 cflib/localization/_ippe.py create mode 100644 cflib/localization/ippe_cf.py diff --git a/cflib/localization/_ippe.py b/cflib/localization/_ippe.py new file mode 100644 index 000000000..7ffd5b3ee --- /dev/null +++ b/cflib/localization/_ippe.py @@ -0,0 +1,408 @@ +import numpy as np + +# Copyright (c) 2016 The Python Packaging Authority (PyPA) + +# Permission is hereby granted, free of charge, to any person obtaining a copy of +# this software and associated documentation files (the "Software"), to deal in +# the Software without restriction, including without limitation the rights to +# use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +# of the Software, and to permit persons to whom the Software is furnished to do +# so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +# This code has been copied from https://github.com/royxue/ippe_py and slightly modified +# to avoid a dependency to Open CV and to collect all functionality in one file. + +# The code uses a coordinate system as defined by Open CV. + +# The code is based on the paper +# @article{ year={2014}, issn={0920-5691}, journal={International Journal of Computer Vision}, volume={109}, number={3}, doi={10.1007/s11263-014-0725-5}, title={Infinitesimal Plane-Based Pose Estimation}, url={http://dx.doi.org/10.1007/s11263-014-0725-5}, publisher={Springer US}, keywords={Plane; Pose; SfM; PnP; Homography}, author={Collins, Toby and Bartoli, Adrien}, pages={252-286}, language={English} } # noqa + + +def mat_run(U, Q, hEstMethod='DLT'): + """ + The solution to Perspective IPPE with point correspondences computed + between points in world coordinates on the plane z=0, and normalised points in the + camera's image. + + Parameters + ---------- + U: 2xN or 3xN matrix holding the model points in world coordinates. If U + is 2xN then the points are defined in world coordinates on the plane z=0 + + Q: 2xN matrix holding the points in the image. These are in normalised + pixel coordinates. That is, the effects of the camera's intrinsic matrix + and lens distortion are corrected, so that the Q projects with a perfect + pinhole model. + + hEstMethod: the homography estimation method, by default Direct Linear Transform is used, + from Peter Kovesi's implementation at http://www.csse.uwa.edu.au/~pk. + + + Return + --------- + IPPEPoses: A Python dictionary that contains 2 sets of pose solution from IPPE including rotation matrix + translation matrix, and reprojection error + """ + if hEstMethod not in ['Harker', 'DLT']: + print('hEstMethod Error') + + # Inputs shape checking + assert(U.shape[0] == 2 or U.shape[0] == 3) + assert(U.shape[1] == Q.shape[1]) + assert(Q.shape[0] == 2) + + n = U.shape[1] + modelDims = U.shape[0] + + Uuncentred = U + + if modelDims == 2: + # Zero center the model points + # Zero center the model points + Pbar = np.vstack((np.mean(U, axis=1, keepdims=True), 0)) + U[0, :] = U[0, :]-Pbar[0] + U[1, :] = U[1, :]-Pbar[1] + else: + # Rotate the model points onto the plane z=0 and zero center them + Pbar = np.mean(U[:1]) + MCenter = np.eye(4) + MCenter[0:3, -1] = -Pbar + U_ = MCenter[0:3, :].dot(np.vstack((U, np.ones((1, U.shape[1]))))) + modelRotation, sigs, _ = np.linalg.svd(U_.dot(U_.T)) + modelRotation = modelRotation.T + + modelRotation = np.hstack((np.vstack((modelRotation, np.array([0, 0, 0]))), np.array([[0], [0], [0], [1]]))) + Mcorrective = modelRotation.dot(MCenter) + U = Mcorrective[0:2, :].dot(np.vstack((U, np.ones((1, U.shape[1]))))) + + # TODO: Add support for Harker function + # Compute the model to image homography + if hEstMethod == 'DLT': + _U = np.vstack((U, np.ones((1, n)))) + _Q = np.vstack((Q, np.ones((1, n)))) + H = homography2d(_U, _Q) + + H = H/H[2, 2] + + # Compute the Jacobian J of the homography at (0,0) + J = np.zeros((2, 2)) + J[0, 0] = H[0, 0]-H[2, 0]*H[0, 2] + J[0, 1] = H[0, 1]-H[2, 1]*H[0, 2] + J[1, 0] = H[1, 0]-H[2, 0]*H[1, 2] + J[1, 1] = H[1, 1]-H[2, 1]*H[1, 2] + + # Compute rotate solution + v = np.vstack((H[0, 2], H[1, 2])) + [R1, R2, _] = IPPE_dec(v, J) + + # compute the translation solution + t1_ = estT(R1, U, Q) + t2_ = estT(R2, U, Q) + + if modelDims == 2: + t1 = np.hstack((R1, t1_)).dot(np.vstack((-Pbar, 1))) + t2 = np.hstack((R2, t2_)).dot(np.vstack((-Pbar, 1))) + else: + M1 = np.hstack((R1, t1_)) + M1 = np.vstack((M1, np.array([0, 0, 0, 1]))) + M2 = np.hstack((R2, t2_)) + M2 = np.vstack((M2, np.array([0, 0, 0, 1]))) + M1 = M1.dot(Mcorrective) + M2 = M2.dot(Mcorrective) + R1 = M1[0:3, 0:3] + R2 = M2[0:3, 0:3] + t1 = M1[0:3, -1] + t2 = M2[0:3, -1] + + [reprojErr1, reprojErr2] = computeReprojErrs(R1, R2, t1, t2, Uuncentred, Q) + + if reprojErr1 > reprojErr2: + [R1, R2, t1, t2, reprojErr1, reprojErr2] = swapSolutions(R1, R2, t1, t2, reprojErr1, reprojErr2) + + IPPEPoses = {} + IPPEPoses['R1'] = R1 + IPPEPoses['t1'] = t1 + IPPEPoses['R2'] = R2 + IPPEPoses['t2'] = t2 + IPPEPoses['reprojError1'] = reprojErr1 + IPPEPoses['reprojError2'] = reprojErr2 + + return IPPEPoses + + +def computeReprojErrs(R1, R2, t1, t2, U, Q): + """ + Computes the reprojection errors for the two solutions generated by IPPE. + """ + + # transform model points to camera coordinates and project them onto the image + if U.shape[0] == 2: + PCam1 = R1[:, 0:2].dot(U) + PCam2 = R2[:, 0:2].dot(U[0:2, :]) + else: + PCam1 = R1.dot(U) + PCam2 = R2.dot(U) + + PCam1[0, :] = PCam1[0, :] + t1[0] + PCam1[1, :] = PCam1[1, :] + t1[1] + PCam1[2, :] = PCam1[2, :] + t1[2] + + PCam2[0, :] = PCam2[0, :] + t2[0] + PCam2[1, :] = PCam2[1, :] + t2[1] + PCam2[2, :] = PCam2[2, :] + t2[2] + + Qest_1 = PCam1/np.vstack((PCam1[2, :], PCam1[2, :], PCam1[2, :])) + Qest_2 = PCam2/np.vstack((PCam2[2, :], PCam2[2, :], PCam2[2, :])) + + # Compute reprojection errors: + reprojErr1 = np.linalg.norm(Qest_1[0:2, :]-Q) + reprojErr2 = np.linalg.norm(Qest_2[0:2, :]-Q) + + return [reprojErr1, reprojErr2] + + +def estT(R, psPlane, Q): + """ + Computes the least squares estimate of translation given the rotation solution. + """ + if psPlane.shape[0] == 2: + psPlane = np.vstack((psPlane, np.zeros((1, psPlane.shape[1])))) + + Ps = R.dot(psPlane) + + numPts = psPlane.shape[1] + Ax = np.zeros((numPts, 3)) + bx = np.zeros((numPts, 1)) + + Ay = np.zeros((numPts, 3)) + by = np.zeros((numPts, 1)) + + Ax[:, 0] = 1 + Ax[:, 2] = -Q[0, :] + bx[:] = (Q[0, :]*Ps[2, :] - Ps[0, :]).reshape(4, 1) + + Ay[:, 1] = 1 + Ay[:, 2] = -Q[1, :] + by[:] = (Q[1, :]*Ps[2, :] - Ps[1, :]).reshape(4, 1) + + A = np.vstack((Ax, Ay)) + b = np.vstack((bx, by)) + + AtA = A.conj().T.dot(A) + Atb = A.conj().T.dot(b) + + Ainv = IPPE_inv33(AtA) + t = Ainv.dot(Atb) + return t + + +def swapSolutions(R1_, R2_, t1_, t2_, reprojErr1_, reprojErr2_): + """ + Swap the solutions + """ + + R1 = R2_ + t1 = t2_ + reprojErr1 = reprojErr2_ + + R2 = R1_ + t2 = t1_ + reprojErr2 = reprojErr1_ + + return [R1, R2, t1, t2, reprojErr1, reprojErr2] + + +def IPPE_inv33(A): + """ + Computes the inverse of a 3x3 matrix, assuming it is full-rank. + """ + a11 = A[0, 0] + a12 = A[0, 1] + a13 = A[0, 2] + + a21 = A[1, 0] + a22 = A[1, 1] + a23 = A[1, 2] + + a31 = A[2, 0] + a32 = A[2, 1] + a33 = A[2, 2] + + Ainv = np.vstack((np.array([a22*a33 - a23*a32, a13*a32 - a12*a33, a12*a23 - a13*a22]), + np.array([a23*a31 - a21*a33, a11*a33 - a13*a31, a13*a21 - a11*a23]), + np.array([a21*a32 - a22*a31, a12*a31 - a11*a32, a11*a22 - a12*a21]))) + Ainv = Ainv/(a11*a22*a33 - a11*a23*a32 - a12*a21*a33 + a12*a23*a31 + a13*a21*a32 - a13*a22*a31) + return Ainv + + +def IPPE_dec(v, J): + """ + Calculate 2 solutions to rotate from J Jacobian of the model-to-plane homography H + + Parameters + ---------- + v: 2x1 vector holding the point in normalised pixel coordinates which maps by H^-1 to + the point (0,0,0) in world coordinates. + J: 2x2 Jacobian matrix of H at (0,0). + + Return + --------- + R1: 3x3 Rotation matrix (first solution) + R2: 3x3 Rotation matrix (second solution) + gamma: The positive real-valued inverse-scale factor. + """ + + # Calculate the correction rotation Rv + t = np.linalg.norm(v) + + if t < np.finfo(float).eps: + # the plane is fronto-parallel to the camera, so set the corrective rotation Rv to identity. + # There will be only one solution to pose. + Rv = np.eye(3) + else: + # the plane is not fronto-parallel to the camera, so set the corrective rotation Rv + s = np.linalg.norm(np.vstack((v, 1))) + costh = 1./s + sinth = np.sqrt(1-1./s**2) + Kcrs = 1./t*(np.vstack([np.hstack([np.zeros((2, 2)), v]), np.hstack([-v.T, np.zeros((1, 1))])])) + Rv = np.eye(3) + sinth*Kcrs + (1.-costh)*Kcrs.dot(Kcrs) + + # Set up 2x2 SVD decomposition + B = np.hstack((np.eye(2), -v)).dot(Rv[:, 0:2]) + dt = B[0, 0]*B[1, 1] - B[0, 1]*B[1, 0] + Binv = np.vstack([np.hstack([B[1, 1]/dt, -B[0, 1]/dt]), np.hstack([-B[1, 0]/dt, B[0, 0]/dt])]) + A = Binv.dot(J) + + # Compute the largest singular value of A + AAT = A.dot(A.T) + gamma = np.sqrt(1./2*(AAT[0, 0] + AAT[1, 1] + np.sqrt((AAT[0, 0]-AAT[1, 1])**2 + 4*AAT[0, 1]**2))) + + # Reconstruct the full rotation matrices + R22_tild = A/gamma + + h = np.eye(2)-R22_tild.T.dot(R22_tild) + b = np.vstack((np.sqrt(h[0, 0]), np.sqrt(h[1, 1]))) + if h[0, 1] < 0: + b[1] = -b[1] + v1 = np.vstack((R22_tild[:, 0:1], np.array([b[0]]))) + v2 = np.vstack((R22_tild[:, 1:2], np.array([b[1]]))) + d = IPPE_crs(v1, v2) + c = d[0:2] + a = d[2] + R1 = Rv.dot(np.vstack((np.hstack((R22_tild, c)), np.hstack((b.conj().T, np.array([a])))))) + R2 = Rv.dot(np.vstack((np.hstack((R22_tild, -c)), np.hstack((-b.conj().T, np.array([a])))))) + + return [R1, R2, gamma] + + +def IPPE_crs(v1, v2): + """ + 3D cross product for vectors v1 and v2 + """ + v3 = np.zeros((3, 1)) + v3[0] = v1[1]*v2[2]-v1[2]*v2[1] + v3[1] = v1[2]*v2[0]-v1[0]*v2[2] + v3[2] = v1[0]*v2[1]-v1[1]*v2[0] + + return v3 + + +def homography2d(x1, x2): + """ + Direct Linear Transform + + Input: + x1: 3xN set of homogeneous points + x2: 3xN set of homogeneous points such that x1<->x2 + + Returns: + H: the 3x3 homography such that x2 = H*x1 + """ + [x1, T1] = normalise2dpts(x1) + [x2, T2] = normalise2dpts(x2) + + Npts = x1.shape[1] + + A = np.zeros((3*Npts, 9)) + + O = np.zeros(3) # noqa + + for i in range(0, Npts): + X = x1[:, i].T + x = x2[0, i] + y = x2[1, i] + w = x2[2, i] + A[3*i, :] = np.array([O, -w*X, y*X]).reshape(1, 9) + A[3*i+1, :] = np.array([w*X, O, -x*X]).reshape(1, 9) + A[3*i+2, :] = np.array([-y*X, x*X, O]).reshape(1, 9) + + [U, D, Vh] = np.linalg.svd(A) + V = Vh.T + + H = V[:, 8].reshape(3, 3) + + H = np.linalg.solve(T2, H) + H = H.dot(T1) + + return H + + +def normalise2dpts(pts): + """ + Function translates and normalises a set of 2D homogeneous points + so that their centroid is at the origin and their mean distance from + the origin is sqrt(2). This process typically improves the + conditioning of any equations used to solve homographies, fundamental + matrices etc. + + + Inputs: + pts: 3xN array of 2D homogeneous coordinates + + Returns: + newpts: 3xN array of transformed 2D homogeneous coordinates. The + scaling parameter is normalised to 1 unless the point is at + infinity. + T: The 3x3 transformation matrix, newpts = T*pts + """ + if pts.shape[0] != 3: + print('Input shoud be 3') + + finiteind = np.nonzero(abs(pts[2, :]) > np.spacing(1)) + + # if len(finiteind) != pts.shape[1]: + # print('Some points are at infinity') + + dist = [] + for i in finiteind: + pts[0, i] = pts[0, i]/pts[2, i] + pts[1, i] = pts[1, i]/pts[2, i] + pts[2, i] = 1 + + c = np.mean(pts[0:2, i].T, axis=0).T + + newp1 = pts[0, i]-c[0] + newp2 = pts[1, i]-c[1] + + dist.append(np.sqrt(newp1**2 + newp2**2)) + + meandist = np.mean(dist[:]) + + scale = np.sqrt(2)/meandist + + T = np.array([[scale, 0, -scale*c[0]], [0, scale, -scale*c[1]], [0, 0, 1]]) + + newpts = T.dot(pts) + + return [newpts, T] diff --git a/cflib/localization/ippe_cf.py b/cflib/localization/ippe_cf.py new file mode 100644 index 000000000..ccb31a8de --- /dev/null +++ b/cflib/localization/ippe_cf.py @@ -0,0 +1,121 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +from collections import namedtuple + +import numpy as np + +from ._ippe import mat_run + + +class IppeCf: + """ + A wrapper class to simplify usage of IPPE in CF code. + Converts between CF style of coordinate systems/data structures and + IPPE (Open CV) style. + """ + + # Rotation matrix to transform from IPPE to CF + _R_ippe_to_cf = np.array([ + [0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + ]) + + # Rotation matrix to transform from CF to IPPE + _R_cf_to_ippe = np.transpose(_R_ippe_to_cf) + + Solution = namedtuple('Solution', ['R', 't', 'reproj_err']) + + @staticmethod + def mat_run(U_cf, Q_cf): + """ + The solution to Perspective IPPE with point correspondences computed + between points in world coordinates on the plane z=0, and normalised points in the + camera's image. + + This is a wrapper function to convert from/to CF coordinate system/array style + + Parameters + ---------- + U_cf: Nx3 matrix holding the model points in world coordinates. + + Q_cf: Nx2 matrix holding the points in the image. These are in normalised + pixel coordinates. That is, the effects of the camera's intrinsic matrix + and lens distortion are corrected, so that the Q projects with a perfect + pinhole model. + + First param: Y (positive to the left) + Second param: Z (positive up) + + + Return + --------- + IPPEPoses: A list that contains 2 sets of pose solution from IPPE including rotation matrix + translation matrix, and reprojection error. The first solution in the list has + the smallest reprojection error. + """ + + U, Q = IppeCf._cf_to_ippe(U_cf, Q_cf) + solutions = mat_run(U, Q) + return IppeCf._ippe_to_cf(solutions) + + @staticmethod + def _cf_to_ippe(U_cf, Q_cf): + modelDims = U_cf.shape[0] + U_t = np.zeros_like(U_cf, dtype=float) + Q_t = np.zeros_like(Q_cf, dtype=float) + for i in range(modelDims): + U_t[i] = IppeCf._rotate_vector_to_ippe(U_cf[i]) + Q_t[i] = np.array((-Q_cf[i][0], -Q_cf[i][1])) + + U = np.transpose(U_t) + Q = np.transpose(Q_t) + + return U, Q + + @staticmethod + def _ippe_to_cf(solutions): + result = [ + IppeCf.Solution( + IppeCf._rotate_rot_mat_to_cf(solutions['R1']), + IppeCf._rotate_vector_to_cf(solutions['t1']), + solutions['reprojError1'] + ), + IppeCf.Solution( + IppeCf._rotate_rot_mat_to_cf(solutions['R2']), + IppeCf._rotate_vector_to_cf(solutions['t2']), + solutions['reprojError2'] + ) + ] + + return result + + @staticmethod + def _rotate_vector_to_ippe(v): + return np.dot(IppeCf._R_cf_to_ippe, v) + + def _rotate_vector_to_cf(v): + return np.dot(IppeCf._R_ippe_to_cf, v) + + @staticmethod + def _rotate_rot_mat_to_cf(R): + return np.dot(IppeCf._R_ippe_to_cf, np.dot(R, IppeCf._R_cf_to_ippe)) From 30722e26ca90f5ad24b2fdb51eff18b46c132351 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 3 Jan 2022 17:41:29 +0100 Subject: [PATCH 315/861] Added lighthouse sample matcher --- .../localization/lighthouse_sample_matcher.py | 57 +++++++++++++++ cflib/localization/lighthouse_types.py | 70 ++++++++++++++++++ .../test_lighthouse_sample_matcher.py | 73 +++++++++++++++++++ 3 files changed, 200 insertions(+) create mode 100644 cflib/localization/lighthouse_sample_matcher.py create mode 100644 cflib/localization/lighthouse_types.py create mode 100644 test/localization/test_lighthouse_sample_matcher.py diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py new file mode 100644 index 000000000..f85674da7 --- /dev/null +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -0,0 +1,57 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . + +from cflib.localization.lighthouse_types import LhCfPoseSample, LhMeasurement + + +class LighthouseSampleMatcher: + """Utility class to match samples of measurements from multiple lighthouse base stations. + + Assuming that the Crazyflie was moving when the measurements were recorded, + samples that were meassured aproximately at the same position are aggregated into + a list of LhCfPoseSample. Matching is done using the timestamp and a maximum time span. + """ + + @classmethod + def match(cls, samples: list[LhMeasurement], max_time_diff: float =0.010, min_nr_of_bs_in_match: int =0) -> list[LhCfPoseSample]: + result = [] + current: LhCfPoseSample = None + + for sample in samples: + ts = sample.timestamp + + if current is None: + current = LhCfPoseSample(timestamp=ts) + + if ts > (current.timestamp + max_time_diff): + cls._append_result(current, result, min_nr_of_bs_in_match) + current = LhCfPoseSample(timestamp=ts) + + current.angles_calibrated[sample.base_station_id] = sample.angles + + cls._append_result(current, result, min_nr_of_bs_in_match) + return result + + @classmethod + def _append_result(cls, current: LhCfPoseSample, result: list[LhCfPoseSample], min_nr_of_bs_in_match: int): + if len(current.angles_calibrated) > min_nr_of_bs_in_match: + result.append(current) diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py new file mode 100644 index 000000000..46571f9e2 --- /dev/null +++ b/cflib/localization/lighthouse_types.py @@ -0,0 +1,70 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . + +from typing import NamedTuple +import numpy as np +import numpy.typing as npt + +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector + +class Pose: + """ Holds the full pose (position and orientation) of an object. + Contains functionality to convert between various formats.""" + + def __init__(self, R_matrix: npt.ArrayLike, t_vec: npt.ArrayLike) -> None: + # Rotation as a matix + self.R_matrix = np.array(R_matrix) + + # Rotation as a Rodrigues vector + self.R_vec = None + + self.t_vec = np.array(t_vec) + + def matrix_vec(): + pass + + def rotvec_vec(): + pass + + def rotvec_vec_list(): + pass + + +class LhMeasurement(NamedTuple): + """Represents a measurement from one base station.""" + timestamp: float + base_station_id: int + angles: LighthouseBsVector + + +class LhCfPoseSample: + """ Represents a sample of a Crazyflie pose in space, it contains + various data related to the pose such as: + - lighthouse angles from one or more base stations + - initial estimate of the pose + - refined estimate of the pose + - estimated errors + """ + + def __init__(self, timestamp: float =0.0) -> None: + self.timestamp: float = timestamp + self.angles_calibrated = {} diff --git a/test/localization/test_lighthouse_sample_matcher.py b/test/localization/test_lighthouse_sample_matcher.py new file mode 100644 index 000000000..18d063a5b --- /dev/null +++ b/test/localization/test_lighthouse_sample_matcher.py @@ -0,0 +1,73 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 unittest +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector + +from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher +from cflib.localization.lighthouse_types import LhMeasurement + +class TestLighthouseSampleMatcher(unittest.TestCase): + def setUp(self): + self.vec0 = LighthouseBsVector(0.0, 0.0) + self.vec1 = LighthouseBsVector(0.1, 0.1) + self.vec2 = LighthouseBsVector(0.2, 0.2) + self.vec3 = LighthouseBsVector(0.3, 0.3) + + self.samples = [ + LhMeasurement(timestamp=1.000, base_station_id=0, angles=self.vec0), + LhMeasurement(timestamp=1.015, base_station_id=1, angles=self.vec1), + LhMeasurement(timestamp=1.020, base_station_id=0, angles=self.vec2), + LhMeasurement(timestamp=1.035, base_station_id=1, angles=self.vec3), + ] + + + def test_that_samples_are_aggregated(self): + # Fixture + + # Test + actual = LighthouseSampleMatcher.match(self.samples, max_time_diff=0.010) + + # Assert + self.assertEqual(1.000, actual[0].timestamp) + self.assertEqual(1, len(actual[0].angles_calibrated)) + self.assertEqual(self.vec0, actual[0].angles_calibrated[0]) + + self.assertEqual(1.015, actual[1].timestamp) + self.assertEqual(2, len(actual[1].angles_calibrated)) + self.assertEqual(self.vec1, actual[1].angles_calibrated[1]) + self.assertEqual(self.vec2, actual[1].angles_calibrated[0]) + + self.assertEqual(1.035, actual[2].timestamp) + self.assertEqual(1, len(actual[2].angles_calibrated)) + self.assertEqual(self.vec3, actual[2].angles_calibrated[1]) + + def test_that_single_bs_samples_are_fitered(self): + # Fixture + + # Test + actual = LighthouseSampleMatcher.match(self.samples, max_time_diff=0.010, min_nr_of_bs_in_match=1) + + # Assert + self.assertEqual(1.015, actual[0].timestamp) + self.assertEqual(2, len(actual[0].angles_calibrated)) + self.assertEqual(self.vec1, actual[0].angles_calibrated[1]) + self.assertEqual(self.vec2, actual[0].angles_calibrated[0]) From b49e884aa57bff8d27de628b82828045308e5ede Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 5 Jan 2022 13:21:46 +0100 Subject: [PATCH 316/861] Added initial pose estimation --- cflib/localization/ippe_cf.py | 3 +- cflib/localization/lighthouse_bs_vector.py | 59 +++++-- .../lighthouse_initial_estimator.py | 164 ++++++++++++++++++ .../localization/lighthouse_sample_matcher.py | 8 +- cflib/localization/lighthouse_types.py | 101 +++++++++-- test/localization/lighthouse_fixtures.py | 76 ++++++++ test/localization/lighthouse_test_base.py | 51 ++++++ test/localization/test_ippe_cf.py | 58 +++++++ .../localization/test_lighthouse_bs_vector.py | 31 ++++ .../test_lighthouse_initial_estimator.py | 147 ++++++++++++++++ .../test_lighthouse_sample_matcher.py | 4 +- test/localization/test_lighthouse_types.py | 75 ++++++++ 12 files changed, 745 insertions(+), 32 deletions(-) create mode 100644 cflib/localization/lighthouse_initial_estimator.py create mode 100644 test/localization/lighthouse_fixtures.py create mode 100644 test/localization/lighthouse_test_base.py create mode 100644 test/localization/test_ippe_cf.py create mode 100644 test/localization/test_lighthouse_initial_estimator.py create mode 100644 test/localization/test_lighthouse_types.py diff --git a/cflib/localization/ippe_cf.py b/cflib/localization/ippe_cf.py index ccb31a8de..60221a2a4 100644 --- a/cflib/localization/ippe_cf.py +++ b/cflib/localization/ippe_cf.py @@ -22,6 +22,7 @@ from collections import namedtuple import numpy as np +import numpy.typing as npt from ._ippe import mat_run @@ -46,7 +47,7 @@ class IppeCf: Solution = namedtuple('Solution', ['R', 't', 'reproj_err']) @staticmethod - def mat_run(U_cf, Q_cf): + def solve(U_cf: npt.ArrayLike, Q_cf: npt.ArrayLike) -> list[Solution]: """ The solution to Perspective IPPE with point correspondences computed between points in world coordinates on the plane z=0, and normalised points in the diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py index 3126621ee..c5a37df0e 100644 --- a/cflib/localization/lighthouse_bs_vector.py +++ b/cflib/localization/lighthouse_bs_vector.py @@ -22,20 +22,21 @@ import math import numpy as np +import numpy.typing as npt class LighthouseBsVector: """ This class is representing a vector from a base station into space, in the base station reference frame. Typically the intersection of - two light planes. + two light planes defined by angles measured by a base station. It also provides functionality to convert between lighthouse V1 angles, V2 angles and cartesian coordinates. """ T = math.pi / 6 - def __init__(self, lh_v1_horiz_angle, lh_v1_vert_angle): + def __init__(self, lh_v1_horiz_angle: float, lh_v1_vert_angle: float) -> None: """ Initialize from lighthouse V1 angles :param lh_v1_horiz_angle: Horizontal sweep angle, 0 straight forward. Right (seen from the bs) is negative, @@ -46,7 +47,7 @@ def __init__(self, lh_v1_horiz_angle, lh_v1_vert_angle): self._lh_v1_vert_angle = lh_v1_vert_angle @classmethod - def from_lh2(cls, lh_v2_angle_1, lh_v2_angle_2): + def from_lh2(cls, lh_v2_angle_1: float, lh_v2_angle_2: float) -> 'LighthouseBsVector': """ Create a LighthouseBsVector object from lighthouse V2 angles :param lh_v2_angle_1: First sweep angles, 0 straight ahead @@ -60,7 +61,7 @@ def from_lh2(cls, lh_v2_angle_1, lh_v2_angle_2): return cls(lh_v1_horiz_angle, lh_v1_vert_angle) @classmethod - def from_cart(cls, cart_vector): + def from_cart(cls, cart_vector: list[float]) -> 'LighthouseBsVector': """ Create a LighthouseBsVector object from cartesian coordinates. :param cart_vector: (x, y, z) to a point @@ -70,41 +71,79 @@ def from_cart(cls, cart_vector): return cls(lh_v1_horiz_angle, lh_v1_vert_angle) + @classmethod + def from_projection(cls, proj_point: list[float]) -> 'LighthouseBsVector': + """ + Create a LighthouseBsVector object from the projection point on the plane x=1.0 + :param projection point: (y, z) + """ + lh_v1_horiz_angle = math.atan(proj_point[0]) + lh_v1_vert_angle = math.atan(proj_point[1]) + + return cls(lh_v1_horiz_angle, lh_v1_vert_angle) + @property - def lh_v1_horiz_angle(self): + def lh_v1_horiz_angle(self) -> float: """ Lightouse V1 horizontal sweep angle """ return self._lh_v1_horiz_angle @property - def lh_v1_vert_angle(self): + def lh_v1_vert_angle(self) -> float: """ Lightouse V1 vertical sweep angle """ return self._lh_v1_vert_angle @property - def lh_v2_angle_1(self): + def lh_v1_angle_pair(self) -> tuple[float, float]: + """ + Lightouse V1 angle pair (horiz, vert) + """ + return self._lh_v1_horiz_angle, self._lh_v1_vert_angle, + + @property + def lh_v2_angle_1(self) -> float: """ Lightouse V2 first sweep angle """ return self._lh_v1_horiz_angle + math.asin(self._q() * math.tan(-self.T)) @property - def lh_v2_angle_2(self): + def lh_v2_angle_2(self) -> float: """ Lightouse V2 second sweep angle """ return self._lh_v1_horiz_angle + math.asin(self._q() * math.tan(self.T)) @property - def cart(self): + def cart(self) -> npt.NDArray[np.float32]: """ A normalized vector in cartesian coordinates """ - v = np.array((1, math.tan(self._lh_v1_horiz_angle), math.tan(self._lh_v1_vert_angle))) + v = np.float32((1, math.tan(self._lh_v1_horiz_angle), math.tan(self._lh_v1_vert_angle))) return v / np.linalg.norm(v) + @property + def projection(self) -> npt.NDArray[np.float32]: + """ + The 2D point (y, z) when projected on the plane x=1.0 (one meter in front of the base station) + """ + return np.float32((math.tan(self._lh_v1_horiz_angle), math.tan(self._lh_v1_vert_angle))) + def _q(self): return math.tan(self._lh_v1_vert_angle) / math.sqrt(1 + math.tan(self._lh_v1_horiz_angle) ** 2) + + +# A LighthouseBsVectors is a list of 4 LighthouseBsVector, one for +# each sensor +# LighthouseBsVectors = list[LighthouseBsVector] + +class LighthouseBsVectors(list[LighthouseBsVector]): + def projection_pair_list(self) -> npt.NDArray: + result = np.empty((len(self), 2), dtype=float) + for i, vector in enumerate(self): + result[i] = vector.projection + + return result diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py new file mode 100644 index 000000000..96d72cf06 --- /dev/null +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -0,0 +1,164 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 numpy as np +import numpy.typing as npt + +from .ippe_cf import IppeCf +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import Pose + + +class LighthouseInitialEstimator: + """ + Make initial estimates of base station and CF poses useing IPPE (analytical solution). + The estimates are not good enough to use for flight but is a starting point for further + Calculations. + """ + + @classmethod + def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> dict[int, Pose]: + """ + Make a rough estimate of the poses of all base stations found in the samples. + + The pose of the Crazyflie in the first sample is used as a reference and will define the + global reference frame. + + :param matched_samples: A list of samples with lihghthouse angles. Note: matched_samples is augmented with + more information during the process, including the estimated poses + :param sensor_positions: An array with the sensor positions on the lighthouse deck (3D, CF ref frame) + """ + + cls._angles_to_poses(matched_samples, sensor_positions) + + # TODO Do not use first sample as reference, pass it in as a parameter + + bs_poses: dict[int, Pose] = {} + cls._get_reference_bs_poses(matched_samples[0], bs_poses) + cls._calc_remaining_bs_poses(matched_samples, bs_poses) + cls._calc_cf_poses(matched_samples, bs_poses) + + return bs_poses + + @classmethod + def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> None: + for sample in matched_samples: + poses: dict[int, Pose] = {} + for bs, angles in sample.angles_calibrated.items(): + Q = angles.projection_pair_list() + estimate = IppeCf.solve(sensor_positions, Q) + + # Pick the first solution from IPPE and convert to cf ref frame + R_mat = estimate[0][0].transpose() + t_vec = np.dot(R_mat, -estimate[0][1]) + + poses[bs] = Pose(R_mat, t_vec) + sample.initial_est_bs_poses = poses + + @classmethod + def _get_reference_bs_poses(cls, sample: LhCfPoseSample, bs_poses: dict[int, Pose]) -> None: + """ + The Pose of the CF in this sample is defining the global ref frame. + Store the poses for the bases stations that are in the sample. + """ + est_ref_cf = sample.initial_est_bs_poses + for bs, pose in est_ref_cf.items(): + bs_poses[bs] = pose + + @classmethod + def _calc_remaining_bs_poses(cls, matched_sampels: list[LhCfPoseSample], bs_poses: dict[int, Pose]) -> None: + # Find all base stations in the list + all_bs = set() + for sample in matched_sampels: + all_bs.update(sample.initial_est_bs_poses.keys()) + + # Remove the reference base stations that we already have the poses for + to_find = all_bs - bs_poses.keys() + + # run through the list of samples until we manage to find them all + remaining = len(to_find) + while remaining > 0: + for sample in matched_sampels: + bs_poses_in_sample = sample.initial_est_bs_poses + unknown = to_find.intersection(bs_poses_in_sample.keys()) + known = set(bs_poses.keys()).intersection(bs_poses_in_sample.keys()) + + # We need (at least) one known bs pose to use when transforming the other poses to the global ref frame + if len(known) > 0: + known_bs = list(known)[0] + + # The known BS pose in the global reference frame + known_global = bs_poses[known_bs] + # The known BS pose in the CF reference frame (of this sample) + known_cf = bs_poses_in_sample[known_bs] + + for bs in unknown: + # The unknown BS pose in the CF reference frame (of this sample) + unknown_cf = bs_poses_in_sample[bs] + # Finally we can calculate the BS pose in the global reference frame + bs_poses[bs] = cls._map_pose_to_ref_frame(known_global, known_cf, unknown_cf) + + to_find = all_bs - bs_poses.keys() + if len(to_find) == 0: + break + + if len(to_find) == remaining: + raise Exception('Can not link positions between all base stations') + + remaining = len(to_find) + + @classmethod + def _calc_cf_poses(cls, matched_samples: list[LhCfPoseSample], bs_poses: list[Pose]) -> None: + for sample in matched_samples: + # Use the first base station pose as a reference + est_ref_cf = sample.initial_est_bs_poses + ref_bs = list(est_ref_cf.keys())[0] + + pose_global = bs_poses[ref_bs] + pose_cf = est_ref_cf[ref_bs] + est_ref_global = cls._map_cf_pos_to_cf_pos(pose_global, pose_cf) + sample.inital_est_pose = est_ref_global + + @classmethod + def _map_pose_to_ref_frame(cls, pose1_ref1: Pose, pose1_ref2: Pose, pose2_ref2: Pose) -> Pose: + """ + Express pose2 in reference system 1, given pose1 in both reference system 1 and 2 + """ + R_o2_in_1, t_o2_in_1 = cls._map_cf_pos_to_cf_pos(pose1_ref1, pose1_ref2).matrix_vec + + t = np.dot(R_o2_in_1, pose2_ref2.translation) + t_o2_in_1 + R = np.dot(R_o2_in_1, pose2_ref2.rot_matrix) + + return Pose(R, t) + + @classmethod + def _map_cf_pos_to_cf_pos(cls, pose1_ref1: Pose, pose1_ref2: Pose) -> Pose: + """ + Find the rotation/translation from ref1 to ref2 given a pose, + that is the returned Pose will tell us where the origin in ref2 is, + expressed in ref1 + """ + + R_inv_ref2 = np.matrix.transpose(pose1_ref2.rot_matrix) + R = np.dot(pose1_ref1.rot_matrix, R_inv_ref2) + t = pose1_ref1.translation - np.dot(R, pose1_ref2.translation) + + return Pose(R, t) diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py index f85674da7..052b3ca97 100644 --- a/cflib/localization/lighthouse_sample_matcher.py +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -19,8 +19,8 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . - -from cflib.localization.lighthouse_types import LhCfPoseSample, LhMeasurement +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import LhMeasurement class LighthouseSampleMatcher: @@ -32,7 +32,9 @@ class LighthouseSampleMatcher: """ @classmethod - def match(cls, samples: list[LhMeasurement], max_time_diff: float =0.010, min_nr_of_bs_in_match: int =0) -> list[LhCfPoseSample]: + def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.010, + min_nr_of_bs_in_match: int = 0) -> list[LhCfPoseSample]: + result = [] current: LhCfPoseSample = None diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index 46571f9e2..55fb6b70d 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -19,41 +19,85 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . - from typing import NamedTuple + import numpy as np import numpy.typing as npt +from scipy.spatial.transform import Rotation + +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors -from cflib.localization.lighthouse_bs_vector import LighthouseBsVector class Pose: """ Holds the full pose (position and orientation) of an object. Contains functionality to convert between various formats.""" - def __init__(self, R_matrix: npt.ArrayLike, t_vec: npt.ArrayLike) -> None: + _NO_ROTATION_MTX = np.identity(3) + _NO_ROTATION_VCT = np.array((0.0, 0.0, 0.0)) + _ORIGIN = np.array((0.0, 0.0, 0.0)) + + def __init__(self, R_matrix: npt.ArrayLike = _NO_ROTATION_MTX, t_vec: npt.ArrayLike = _ORIGIN) -> None: # Rotation as a matix - self.R_matrix = np.array(R_matrix) + self._R_matrix = np.array(R_matrix) + + # Translation vector + self._t_vec = np.array(t_vec) + + @classmethod + def from_rot_vec(cls, R_vec: npt.ArrayLike = _NO_ROTATION_VCT, t_vec: npt.ArrayLike = _ORIGIN) -> 'Pose': + """ + Create a Pose from a rotation vector and translation vector + """ + return Pose(Rotation.from_rotvec(R_vec).as_matrix(), t_vec) + + @property + def rot_matrix(self) -> npt.NDArray: + """ + Get the rotation matrix of the pose + """ + return self._R_matrix - # Rotation as a Rodrigues vector - self.R_vec = None + @property + def rot_vec(self) -> npt.NDArray: + """ + Get the rotation vector of the pose + """ + return Rotation.from_matrix(self._R_matrix).as_rotvec() - self.t_vec = np.array(t_vec) + @property + def translation(self) -> npt.NDArray: + """ + Get the translation vector of the pose + """ + return self._t_vec - def matrix_vec(): - pass + @property + def matrix_vec(self) -> tuple[npt.NDArray, npt.NDArray]: + """ + Get the pose as a rotation matrix and translation vector + """ + return self._R_matrix, self._t_vec - def rotvec_vec(): - pass + def rotate_translate(self, point: npt.ArrayLike) -> npt.NDArray: + """ + Rotate and translate a point, that is transform from local + to global reference frame + """ + return np.dot(self.rot_matrix, point) + self.translation - def rotvec_vec_list(): - pass + def inv_rotate_translate(self, point: npt.ArrayLike) -> npt.NDArray: + """ + Inverse rotate and translate a point, that is transform from global + to local reference frame + """ + return np.dot(np.transpose(self.rot_matrix), point - self.translation) class LhMeasurement(NamedTuple): """Represents a measurement from one base station.""" timestamp: float base_station_id: int - angles: LighthouseBsVector + angles: LighthouseBsVectors class LhCfPoseSample: @@ -65,6 +109,31 @@ class LhCfPoseSample: - estimated errors """ - def __init__(self, timestamp: float =0.0) -> None: + def __init__(self, timestamp: float = 0.0, angles_calibrated: dict[int, LighthouseBsVectors] = None) -> None: self.timestamp: float = timestamp - self.angles_calibrated = {} + + # Angles measured by the Crazyflie and compensated using calibration data + # Stored in a dictionary using base station id as the key + self.angles_calibrated: dict[int, LighthouseBsVectors] = angles_calibrated + if self.angles_calibrated is None: + self.angles_calibrated = {} + + # Initial estimates of bs poses for this sample, in the CF reference frame + self.initial_est_bs_poses: dict[int, Pose] = {} + + # The initial estimate of the CF pose for this sample, in the global ref frame + self.inital_est_pose: Pose = None + + +class LhDeck4SensorPositions: + """ Positions of the sensors on the Lighthouse 4 deck """ + # Sensor distances on the lighthouse deck + _sensor_distance_width = 0.015 + _sensor_distance_length = 0.03 + + # Sensor positions in the Crazyflie reference frame + positions = np.float32([ + (-_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0), + (-_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0), + (_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0), + (_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0)]) diff --git a/test/localization/lighthouse_fixtures.py b/test/localization/lighthouse_fixtures.py new file mode 100644 index 000000000..6e8d5dc4d --- /dev/null +++ b/test/localization/lighthouse_fixtures.py @@ -0,0 +1,76 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 numpy as np + +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors +from cflib.localization.lighthouse_types import LhDeck4SensorPositions +from cflib.localization.lighthouse_types import Pose + + +class LighthouseFixtures: + """ + Fixtures to be used in lighthouse unit tests + """ + + # Stock objects + # BS0 is pointing along the X-axis + BS0_POSE = Pose(t_vec=(-2.0, 1.0, 3.0)) + # BS1 is pointing along the Y-axis + BS1_POSE = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2), t_vec=(0.0, -2.0, 3.0)) + # BS2 is pointing along the negative Y-axis + BS2_POSE = Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(0.0, 2.0, 3.0)) + # BS3 is pointing along the negative X-axis + BS3_POSE = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(2.0, 0.0, 2.0)) + + # CF_ORIGIN is in the origin, pointing along the X-axis + CF_ORIGIN_POSE = Pose() + + # CF1 is pointing along the X-axis + CF1_POSE = Pose(t_vec=(0.3, 0.2, 0.1)) + + # CF2 is pointing along the Y-axis + CF2_POSE = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2), t_vec=(1.0, 0.0, 0.0)) + + def __init__(self) -> None: + self.angles_cf_origin_bs0 = self.synthesize_angles(self.CF_ORIGIN_POSE, self.BS0_POSE) + self.angles_cf_origin_bs1 = self.synthesize_angles(self.CF_ORIGIN_POSE, self.BS1_POSE) + + self.angles_cf1_bs1 = self.synthesize_angles(self.CF1_POSE, self.BS1_POSE) + self.angles_cf1_bs2 = self.synthesize_angles(self.CF1_POSE, self.BS2_POSE) + + self.angles_cf2_bs0 = self.synthesize_angles(self.CF2_POSE, self.BS0_POSE) + self.angles_cf2_bs1 = self.synthesize_angles(self.CF2_POSE, self.BS1_POSE) + self.angles_cf2_bs2 = self.synthesize_angles(self.CF2_POSE, self.BS2_POSE) + self.angles_cf2_bs3 = self.synthesize_angles(self.CF2_POSE, self.BS3_POSE) + + def synthesize_angles(self, pose_cf: Pose, pose_bs: Pose) -> LighthouseBsVectors: + """ + Genereate a LighthouseBsVectors object based + """ + + result = LighthouseBsVectors() + for sens_pos_ref_cf in LhDeck4SensorPositions.positions: + sens_pos_ref_global = pose_cf.rotate_translate(sens_pos_ref_cf) + sens_pos_ref_bs = pose_bs.inv_rotate_translate(sens_pos_ref_global) + result.append(LighthouseBsVector.from_cart(sens_pos_ref_bs)) + return result diff --git a/test/localization/lighthouse_test_base.py b/test/localization/lighthouse_test_base.py new file mode 100644 index 000000000..049f5d7ec --- /dev/null +++ b/test/localization/lighthouse_test_base.py @@ -0,0 +1,51 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 unittest + +import numpy as np +import numpy.typing as npt + +from cflib.localization.lighthouse_types import Pose + + +class LighthouseTestBase(unittest.TestCase): + """ + Utilitis to simplify testing of lighthouse code + """ + + def assertVectorsAlmostEqual(self, expected: npt.ArrayLike, actual: npt.ArrayLike, places: int = 5) -> None: + _expected = np.array(expected) + _actual = np.array(actual) + + self.assertEqual(_expected.shape[0], _actual.shape[0], 'Shape differs') + + for i in range(_expected.shape[0]): + self.assertAlmostEqual(_expected[i], _actual[i], places, f'Lists differs in element {i}') + + def assertPosesAlmostEqual(self, expected: Pose, actual: Pose, places: int = 5): + translation_diff = expected.translation - actual.translation + self.assertAlmostEqual(0.0, np.linalg.norm(translation_diff), places, + f'Translation different, expected: {expected.translation}, actual: {actual.translation}') + + rotation_diff = expected.rot_vec - actual.rot_vec + self.assertAlmostEqual(0.0, np.linalg.norm(rotation_diff), places, + f'Rotation different, expected: {expected.rot_vec}, actual: {actual.rot_vec}') diff --git a/test/localization/test_ippe_cf.py b/test/localization/test_ippe_cf.py new file mode 100644 index 000000000..fd0e64a84 --- /dev/null +++ b/test/localization/test_ippe_cf.py @@ -0,0 +1,58 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +from test.localization.lighthouse_fixtures import LighthouseFixtures +from test.localization.lighthouse_test_base import LighthouseTestBase + +import numpy as np + +from cflib.localization.ippe_cf import IppeCf +from cflib.localization.lighthouse_types import LhDeck4SensorPositions +from cflib.localization.lighthouse_types import Pose + + +class TestIppeCf(LighthouseTestBase): + def setUp(self): + self.fixtures = LighthouseFixtures() + + def test_that_pose_is_found(self): + # Fixture + pose_bs = Pose(t_vec=(0.0, 0.0, 0.0)) + pose_cf = Pose(t_vec=(1.0, 0.0, -1.0)) + + U = LhDeck4SensorPositions.positions + Q = self.fixtures.synthesize_angles(pose_cf, pose_bs).projection_pair_list() + + # The CF pose seen from the base station + expected_0 = pose_cf + + # Not sure if (why) this is the expected mirror solution + expected_1 = Pose.from_rot_vec(R_vec=(0.0, -np.pi / 2, 0.0), t_vec=pose_cf.translation) + + # Test + actual = IppeCf.solve(U, Q) + + # Assert + actual_pose_0 = Pose(actual[0].R, actual[0].t) + self.assertPosesAlmostEqual(expected_0, actual_pose_0, places=3) + + actual_pose_1 = Pose(actual[1].R, actual[1].t) + self.assertPosesAlmostEqual(expected_1, actual_pose_1, places=3) diff --git a/test/localization/test_lighthouse_bs_vector.py b/test/localization/test_lighthouse_bs_vector.py index 27eb2080a..43a9b4e77 100644 --- a/test/localization/test_lighthouse_bs_vector.py +++ b/test/localization/test_lighthouse_bs_vector.py @@ -24,6 +24,7 @@ import numpy as np from cflib.localization import LighthouseBsVector +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors class TestLighthouseBsVector(unittest.TestCase): @@ -116,3 +117,33 @@ def test_cartesian_is_normalized(self): # Assert self.assertAlmostEqual(1.0, actual) + + def test_conversion_to_from_projection(self): + # Fixture + horiz = 0.123 + vert = 0.456 + v1 = LighthouseBsVector(horiz, vert) + + # Test + actual = LighthouseBsVector.from_projection(v1.projection) + + # Assert + self.assertAlmostEqual(horiz, actual.lh_v1_horiz_angle) + self.assertAlmostEqual(vert, actual.lh_v1_vert_angle) + + def test_conversion_to_projection_pair_list(self): + # Fixture + vectors = LighthouseBsVectors(( + LighthouseBsVector(0.0, 0.0), + LighthouseBsVector(0.1, 0.1), + LighthouseBsVector(0.2, 0.2), + LighthouseBsVector(0.3, 0.3), + )) + + # Test + actual = vectors.projection_pair_list() + + # Assert + self.assertEqual(len(vectors), len(actual)) + self.assertListEqual(vectors[0].projection.tolist(), actual[0].tolist()) + self.assertListEqual(vectors[3].projection.tolist(), actual[3].tolist()) diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py new file mode 100644 index 000000000..085f26b0a --- /dev/null +++ b/test/localization/test_lighthouse_initial_estimator.py @@ -0,0 +1,147 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +from test.localization.lighthouse_fixtures import LighthouseFixtures +from test.localization.lighthouse_test_base import LighthouseTestBase + +import numpy as np + +from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import LhDeck4SensorPositions +from cflib.localization.lighthouse_types import Pose + + +class TestLighthouseInitialEstimator(LighthouseTestBase): + def setUp(self): + self.fixtures = LighthouseFixtures() + + def test_that_one_bs_pose_is_found(self): + # Fixture + # CF_ORIGIN is used in the first sample and will define the global reference frame + bs_id = 3 + samples = [ + LhCfPoseSample(angles_calibrated={bs_id: self.fixtures.angles_cf_origin_bs0}), + ] + + # Test + actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + + # Assert + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual[bs_id], places=3) + + def test_that_two_bs_poses_in_same_sample_are_found(self): + # Fixture + # CF_ORIGIN is used in the first sample and will define the global reference frame + bs_id0 = 3 + bs_id1 = 1 + samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf_origin_bs0, + bs_id1: self.fixtures.angles_cf_origin_bs1, + }), + ] + + # Test + actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + + # Assert + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual[bs_id0], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual[bs_id1], places=3) + + def test_that_linked_bs_poses_in_multiple_samples_are_found(self): + # Fixture + # CF_ORIGIN is used in the first sample and will define the global reference frame + bs_id0 = 3 + bs_id1 = 1 + bs_id2 = 2 + bs_id3 = 0 + samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf_origin_bs0, + bs_id1: self.fixtures.angles_cf_origin_bs1, + }), + LhCfPoseSample(angles_calibrated={ + bs_id1: self.fixtures.angles_cf1_bs1, + bs_id2: self.fixtures.angles_cf1_bs2, + }), + LhCfPoseSample(angles_calibrated={ + bs_id2: self.fixtures.angles_cf2_bs2, + bs_id3: self.fixtures.angles_cf2_bs3, + }), + ] + + # Test + actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + + # Assert + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual[bs_id0], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual[bs_id1], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, actual[bs_id2], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS3_POSE, actual[bs_id3], places=3) + + def test_that_the_global_ref_frame_is_used(self): + # Fixture + # CF2 is used in the first sample and will define the global reference frame + bs_id0 = 3 + bs_id1 = 1 + bs_id2 = 2 + samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf2_bs0, + bs_id1: self.fixtures.angles_cf2_bs1, + }), + LhCfPoseSample(angles_calibrated={ + bs_id1: self.fixtures.angles_cf1_bs1, + bs_id2: self.fixtures.angles_cf1_bs2, + }), + ] + + # Test + actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + + # Assert + self.assertPosesAlmostEqual( + Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(1.0, 3.0, 3.0)), actual[bs_id0], places=3) + self.assertPosesAlmostEqual( + Pose.from_rot_vec(R_vec=(0.0, 0.0, 0.0), t_vec=(-2.0, 1.0, 3.0)), actual[bs_id1], places=3) + self.assertPosesAlmostEqual( + Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(2.0, 1.0, 3.0)), actual[bs_id2], places=3) + + def test_that_raises_for_isolated_bs(self): + # Fixture + bs_id0 = 3 + bs_id1 = 1 + bs_id2 = 2 + samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf_origin_bs0, + bs_id1: self.fixtures.angles_cf_origin_bs1, + }), + LhCfPoseSample(angles_calibrated={ + bs_id2: self.fixtures.angles_cf1_bs2, + }), + ] + + # Test + # Assert + with self.assertRaises(Exception): + LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) diff --git a/test/localization/test_lighthouse_sample_matcher.py b/test/localization/test_lighthouse_sample_matcher.py index 18d063a5b..557c7e970 100644 --- a/test/localization/test_lighthouse_sample_matcher.py +++ b/test/localization/test_lighthouse_sample_matcher.py @@ -20,11 +20,12 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import unittest -from cflib.localization.lighthouse_bs_vector import LighthouseBsVector +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher from cflib.localization.lighthouse_types import LhMeasurement + class TestLighthouseSampleMatcher(unittest.TestCase): def setUp(self): self.vec0 = LighthouseBsVector(0.0, 0.0) @@ -39,7 +40,6 @@ def setUp(self): LhMeasurement(timestamp=1.035, base_station_id=1, angles=self.vec3), ] - def test_that_samples_are_aggregated(self): # Fixture diff --git a/test/localization/test_lighthouse_types.py b/test/localization/test_lighthouse_types.py new file mode 100644 index 000000000..36d64ed4d --- /dev/null +++ b/test/localization/test_lighthouse_types.py @@ -0,0 +1,75 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 unittest + +import numpy as np + +from cflib.localization.lighthouse_types import Pose + + +class TestLighthouseTypes(unittest.TestCase): + def setUp(self): + pass + + def test_default_matrix_constructor(self): + # Fixture + # Test + actual = Pose() + + # Assert + self.assertEqual(0.0, np.linalg.norm(actual.translation)) + self.assertEqual(0.0, np.linalg.norm(actual.rot_matrix - np.identity(3))) + + def test_default_rot_vec_constructor(self): + # Fixture + # Test + actual = Pose.from_rot_vec() + + # Assert + self.assertEqual(0.0, np.linalg.norm(actual.translation)) + self.assertEqual(0.0, np.linalg.norm(actual.rot_matrix - np.identity(3))) + + def test_rotate_translate(self): + # Fixture + pose = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2), t_vec=(1.0, 0.0, 0.0)) + point = (2.0, 0.0, 0.0) + + # Test + actual = pose.rotate_translate(point) + + # Assert + self.assertAlmostEqual(1.0, actual[0]) + self.assertAlmostEqual(2.0, actual[1]) + self.assertAlmostEqual(0.0, actual[2]) + + def test_rotate_translate_and_back(self): + # Fixture + pose = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0), t_vec=(0.1, 0.2, 0.3)) + expected = (2.0, 3.0, 4.0) + + # Test + actual = pose.inv_rotate_translate(pose.rotate_translate(expected)) + + # Assert + self.assertAlmostEqual(expected[0], actual[0]) + self.assertAlmostEqual(expected[1], actual[1]) + self.assertAlmostEqual(expected[2], actual[2]) From 8fc9878cfc41e2fedd5945d49ae57604baba5f10 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 5 Jan 2022 17:12:19 +0100 Subject: [PATCH 317/861] Added geometry solver --- cflib/localization/lighthouse_bs_vector.py | 14 + .../lighthouse_geometry_solver.py | 460 ++++++++++++++++++ cflib/localization/lighthouse_types.py | 3 + test/localization/lighthouse_test_base.py | 10 +- .../localization/test_lighthouse_bs_vector.py | 19 +- .../test_lighthouse_geometry_solver.py | 111 +++++ 6 files changed, 614 insertions(+), 3 deletions(-) create mode 100644 cflib/localization/lighthouse_geometry_solver.py create mode 100644 test/localization/test_lighthouse_geometry_solver.py diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py index c5a37df0e..ee14d9de5 100644 --- a/cflib/localization/lighthouse_bs_vector.py +++ b/cflib/localization/lighthouse_bs_vector.py @@ -142,8 +142,22 @@ def _q(self): class LighthouseBsVectors(list[LighthouseBsVector]): def projection_pair_list(self) -> npt.NDArray: + """ + Genereate a list of projection pairs for all vectors + """ result = np.empty((len(self), 2), dtype=float) for i, vector in enumerate(self): result[i] = vector.projection return result + + def angle_list(self) -> npt.NDArray: + """ + Genereate a list of angles for all vectors, the order is horizontal, vertical, horizontal, vertical... + """ + result = np.empty((len(self) * 2), dtype=float) + for i, vector in enumerate(self): + result[i * 2] = vector.lh_v1_horiz_angle + result[i * 2 + 1] = vector.lh_v1_vert_angle + + return result diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py new file mode 100644 index 000000000..68ec87a8f --- /dev/null +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -0,0 +1,460 @@ +from collections import namedtuple + +import numpy as np +import numpy.typing as npt +import scipy.optimize + +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import Pose + +Ray = namedtuple('Ray', ['position', 'vector']) + + +class LighthouseGeometrySolution: + """ + Represents a solution from the geometry solver. + + Some data in the object is also used during the solving process for context. + """ + + def __init__(self) -> None: + # Nr of parameters in a rotation vector + self.len_rot_vec = 3 + # Nr of parameters + self.len_pose = 6 + + # Nr of base stations + self.n_bss: int = None + # Nr of parametrs per base station + self.n_params_per_bs = self.len_pose + + # Nr of sampled Crazyflie poses + self.n_cfs: int = None + # Nr of sampled Crazyflie poses used in the parameter set + self.n_cfs_in_params: int = None + # Nr of parameters per Crazyflie pose + self.n_params_per_cf = self.len_pose + + # Nr of sensors + self.n_sensors: int = None + + # The maximum nr of iterations to execute when solving the system + self.max_nr_iter = 10 + + self.bs_id_to_index: dict[int, int] = {} + self.bs_index_to_id: dict[int, int] = {} + + # The solution ###################### + + # The estimated poses of the base stations + self.bs_poses: dict[int, Pose] = {} + + # The raw result from the scipy.optimize.least_squares() function + self.lsq_result = None + + @property + def max_iter_reached(self): + """ + True if the sover was terminated due to reaching the maximum nr of alowed iterations + """ + result = False + if self.lsq_result is not None: + result = self.lsq_result == 0 + return result + + +class LighthouseGeometrySolver: + """ + Finds the poses of base stations and Crazyflie samples given a list of matched samples. + The solver is iterative and uses least squares fitting to minimize the distance from + the lighthouse sensors to each "ray" measured in the samples. + + The equation system that is solved is defined as: + + Columns are the estimated poses (what we solve for). Each pose is composed of 6 numbers (often referred to as + parameters in the code): rotation vector (3) and position (3). + + Rows are representing one angle from one base station. The number of rows for each sample is given by the + number of bs in the sample * n_sensors * 2. + + An examples matrix: + + bs0_pose, bs1_pose, bs2_pose, bs3_pose, cf1_pose, cf2_pose, ... + + cf0/bs2/sens0/ang0 X + cf0/bs2/sens0/ang1 X + cf0/bs2/sens1/ang0 X + cf0/bs2/sens1/ang1 X + ... + cf0/bs3/sens0/ang0 X + cf0/bs3/sens0/ang1 X + cf0/bs3/sens1/ang0 X + cf0/bs3/sens1/ang1 X + ... + cf1/bs1/sens0/ang0 X X + cf1/bs1/sens0/ang1 X X + cf1/bs1/sens1/ang0 X X + cf1/bs1/sens1/ang1 X X + ... + cf1/bs2/sens0/ang0 X X + cf1/bs2/sens0/ang1 X X + cf1/bs2/sens1/ang0 X X + cf1/bs2/sens1/ang1 X X + ... + cf2/bs1/sens0/ang0 X X + cf2/bs1/sens0/ang1 X X + cf2/bs1/sens1/ang0 X X + cf2/bs1/sens1/ang1 X X + ... + cf2/bs3/sens0/ang0 X X + cf2/bs3/sens0/ang1 X X + cf2/bs3/sens1/ang0 X X + cf2/bs3/sens1/ang1 X X + ... + + """ + + @classmethod + def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[LhCfPoseSample], + sensor_positions: npt.ArrayLike) -> LighthouseGeometrySolution: + """ + Solve for the pose of base stations and CF samples. + The pose of the CF in sample 0 defines the global reference frame. + + Iteration is terminated after a fixed number of iteration if acceptable solution + is found. + + :param initial_guess_bs_poses: Initial guess for the base station poses + :param matched_samples: List of matched samples, must have been augmented with + initial guesses of the CF poses + :param sensor_positions: Sensor positions (3D), in the CF reference frame + :return: an instance of LighthouseGeometrySolution + """ + defs = LighthouseGeometrySolution() + + defs.n_bss = len(initial_guess_bs_poses) + defs.n_cfs = len(matched_samples) + defs.n_cfs_in_params = len(matched_samples) - 1 + defs.n_sensors = len(sensor_positions) + defs.bs_id_to_index, defs.bs_index_to_id = cls._crate_bs_map(initial_guess_bs_poses) + + target_angles = cls._populate_target_angles(matched_samples, defs) + idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, jac_sparsity = cls._populate_indexes_and_jacobian( + matched_samples, defs) + params_bs, params_cfs = cls._populate_initial_guess(initial_guess_bs_poses, matched_samples, defs) + + # Extra arguments passed on to calc_residual() + args = (defs, idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, target_angles, sensor_positions) + + # Vector to optimize. Composed of base station parameters followed by cf parameters + x0 = np.hstack((params_bs.ravel(), params_cfs.ravel())) + + result = scipy.optimize.least_squares(cls._calc_residual, + x0, + verbose=0, + jac_sparsity=jac_sparsity, + x_scale='jac', + ftol=1e-8, + method='trf', + max_nfev=defs.max_nr_iter, + args=args) + + defs.lsq_result = result + + bss, cf_poses = cls._params_to_struct(result.x, defs) + + matched_samples[0].estimated_pose = Pose() + for i, sample in enumerate(matched_samples[1:]): + sample.estimated_pose = cls._params_to_pose(cf_poses[i], defs) + + defs.bs_poses = {} + for index, pose in enumerate(bss): + bs_id = defs.bs_index_to_id[index] + defs.bs_poses[bs_id] = cls._params_to_pose(pose, defs) + + return defs + + @classmethod + def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample], defs: LighthouseGeometrySolution + ) -> npt.NDArray: + """ + A np.array of all measured angles, the target angles + """ + result = [] + for sample in matched_samples: + for bs_id, angles in sorted(sample.angles_calibrated.items()): + result += angles.angle_list().tolist() + + return np.array(result) + + @classmethod + def _populate_indexes_and_jacobian(cls, matched_samples: list[LhCfPoseSample], defs: LighthouseGeometrySolution + ) -> tuple[npt.NDArray, npt.NDArray, npt.NDArray, npt.NDArray]: + """ + To speed up calculations all operations in the iteration phase are done on np.arrays of equal length (ish), + the numpy flavour of parallell work. Some data is reused in multiple equations (for instance sensor + positions) and to avoid copying of data we use np.arrays as indexes. This method creates the necessary + indexes. + + Since the equation system we are solving is sparse, we can also do a second optimization and only calculate + the parts of the matrix that are non-zero. We have to provide a matrix containing 1 in the positions where + the jacobian is non-zero, this matrix is also generated here. + """ + index_angle_pair_to_bs = [] + index_angle_pair_to_cf = [] + index_angle_pair_to_sensor_base = [] + + # Note: Indexes are for angle pairs, that is one set of indexes for two equations in the matrix. + # Each set of indexes will result in two angles, horizontal and vertical, which means there is one set of + # indexes per sensor. + + for cf_i, sample in enumerate(matched_samples): + for bs_id in sorted(sample.angles_calibrated.keys()): + bs_index = defs.bs_id_to_index[bs_id] + for sensor_i in range(defs.n_sensors): + index_angle_pair_to_cf.append(cf_i) + index_angle_pair_to_bs.append(bs_index) + index_angle_pair_to_sensor_base.append(sensor_i) + + # Length of residual vector + len_residual_vec = len(index_angle_pair_to_cf) * 2 + + # Length of param vector + len_param_vec = defs.n_bss * defs.n_params_per_bs + defs.n_cfs_in_params * defs.n_params_per_cf + + # The jac_sparsity matrix should have ones in all locations where data is used in the equations + jac_sparsity = scipy.sparse.lil_matrix((len_residual_vec, len_param_vec), dtype=int) + row_i = 0 + n_tot_bs_params = defs.n_bss * defs.n_params_per_bs + for cf_i, sample in enumerate(matched_samples): + for bs_id in sorted(sample.angles_calibrated.keys()): + bs_index = defs.bs_id_to_index[bs_id] + for sensor_i in range(defs.n_sensors * 2): + # Add bs parameters + first = bs_index * defs.n_params_per_bs + for i in range(first, first + defs.n_params_per_bs): + jac_sparsity[row_i, i] = 1 + # Add cf parameters + if cf_i > 0: + first = n_tot_bs_params + (cf_i - 1) * defs.n_params_per_cf + for i in range(first, first + defs.n_params_per_cf): + jac_sparsity[row_i, i] = 1 + + row_i += 1 + + return (np.array(index_angle_pair_to_bs), + np.array(index_angle_pair_to_cf), + np.array(index_angle_pair_to_sensor_base), + jac_sparsity) + + @classmethod + def _populate_initial_guess(cls, initial_guess_bs_poses: list[Pose], matched_samples: list[LhCfPoseSample], + defs: LighthouseGeometrySolution) -> tuple[npt.NDArray, npt.NDArray]: + """ + Generate parameters for base stations and CFs, this is the initial guess we start to iterate from. + """ + params_bs = np.zeros((len(initial_guess_bs_poses) * defs.n_params_per_bs)) + for bs_id, pose in initial_guess_bs_poses.items(): + index = defs.bs_id_to_index[bs_id] * defs.n_params_per_bs + params_bs[index:index + defs.n_params_per_bs] = cls._pose_to_params(pose) + + # Skip the first CF pose, it is the definition of the origin and is not a parameter + params_cfs = [] + for sample in matched_samples[1:]: + params_cfs.append(cls._pose_to_params(sample.inital_est_pose)) + + return params_bs, np.array(params_cfs) + + @classmethod + def _params_to_struct(cls, params, defs: LighthouseGeometrySolution): + """ + Convert the params list to two arrays, one for base stations and one for CFs + """ + bs_param_count = defs.n_bss * defs.n_params_per_bs + params_bs_poses = params[:bs_param_count].reshape((defs.n_bss, defs.n_params_per_bs)) + + params_cf_poses = params[bs_param_count:].reshape((defs.n_cfs_in_params, defs.n_params_per_cf)) + + return params_bs_poses, params_cf_poses + + @classmethod + def _calc_residual(cls, params, defs: LighthouseGeometrySolution, index_angle_pair_to_bs, index_angle_pair_to_cf, + index_angle_pair_to_sensor_base, target_angles, sensor_positions): + """ + Calculate the residual for a set of parameters. The residual is defined as the distance from a sensor to the + plane given by a measured base station angle. + + :param params: list of parameters for base stations and CFs + :param defs: information about the context + :param index_angle_pair_to_bs: index array to index into the base station part of the parameter set + :param index_angle_pair_to_cf: index array to index into the CF part of the parameter set + :param index_angle_pair_to_sensor_base: index array to index into the sensor position array + :param target_angles: the measured angles + :param sensor_positions: Array with sensor positions + :return: Array with residuals + """ + bss, cfs = cls._params_to_struct(params, defs) + + # The first CF pose is defining the origin and is added here + cfs_full = np.concatenate((np.zeros((1, defs.n_params_per_cf), dtype=float), cfs)) + + angle_pairs = cls._poses_to_angle_pairs(bss, cfs_full, sensor_positions, index_angle_pair_to_bs, + index_angle_pair_to_cf, index_angle_pair_to_sensor_base, defs) + angles = np.ravel(angle_pairs) + + diff = angles - target_angles + + # Calculate the error at the CF positions + distances_to_cfs = np.repeat(np.linalg.norm( + bss[index_angle_pair_to_bs] - cfs_full[index_angle_pair_to_cf], axis=1), 2) + residual = np.tan(diff) * distances_to_cfs + + return residual + + @classmethod + def _poses_to_angle_pairs(cls, bss, cf_poses, sensor_base_pos, index_angle_pair_to_bs, index_angle_pair_to_cf, + index_angle_pair_to_sensor_base, defs: LighthouseGeometrySolution): + pairs = cls._calc_angle_pairs(bss[index_angle_pair_to_bs], cf_poses[index_angle_pair_to_cf], + sensor_base_pos[index_angle_pair_to_sensor_base], defs) + return pairs + + @classmethod + def _calc_angle_pairs(cls, bs_p_a, cf_p_a, sens_pos_p_a, defs: LighthouseGeometrySolution): + """ + Calculate angle pairs based on base station poses, cf poses and sensor positions + + :param bs_p_a: Poses base stations + :param cf_p_a: Poses CFs + :paran sens_pos_p_a: Sensor positions + :return: angle pairs + + All lists are equally long, one entry per output angle pair + """ + sensor_points = cls._rotate_translate(sens_pos_p_a, cf_p_a[:, :defs.len_rot_vec], cf_p_a[:, defs.len_rot_vec:]) + + # translate and inverse rotate (-rotation vector == inverse rotation) + points_bs_ref = cls._rotate_translate(sensor_points - bs_p_a[:, defs.len_rot_vec:defs.n_params_per_bs], + -bs_p_a[:, :defs.len_rot_vec], + np.zeros_like(bs_p_a[:, defs.len_rot_vec:defs.n_params_per_bs])) + + angle_pair = np.arctan2(points_bs_ref[:, 1:3], points_bs_ref[:, 0, np.newaxis]) + return angle_pair + + @classmethod + def _rotate_translate(cls, points, rot_vecs, translations): + """Rotate points by given rotation vectors and translate + + Rodrigues' rotation formula is used. + """ + theta = np.linalg.norm(rot_vecs, axis=1)[:, np.newaxis] + with np.errstate(invalid='ignore'): + v = rot_vecs / theta + v = np.nan_to_num(v) + dot = np.sum(points * v, axis=1)[:, np.newaxis] + cos_theta = np.cos(theta) + sin_theta = np.sin(theta) + + return cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v + translations + + @classmethod + def _pose_to_params(cls, pose: Pose) -> npt.NDArray: + """ + Convert from Pose to the array format used in the solver + """ + return np.concatenate((pose.rot_vec, pose.translation)) + + @classmethod + def _params_to_pose(cls, params: npt.ArrayLike, defs: LighthouseGeometrySolution) -> Pose: + """ + Convert from the array format used in the solver to Pose + """ + r_vec = params[:defs.len_rot_vec] + t = params[defs.len_rot_vec:defs.len_pose] + return Pose.from_rot_vec(R_vec=r_vec, t_vec=t) + + @classmethod + def _crate_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[int, int], dict[int, int]]: + """ + We might have gaps in the list of base station ids that is used in the system, use an index instead + when refering to a base station. This method creates dictionaries to go from index to base station id, + or the other way around. + + Base station ids are indexed in an increasing order which means that sorting keys will result + in sorted indexes as well. + """ + bs_id_to_index = {} + bs_index_to_id = {} + + for index, id in enumerate(sorted(initial_guess_bs_poses.keys())): + bs_id_to_index[id] = index + bs_index_to_id[index] = id + + return bs_id_to_index, bs_index_to_id + + # TODO + + # @classmethod + # def estimate_error(cls, bs_poses, matched_samples, sensor_base_pos): + # # Estimate the error (in meters) for all CF positions by calculating + # # the distance from the ray (defined by measured angles/bs pose) to + # # the estimated sensor position + # for sample in matched_samples: + # cf_pose = sample['pose'] + # errors = {} + # for bs_id, angles in sample['data'].items(): + # # Only look at sensor 0 + # horiz = angles[0] + # vert = angles[1] + + # bs_pose = bs_poses[bs_id] + # ray = cls._calc_ray(bs_pose, horiz, vert) + + # sensor_pos = np.dot(cf_pose[0], sensor_base_pos[0]) + cf_pose[1] + + # error_dist = cls._distance_point_to_ray(sensor_pos, ray) + # errors[bs_id] = error_dist + # sample['error'] = errors + + # error_info = cls._analyze_errors(matched_samples) + # return error_info + + # @classmethod + # def _analyze_errors(cls, matched_samples): + # error_per_bs = {} + # errors = [] + # for sample in matched_samples: + # for bs_id, error in sample['error'].items(): + # if bs_id not in error_per_bs: + # error_per_bs[bs_id] = [] + # error_per_bs[bs_id].append(error) + # errors.append(error) + + # error_info = {} + # error_info['mean_error'] = np.mean(errors) + # error_info['max_error'] = np.max(errors) + # error_info['std_error'] = np.std(errors) + + # error_info['bs'] = {} + # for bs_id, errors in error_per_bs.items(): + # error_info['bs'][bs_id] = {} + # error_info['bs'][bs_id]['mean_error'] = np.mean(errors) + # error_info['bs'][bs_id]['max_error'] = np.max(errors) + # error_info['bs'][bs_id]['std_error'] = np.std(errors) + + # return error_info + + # @classmethod + # def _calc_ray(cls, geometry, horiz, vert): + # n1 = np.array([np.sin(horiz), -np.cos(horiz), 0.0]) + # n2 = np.array([-np.sin(vert), 0.0, np.cos(vert)]) + + # n21 = np.cross(n2, n1) + # normal = n21 / np.linalg.norm(n21) + + # R = geometry[0] + # vec = np.dot(R, normal) + # return Ray(geometry[1], vec) + + # @classmethod + # def _distance_point_to_ray(cls, point, ray): + # return np.linalg.norm(np.cross(ray.vector, ray.position - point)) / np.linalg.norm(ray.vector) diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index 55fb6b70d..7452134fb 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -124,6 +124,9 @@ def __init__(self, timestamp: float = 0.0, angles_calibrated: dict[int, Lighthou # The initial estimate of the CF pose for this sample, in the global ref frame self.inital_est_pose: Pose = None + # The refined estimate of the CF pose for this sample, in the global ref frame + self.estimated_pose: Pose = None + class LhDeck4SensorPositions: """ Positions of the sensors on the Lighthouse 4 deck """ diff --git a/test/localization/lighthouse_test_base.py b/test/localization/lighthouse_test_base.py index 049f5d7ec..cdc966f4b 100644 --- a/test/localization/lighthouse_test_base.py +++ b/test/localization/lighthouse_test_base.py @@ -23,6 +23,7 @@ import numpy as np import numpy.typing as npt +from scipy.spatial.transform.rotation import Rotation from cflib.localization.lighthouse_types import Pose @@ -46,6 +47,13 @@ def assertPosesAlmostEqual(self, expected: Pose, actual: Pose, places: int = 5): self.assertAlmostEqual(0.0, np.linalg.norm(translation_diff), places, f'Translation different, expected: {expected.translation}, actual: {actual.translation}') - rotation_diff = expected.rot_vec - actual.rot_vec + def un_ambiguize(rot_vec): + quat = Rotation.from_rotvec(expected.rot_vec).as_quat() + return Rotation.from_quat(quat).as_rotvec() + + _expected_rot_vec = un_ambiguize(expected.rot_vec) + _actual_rot_vec = un_ambiguize(actual.rot_vec) + + rotation_diff = _expected_rot_vec - _actual_rot_vec self.assertAlmostEqual(0.0, np.linalg.norm(rotation_diff), places, f'Rotation different, expected: {expected.rot_vec}, actual: {actual.rot_vec}') diff --git a/test/localization/test_lighthouse_bs_vector.py b/test/localization/test_lighthouse_bs_vector.py index 43a9b4e77..8d490cd91 100644 --- a/test/localization/test_lighthouse_bs_vector.py +++ b/test/localization/test_lighthouse_bs_vector.py @@ -19,7 +19,7 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import unittest +from test.localization.lighthouse_test_base import LighthouseTestBase import numpy as np @@ -27,7 +27,7 @@ from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors -class TestLighthouseBsVector(unittest.TestCase): +class TestLighthouseBsVector(LighthouseTestBase): def setUp(self): pass @@ -147,3 +147,18 @@ def test_conversion_to_projection_pair_list(self): self.assertEqual(len(vectors), len(actual)) self.assertListEqual(vectors[0].projection.tolist(), actual[0].tolist()) self.assertListEqual(vectors[3].projection.tolist(), actual[3].tolist()) + + def test_conversion_to_angle_list(self): + # Fixture + vectors = LighthouseBsVectors(( + LighthouseBsVector(0.0, 0.1), + LighthouseBsVector(0.2, 0.3), + LighthouseBsVector(0.4, 0.5), + LighthouseBsVector(0.6, 0.7), + )) + + # Test + actual = vectors.angle_list() + + # Assert + self.assertVectorsAlmostEqual((0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7), actual) diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py new file mode 100644 index 000000000..62a19cd27 --- /dev/null +++ b/test/localization/test_lighthouse_geometry_solver.py @@ -0,0 +1,111 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +from test.localization.lighthouse_fixtures import LighthouseFixtures +from test.localization.lighthouse_test_base import LighthouseTestBase + +from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver +from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import LhDeck4SensorPositions + + +class TestLighthouseGeometrySolver(LighthouseTestBase): + def setUp(self): + self.fixtures = LighthouseFixtures() + + def test_that_one_bs_poses_in_one_sample_is_estimated(self): + # Fixture + # CF_ORIGIN is used in the first sample and will define the global reference frame + bs_id0 = 3 + matched_samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf_origin_bs0, + }), + ] + + initial_guess_bs_poses = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + + # Test + actual = LighthouseGeometrySolver.solve( + initial_guess_bs_poses, matched_samples, LhDeck4SensorPositions.positions) + + # Assert + bs_poses = actual.bs_poses + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3) + + def test_that_two_bs_poses_in_one_sample_are_estimated(self): + # Fixture + # CF_ORIGIN is used in the first sample and will define the global reference frame + bs_id0 = 3 + bs_id1 = 1 + matched_samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf_origin_bs0, + bs_id1: self.fixtures.angles_cf_origin_bs1, + }), + ] + + initial_guess_bs_poses = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + + # Test + actual = LighthouseGeometrySolver.solve( + initial_guess_bs_poses, matched_samples, LhDeck4SensorPositions.positions) + + # Assert + bs_poses = actual.bs_poses + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, bs_poses[bs_id1], places=3) + + def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self): + # Fixture + # CF_ORIGIN is used in the first sample and will define the global reference frame + bs_id0 = 3 + bs_id1 = 1 + bs_id2 = 2 + bs_id3 = 0 + matched_samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf_origin_bs0, + bs_id1: self.fixtures.angles_cf_origin_bs1, + }), + LhCfPoseSample(angles_calibrated={ + bs_id1: self.fixtures.angles_cf1_bs1, + bs_id2: self.fixtures.angles_cf1_bs2, + }), + LhCfPoseSample(angles_calibrated={ + bs_id2: self.fixtures.angles_cf2_bs2, + bs_id3: self.fixtures.angles_cf2_bs3, + }), + ] + + initial_guess_bs_poses = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + + # Test + actual = LighthouseGeometrySolver.solve( + initial_guess_bs_poses, matched_samples, LhDeck4SensorPositions.positions) + + # Assert + bs_poses = actual.bs_poses + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, bs_poses[bs_id1], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, bs_poses[bs_id2], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS3_POSE, bs_poses[bs_id3], places=3) From fe761049aeb38c4bbc472099a6fd0b9130693246 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 7 Jan 2022 16:40:30 +0100 Subject: [PATCH 318/861] Use new geometry estimator instead of open cv --- cflib/localization/lighthouse_bs_geo.py | 177 +++----------------- test/localization/test_lighthouse_bs_geo.py | 47 ------ 2 files changed, 19 insertions(+), 205 deletions(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index 8771063d2..459b3cecd 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -6,7 +6,7 @@ # | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2021 Bitcraze AB +# Copyright (C) 2021-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 @@ -22,10 +22,13 @@ """ Functionality to handle base station geometry in the lighthouse poistioning system """ -import math - -import cv2 as cv -import numpy as np +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors +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_types import LhDeck4SensorPositions +from cflib.localization.lighthouse_types import LhMeasurement class LighthouseBsGeoEstimator: @@ -35,48 +38,10 @@ class LighthouseBsGeoEstimator: """ def __init__(self): - self._directions = { - self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), - self._hash_sensor_order([2, 0, 3, 1]): math.radians(25), - self._hash_sensor_order([2, 3, 0, 1]): math.radians(65), - self._hash_sensor_order([3, 2, 0, 1]): math.radians(90), - self._hash_sensor_order([3, 2, 1, 0]): math.radians(115), - self._hash_sensor_order([3, 1, 2, 0]): math.radians(155), - self._hash_sensor_order([1, 3, 2, 0]): math.radians(180), - self._hash_sensor_order([1, 3, 0, 2]): math.radians(205), - self._hash_sensor_order([1, 0, 3, 2]): math.radians(245), - self._hash_sensor_order([0, 1, 3, 2]): math.radians(270), - self._hash_sensor_order([0, 1, 2, 3]): math.radians(295), - self._hash_sensor_order([0, 2, 1, 3]): math.radians(335), - } - - # Sensor distances on the lighthouse deck - sensor_distance_width = 0.015 - sensor_distance_length = 0.03 - - # Sensor positions in world coordinates, open cv style - self._lighthouse_3d = np.float32( - [ - [-sensor_distance_width / 2, 0, -sensor_distance_length / 2], - [sensor_distance_width / 2, 0, -sensor_distance_length / 2], - [-sensor_distance_width / 2, 0, sensor_distance_length / 2], - [sensor_distance_width / 2, 0, sensor_distance_length / 2] - ]) - - # Camera matrix - self._K = np.float64( - [ - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0] - ]) - - self._dist_coef = np.zeros(4) - # Sanity check maximum pos - self._sanity_max_pos = 10 + self._sanity_max_pos = 15 - def estimate_geometry(self, bs_vectors): + def estimate_geometry(self, bs_vectors: list[LighthouseBsVector]): """ Estimate the full pose of a base station based on angles from the 4 sensors on a lighthouse deck. The result is a rotation matrix and position of the @@ -86,11 +51,15 @@ def estimate_geometry(self, bs_vectors): :return rot_bs_in_cf_coord: Rotation matrix of the BS in the CFs coordinate system :return pos_bs_in_cf_coord: Position vector of the BS in the CFs coordinate system """ - guess_yaw = self._find_initial_yaw_guess(bs_vectors) - rvec_guess, tvec_guess = self._convert_yaw_to_open_cv(guess_yaw) - rw_ocv, tw_ocv = self._estimate_pose_by_pnp(bs_vectors, rvec_guess, tvec_guess) - rot_bs_in_cf_coord, pos_bs_in_cf_coord = self._opencv_to_cf(rw_ocv, tw_ocv) - return rot_bs_in_cf_coord, pos_bs_in_cf_coord + bs_id = 0 + + samples = [LhMeasurement(timestamp=0.0, base_station_id=bs_id, angles=LighthouseBsVectors(bs_vectors))] + matched_samples = LighthouseSampleMatcher.match(samples) + initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) + pose = solution.bs_poses[bs_id] + + return pose.rot_matrix, pose.translation def sanity_check_result(self, pos_bs_in_cf_coord): """ @@ -101,111 +70,3 @@ def sanity_check_result(self, pos_bs_in_cf_coord): if (abs(coord) > self._sanity_max_pos): return False return True - - def _find_initial_yaw_guess(self, bs_vectors): - # Assume bs is faicing slighly downwards and fairly horizontal - # Sort sensors in the order they are hit by the horizontal sweep - # and use the order to figure out roughly the direction to the - # base station - sweeps_x = { - 0: bs_vectors[0].lh_v1_horiz_angle, - 1: bs_vectors[1].lh_v1_horiz_angle, - 2: bs_vectors[2].lh_v1_horiz_angle, - 3: bs_vectors[3].lh_v1_horiz_angle - } - - ordered_map = {k: v for k, v in sorted(sweeps_x.items(), key=lambda item: item[1])} - sensor_order = list(ordered_map.keys()) - - # The base station is roughly in this direction, in CF (world) coordinates - return self._directions[self._hash_sensor_order(sensor_order)] - - def _hash_sensor_order(self, order): - hash = 0 - for i in range(4): - hash += order[i] * 4 ** i - return hash - - def _convert_yaw_to_open_cv(self, yaw): - # Base station height - bs_h = 2.0 - # Distance to base station along the floor - bs_fd = 3.0 - # Distance to base station - bs_dist = math.sqrt(bs_h ** 2 + bs_fd ** 2) - elevation = math.atan2(bs_h, bs_fd) - - # Initial position of the CF in camera coordinate system, open cv style - tvec_start = np.array([0, 0, bs_dist]) - - # Calculate rotation matrix - d_c = math.cos(-yaw + math.pi) - d_s = math.sin(-yaw + math.pi) - R_rot_y = np.array([ - [d_c, 0.0, d_s], - [0.0, 1.0, 0.0], - [-d_s, 0.0, d_c], - ]) - - e_c = math.cos(elevation) - e_s = math.sin(elevation) - R_rot_x = np.array([ - [1.0, 0.0, 0.0], - [0.0, e_c, -e_s], - [0.0, e_s, e_c], - ]) - - R = np.dot(R_rot_x, R_rot_y) - rvec_start, _ = cv.Rodrigues(R) - - return rvec_start, tvec_start - - def _estimate_pose_by_pnp(self, bs_vectors, rvec_start, tvec_start): - # Sensors as seen by the "camera" - lighthouse_image_projection = np.float32( - [ - [-math.tan(bs_vectors[0].lh_v1_horiz_angle), -math.tan(bs_vectors[0].lh_v1_vert_angle)], - [-math.tan(bs_vectors[1].lh_v1_horiz_angle), -math.tan(bs_vectors[1].lh_v1_vert_angle)], - [-math.tan(bs_vectors[2].lh_v1_horiz_angle), -math.tan(bs_vectors[2].lh_v1_vert_angle)], - [-math.tan(bs_vectors[3].lh_v1_horiz_angle), -math.tan(bs_vectors[3].lh_v1_vert_angle)] - ]) - - _ret, rvec_est, tvec_est = cv.solvePnP( - self._lighthouse_3d, - lighthouse_image_projection, - self._K, - self._dist_coef, - flags=cv.SOLVEPNP_ITERATIVE, - rvec=rvec_start, - tvec=tvec_start, - useExtrinsicGuess=True) - - if not _ret: - raise Exception('No solution found') - - Rw_ocv, Tw_ocv = self._cam_to_world(rvec_est, tvec_est) - return Rw_ocv, Tw_ocv - - def _cam_to_world(self, rvec_c, tvec_c): - R_c, _ = cv.Rodrigues(rvec_c) - R_w = np.linalg.inv(R_c) - tvec_w = -np.matmul(R_w, tvec_c) - return R_w, tvec_w - - def _opencv_to_cf(self, R_cv, t_cv): - R_opencv_to_cf = np.array([ - [0.0, 0.0, 1.0], - [-1.0, 0.0, 0.0], - [0.0, -1.0, 0.0], - ]) - - R_cf_to_opencv = np.array([ - [0.0, -1.0, 0.0], - [0.0, 0.0, -1.0], - [1.0, 0.0, 0.0], - ]) - - t_cf = np.dot(R_opencv_to_cf, t_cv) - R_cf = np.dot(R_opencv_to_cf, np.dot(R_cv, R_cf_to_opencv)) - - return R_cf, t_cf diff --git a/test/localization/test_lighthouse_bs_geo.py b/test/localization/test_lighthouse_bs_geo.py index 2ea388b04..aee8cf295 100644 --- a/test/localization/test_lighthouse_bs_geo.py +++ b/test/localization/test_lighthouse_bs_geo.py @@ -19,11 +19,9 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import math import unittest from cflib.localization import LighthouseBsGeoEstimator -from cflib.localization import LighthouseBsVector class TestLighthouseBsGeoEstimator(unittest.TestCase): @@ -31,51 +29,6 @@ def setUp(self): self.sut = LighthouseBsGeoEstimator() - def test_that_initial_yaw_guess_is_correct_ba_is_behind(self): - # Fixture - bs_vectors = [ - LighthouseBsVector(1, 0), - LighthouseBsVector(2, 0), - LighthouseBsVector(0, 0), - LighthouseBsVector(3, 0), - ] - - # Test - actual = self.sut._find_initial_yaw_guess(bs_vectors) - - # Assert - self.assertEqual(0.0, actual) - - def test_that_initial_yaw_guess_is_correct_ba_is_front(self): - # Fixture 1, 3, 2, 0 - bs_vectors = [ - LighthouseBsVector(3, 0), - LighthouseBsVector(0, 0), - LighthouseBsVector(2, 0), - LighthouseBsVector(1, 0), - ] - - # Test - actual = self.sut._find_initial_yaw_guess(bs_vectors) - - # Assert - self.assertEqual(math.radians(180), actual) - - def test_that_initial_yaw_guess_is_correct_bs_left_behind(self): - # Fixture - bs_vectors = [ - LighthouseBsVector(1.0, 0), - LighthouseBsVector(-0.5, 0), - LighthouseBsVector(0.5, 0), - LighthouseBsVector(-1.0, 0), - ] - - # Test - actual = self.sut._find_initial_yaw_guess(bs_vectors) - - # Assert - self.assertEqual(math.radians(155), actual) - def test_that_sanity_check_finds_coordinate_out_of_bounds(self): # Fixture pos_bs_in_cf_coord = [0, -20, 0] From 59aaaed932a0154d19ccf6b41a4c351dab2d404d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 7 Jan 2022 16:41:45 +0100 Subject: [PATCH 319/861] Removed dependency to open cv --- setup.py | 1 - 1 file changed, 1 deletion(-) diff --git a/setup.py b/setup.py index cfe0e1d15..58183b61b 100644 --- a/setup.py +++ b/setup.py @@ -34,7 +34,6 @@ install_requires=[ 'pyusb>=1.0.0b2', - 'opencv-python-headless~=4.5.1' ], # $ pip install -e .[dev] From 223e8167c2b584c6e20a4fc31e36e0921a22cb1f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 7 Jan 2022 18:48:54 +0100 Subject: [PATCH 320/861] Added script to run calibration --- cflib/localization/lighthouse_bs_geo.py | 3 +- .../localization/lighthouse_sample_matcher.py | 2 +- .../lighthouse_sweep_angle_reader.py | 5 +- examples/lighthouse/bs_geometry_estimation.py | 178 ++++++++++++++++++ .../test_lighthouse_sample_matcher.py | 13 +- 5 files changed, 194 insertions(+), 7 deletions(-) create mode 100644 examples/lighthouse/bs_geometry_estimation.py diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index 459b3cecd..db63971a6 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -23,7 +23,6 @@ Functionality to handle base station geometry in the lighthouse poistioning system """ from cflib.localization.lighthouse_bs_vector import LighthouseBsVector -from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher @@ -53,7 +52,7 @@ def estimate_geometry(self, bs_vectors: list[LighthouseBsVector]): """ bs_id = 0 - samples = [LhMeasurement(timestamp=0.0, base_station_id=bs_id, angles=LighthouseBsVectors(bs_vectors))] + samples = [LhMeasurement(timestamp=0.0, base_station_id=bs_id, angles=bs_vectors)] matched_samples = LighthouseSampleMatcher.match(samples) initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py index 052b3ca97..bac20ef62 100644 --- a/cflib/localization/lighthouse_sample_matcher.py +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -55,5 +55,5 @@ def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.010, @classmethod def _append_result(cls, current: LhCfPoseSample, result: list[LhCfPoseSample], min_nr_of_bs_in_match: int): - if len(current.angles_calibrated) > min_nr_of_bs_in_match: + if current is not None and len(current.angles_calibrated) >= min_nr_of_bs_in_match: result.append(current) diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py index a2e47a891..ae70de3fe 100644 --- a/cflib/localization/lighthouse_sweep_angle_reader.py +++ b/cflib/localization/lighthouse_sweep_angle_reader.py @@ -20,6 +20,7 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . from cflib.localization import LighthouseBsVector +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors class LighthouseSweepAngleReader(): @@ -66,7 +67,7 @@ def _packet_received_cb(self, packet): for i in range(self.NR_OF_SENSORS): result.append(LighthouseBsVector(horiz_angles[i], vert_angles[i])) - self._cb(base_station_id, result) + self._cb(base_station_id, LighthouseBsVectors(result)) class LighthouseSweepAngleAverageReader(): @@ -142,7 +143,7 @@ def _average_sample_lists(self, sample_lists): for i in range(self._reader.NR_OF_SENSORS): result.append(self._average_sample_list(sample_lists[i])) - return result + return LighthouseBsVectors(result) def _average_sample_list(self, sample_list): sum_horiz = 0.0 diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py new file mode 100644 index 000000000..13038dd25 --- /dev/null +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -0,0 +1,178 @@ +# -*- 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +''' +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 supports large systems with more than 2 base stations (if +supported by the CF firmware). + +This script is a temporary implementation until similar functionality has been +added to the client. + +Prerequisite: The base station calibration data must have been +received by the Crazyflie before this script is executed. +''' +import logging +import time +from threading import Event + +import cflib.crtp # noqa +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.mem import LighthouseMemHelper +from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +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_types import LhCfPoseSample +from cflib.localization.lighthouse_types import LhDeck4SensorPositions +from cflib.localization.lighthouse_types import LhMeasurement +from cflib.utils import uri_helper + + +def record_angles_average(scf): + recorded_angles = None + + isReady = Event() + + def ready_cb(averages): + nonlocal recorded_angles + recorded_angles = averages + isReady.set() + + reader = LighthouseSweepAngleAverageReader(scf.cf, ready_cb) + reader.start_angle_collection() + isReady.wait() + + angles_calibrated = {} + for bs_id, data in recorded_angles.items(): + angles_calibrated[bs_id] = data[1] + + return LhCfPoseSample(angles_calibrated=angles_calibrated) + + +def record_angles_sequence(scf, recording_time_s): + result = [] + + def ready_cb(bs_id, angles): + now = time.time() + measurement = LhMeasurement(timestamp=now, base_station_id=bs_id, angles=angles) + result.append(measurement) + + reader = LighthouseSweepAngleReader(scf.cf, ready_cb) + reader.start() + time.sleep(recording_time_s) + reader.stop() + + return result + + +def parse(recording_time, default): + try: + return int(recording_time) + except ValueError: + return default + + +def estimate_geometry(origin, samples): + matched_samples = [origin] + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2) + initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) + + print(' Base stations at:') + for bs_id, pose in sorted(solution.bs_poses.items()): + pos = pose.translation + print(f' {bs_id}: ({pos[0]}, {pos[1]}, {pos[2]})') + + return solution.bs_poses + + +def upload_geometry(scf, bs_poses): + 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(success): + if not success: + raise Exception('Upload failed') + event.set() + + helper = LighthouseMemHelper(scf.cf) + helper.write_geos(geo_dict, data_written) + event.wait() + + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + +if __name__ == '__main__': + uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + print(f'Step 1. Connecting to the Crazyflie on uri {uri}...') + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + print(' Connected') + print('Step 2. Put the Crazyflie where you want the origin of your coordinate system, ' + + 'oriented with forward in the positive X direction.') + input('Press return when ready. ') + print(' Recoding...') + origin = record_angles_average(scf) + print(' Position recorded') + + print('Step 3. 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. ') + print('This step does not add anything in a system with only one base station, enter 0 in this case.') + default_time = 10 + 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, default_time) + print(' Recording started...') + samples = record_angles_sequence(scf, recording_time_s) + print(' Recording ended') + + print('Step 4. Estimating geometry...') + bs_poses = estimate_geometry(origin, samples) + print(' Geometry estimated') + + print('Step 5. Upload geometry to the Crazyflie') + input('Press enter to upload geometry. ') + upload_geometry(scf, bs_poses) + print('Geometry uploaded') diff --git a/test/localization/test_lighthouse_sample_matcher.py b/test/localization/test_lighthouse_sample_matcher.py index 557c7e970..225be6f85 100644 --- a/test/localization/test_lighthouse_sample_matcher.py +++ b/test/localization/test_lighthouse_sample_matcher.py @@ -60,14 +60,23 @@ def test_that_samples_are_aggregated(self): self.assertEqual(1, len(actual[2].angles_calibrated)) self.assertEqual(self.vec3, actual[2].angles_calibrated[1]) - def test_that_single_bs_samples_are_fitered(self): + def test_that_single_bs_samples_are_fitered_out(self): # Fixture # Test - actual = LighthouseSampleMatcher.match(self.samples, max_time_diff=0.010, min_nr_of_bs_in_match=1) + actual = LighthouseSampleMatcher.match(self.samples, max_time_diff=0.010, min_nr_of_bs_in_match=2) # Assert self.assertEqual(1.015, actual[0].timestamp) self.assertEqual(2, len(actual[0].angles_calibrated)) self.assertEqual(self.vec1, actual[0].angles_calibrated[1]) self.assertEqual(self.vec2, actual[0].angles_calibrated[0]) + + def test_that_empty_sample_list_works(self): + # Fixture + + # Test + actual = LighthouseSampleMatcher.match([]) + + # Assert + self.assertEqual(0, len(actual)) From 54fdc2f058d23cfd0e407518189dc88ab1778d73 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 8 Jan 2022 10:02:04 +0100 Subject: [PATCH 321/861] Added error measurement --- .../lighthouse_geometry_solver.py | 70 +++++++++---------- 1 file changed, 34 insertions(+), 36 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 68ec87a8f..99995a6fa 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -49,18 +49,9 @@ def __init__(self) -> None: # The estimated poses of the base stations self.bs_poses: dict[int, Pose] = {} - # The raw result from the scipy.optimize.least_squares() function - self.lsq_result = None + # True if the sover was terminated due to reaching the maximum nr of alowed iterations + self.max_iter_reached: bool = False - @property - def max_iter_reached(self): - """ - True if the sover was terminated due to reaching the maximum nr of alowed iterations - """ - result = False - if self.lsq_result is not None: - result = self.lsq_result == 0 - return result class LighthouseGeometrySolver: @@ -130,21 +121,21 @@ def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[Lh :param sensor_positions: Sensor positions (3D), in the CF reference frame :return: an instance of LighthouseGeometrySolution """ - defs = LighthouseGeometrySolution() + solution = LighthouseGeometrySolution() - defs.n_bss = len(initial_guess_bs_poses) - defs.n_cfs = len(matched_samples) - defs.n_cfs_in_params = len(matched_samples) - 1 - defs.n_sensors = len(sensor_positions) - defs.bs_id_to_index, defs.bs_index_to_id = cls._crate_bs_map(initial_guess_bs_poses) + solution.n_bss = len(initial_guess_bs_poses) + solution.n_cfs = len(matched_samples) + solution.n_cfs_in_params = len(matched_samples) - 1 + solution.n_sensors = len(sensor_positions) + solution.bs_id_to_index, solution.bs_index_to_id = cls._crate_bs_map(initial_guess_bs_poses) - target_angles = cls._populate_target_angles(matched_samples, defs) + target_angles = cls._populate_target_angles(matched_samples, solution) idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, jac_sparsity = cls._populate_indexes_and_jacobian( - matched_samples, defs) - params_bs, params_cfs = cls._populate_initial_guess(initial_guess_bs_poses, matched_samples, defs) + matched_samples, solution) + params_bs, params_cfs = cls._populate_initial_guess(initial_guess_bs_poses, matched_samples, solution) # Extra arguments passed on to calc_residual() - args = (defs, idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, target_angles, sensor_positions) + args = (solution, idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, target_angles, sensor_positions) # Vector to optimize. Composed of base station parameters followed by cf parameters x0 = np.hstack((params_bs.ravel(), params_cfs.ravel())) @@ -156,23 +147,11 @@ def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[Lh x_scale='jac', ftol=1e-8, method='trf', - max_nfev=defs.max_nr_iter, + max_nfev=solution.max_nr_iter, args=args) - defs.lsq_result = result - - bss, cf_poses = cls._params_to_struct(result.x, defs) - - matched_samples[0].estimated_pose = Pose() - for i, sample in enumerate(matched_samples[1:]): - sample.estimated_pose = cls._params_to_pose(cf_poses[i], defs) - - defs.bs_poses = {} - for index, pose in enumerate(bss): - bs_id = defs.bs_index_to_id[index] - defs.bs_poses[bs_id] = cls._params_to_pose(pose, defs) - - return defs + cls._condense_results(result, solution, matched_samples) + return solution @classmethod def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample], defs: LighthouseGeometrySolution @@ -391,6 +370,25 @@ def _crate_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[in return bs_id_to_index, bs_index_to_id + @classmethod + def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, + matched_samples: list[LhCfPoseSample]) -> None: + solution.lsq_result = lsq_result + + bss, cf_poses = cls._params_to_struct(lsq_result.x, solution) + + matched_samples[0].estimated_pose = Pose() + for i, sample in enumerate(matched_samples[1:]): + sample.estimated_pose = cls._params_to_pose(cf_poses[i], solution) + + solution.bs_poses = {} + for index, pose in enumerate(bss): + bs_id = solution.bs_index_to_id[index] + solution.bs_poses[bs_id] = cls._params_to_pose(pose, solution) + + solution.max = lsq_result == 0 + + # TODO # @classmethod From dceba354220bdadba7bcb2ea7597c92c17b5bef0 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 8 Jan 2022 16:36:21 +0100 Subject: [PATCH 322/861] Fixed initial guess --- .../localization/lighthouse_geometry_solver.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 99995a6fa..e9e27c393 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -116,8 +116,9 @@ def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[Lh is found. :param initial_guess_bs_poses: Initial guess for the base station poses - :param matched_samples: List of matched samples, must have been augmented with - initial guesses of the CF poses + :param matched_samples: List of matched samples, must have been augmented with initial guesses of the CF + poses. The samples will be updated with their estimated pose and error during the + process. :param sensor_positions: Sensor positions (3D), in the CF reference frame :return: an instance of LighthouseGeometrySolution """ @@ -232,17 +233,16 @@ def _populate_initial_guess(cls, initial_guess_bs_poses: list[Pose], matched_sam """ Generate parameters for base stations and CFs, this is the initial guess we start to iterate from. """ - params_bs = np.zeros((len(initial_guess_bs_poses) * defs.n_params_per_bs)) + params_bs = np.zeros((defs.n_bss, defs.n_params_per_bs)) for bs_id, pose in initial_guess_bs_poses.items(): - index = defs.bs_id_to_index[bs_id] * defs.n_params_per_bs - params_bs[index:index + defs.n_params_per_bs] = cls._pose_to_params(pose) + params_bs[defs.bs_id_to_index[bs_id], :] = cls._pose_to_params(pose) # Skip the first CF pose, it is the definition of the origin and is not a parameter - params_cfs = [] - for sample in matched_samples[1:]: - params_cfs.append(cls._pose_to_params(sample.inital_est_pose)) + params_cfs = np.zeros((defs.n_cfs_in_params, defs.n_params_per_cf)) + for index, sample in enumerate(matched_samples[1:]): + params_cfs[index, :] = cls._pose_to_params(sample.inital_est_pose) - return params_bs, np.array(params_cfs) + return params_bs, params_cfs @classmethod def _params_to_struct(cls, params, defs: LighthouseGeometrySolution): From c2a0ba02f7d978076121123fa653467e3b826b9e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 8 Jan 2022 17:22:56 +0100 Subject: [PATCH 323/861] Corrected residual calculation --- cflib/localization/lighthouse_geometry_solver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index e9e27c393..012053810 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -285,7 +285,7 @@ def _calc_residual(cls, params, defs: LighthouseGeometrySolution, index_angle_pa # Calculate the error at the CF positions distances_to_cfs = np.repeat(np.linalg.norm( - bss[index_angle_pair_to_bs] - cfs_full[index_angle_pair_to_cf], axis=1), 2) + bss[index_angle_pair_to_bs][:,3:] - cfs_full[index_angle_pair_to_cf][:,3:], axis=1), 2) residual = np.tan(diff) * distances_to_cfs return residual From 3263232baab85585cfaf704d0dd0c7df48442274 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 8 Jan 2022 17:50:07 +0100 Subject: [PATCH 324/861] Use residual for error approximation --- .../lighthouse_geometry_solver.py | 118 ++++++------------ cflib/localization/lighthouse_types.py | 3 + examples/lighthouse/bs_geometry_estimation.py | 5 +- .../test_lighthouse_geometry_solver.py | 8 +- 4 files changed, 52 insertions(+), 82 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 012053810..d6ea2afe5 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -1,5 +1,3 @@ -from collections import namedtuple - import numpy as np import numpy.typing as npt import scipy.optimize @@ -7,8 +5,6 @@ from cflib.localization.lighthouse_types import LhCfPoseSample from cflib.localization.lighthouse_types import Pose -Ray = namedtuple('Ray', ['position', 'vector']) - class LighthouseGeometrySolution: """ @@ -52,6 +48,8 @@ def __init__(self) -> None: # True if the sover was terminated due to reaching the maximum nr of alowed iterations self.max_iter_reached: bool = False + # Information about errors in the solution + self.error_info = {} class LighthouseGeometrySolver: @@ -285,7 +283,7 @@ def _calc_residual(cls, params, defs: LighthouseGeometrySolution, index_angle_pa # Calculate the error at the CF positions distances_to_cfs = np.repeat(np.linalg.norm( - bss[index_angle_pair_to_bs][:,3:] - cfs_full[index_angle_pair_to_cf][:,3:], axis=1), 2) + bss[index_angle_pair_to_bs][:, 3:] - cfs_full[index_angle_pair_to_cf][:, 3:], axis=1), 2) residual = np.tan(diff) * distances_to_cfs return residual @@ -373,86 +371,52 @@ def _crate_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[in @classmethod def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, matched_samples: list[LhCfPoseSample]) -> None: - solution.lsq_result = lsq_result - bss, cf_poses = cls._params_to_struct(lsq_result.x, solution) + # Update matched samples with estimated CF poses matched_samples[0].estimated_pose = Pose() for i, sample in enumerate(matched_samples[1:]): sample.estimated_pose = cls._params_to_pose(cf_poses[i], solution) + # Extract base station pose estimates solution.bs_poses = {} for index, pose in enumerate(bss): bs_id = solution.bs_index_to_id[index] solution.bs_poses[bs_id] = cls._params_to_pose(pose, solution) - solution.max = lsq_result == 0 - - - # TODO - - # @classmethod - # def estimate_error(cls, bs_poses, matched_samples, sensor_base_pos): - # # Estimate the error (in meters) for all CF positions by calculating - # # the distance from the ray (defined by measured angles/bs pose) to - # # the estimated sensor position - # for sample in matched_samples: - # cf_pose = sample['pose'] - # errors = {} - # for bs_id, angles in sample['data'].items(): - # # Only look at sensor 0 - # horiz = angles[0] - # vert = angles[1] - - # bs_pose = bs_poses[bs_id] - # ray = cls._calc_ray(bs_pose, horiz, vert) - - # sensor_pos = np.dot(cf_pose[0], sensor_base_pos[0]) + cf_pose[1] - - # error_dist = cls._distance_point_to_ray(sensor_pos, ray) - # errors[bs_id] = error_dist - # sample['error'] = errors - - # error_info = cls._analyze_errors(matched_samples) - # return error_info - - # @classmethod - # def _analyze_errors(cls, matched_samples): - # error_per_bs = {} - # errors = [] - # for sample in matched_samples: - # for bs_id, error in sample['error'].items(): - # if bs_id not in error_per_bs: - # error_per_bs[bs_id] = [] - # error_per_bs[bs_id].append(error) - # errors.append(error) - - # error_info = {} - # error_info['mean_error'] = np.mean(errors) - # error_info['max_error'] = np.max(errors) - # error_info['std_error'] = np.std(errors) - - # error_info['bs'] = {} - # for bs_id, errors in error_per_bs.items(): - # error_info['bs'][bs_id] = {} - # error_info['bs'][bs_id]['mean_error'] = np.mean(errors) - # error_info['bs'][bs_id]['max_error'] = np.max(errors) - # error_info['bs'][bs_id]['std_error'] = np.std(errors) - - # return error_info - - # @classmethod - # def _calc_ray(cls, geometry, horiz, vert): - # n1 = np.array([np.sin(horiz), -np.cos(horiz), 0.0]) - # n2 = np.array([-np.sin(vert), 0.0, np.cos(vert)]) - - # n21 = np.cross(n2, n1) - # normal = n21 / np.linalg.norm(n21) - - # R = geometry[0] - # vec = np.dot(R, normal) - # return Ray(geometry[1], vec) - - # @classmethod - # def _distance_point_to_ray(cls, point, ray): - # return np.linalg.norm(np.cross(ray.vector, ray.position - point)) / np.linalg.norm(ray.vector) + solution.max_iter_reached = lsq_result == 0 + + # Extract the final error for each sample. Use the approximate distance from the first sensor to the ray + residuals = lsq_result.fun + i = 0 + for sample in matched_samples: + for bs_id in sorted(sample.angles_calibrated.keys()): + sample.estimated_errors[bs_id] = np.linalg.norm(residuals[i:i + 2]) + i += solution.n_sensors * 2 + + solution.error_info = cls._aggregate_error_info(matched_samples) + + @classmethod + def _aggregate_error_info(cls, matched_samples: list[LhCfPoseSample]): + error_per_bs = {} + errors = [] + for sample in matched_samples: + for bs_id, error in sample.estimated_errors.items(): + if bs_id not in error_per_bs: + error_per_bs[bs_id] = [] + error_per_bs[bs_id].append(error) + errors.append(error) + + error_info = {} + error_info['mean_error'] = np.mean(errors) + error_info['max_error'] = np.max(errors) + error_info['std_error'] = np.std(errors) + + error_info['bs'] = {} + for bs_id, errors in error_per_bs.items(): + error_info['bs'][bs_id] = {} + error_info['bs'][bs_id]['mean_error'] = np.mean(errors) + error_info['bs'][bs_id]['max_error'] = np.max(errors) + error_info['bs'][bs_id]['std_error'] = np.std(errors) + + return error_info diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index 7452134fb..16cb1bb48 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -127,6 +127,9 @@ def __init__(self, timestamp: float = 0.0, angles_calibrated: dict[int, Lighthou # The refined estimate of the CF pose for this sample, in the global ref frame self.estimated_pose: Pose = None + # The aprroximate errors in final solution + self.estimated_errors: dict[int, float] = {} + class LhDeck4SensorPositions: """ Positions of the sensors on the Lighthouse 4 deck """ diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index 13038dd25..923b9fb7d 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -112,6 +112,9 @@ def estimate_geometry(origin, samples): for bs_id, pose in sorted(solution.bs_poses.items()): pos = pose.translation print(f' {bs_id}: ({pos[0]}, {pos[1]}, {pos[2]})') + print(' Solution match per base station:') + for bs_id, value in solution.error_info['bs'].items(): + print(f' {bs_id}: {value}') return solution.bs_poses @@ -152,7 +155,7 @@ def data_written(success): print('Step 2. Put the Crazyflie where you want the origin of your coordinate system, ' + 'oriented with forward in the positive X direction.') input('Press return when ready. ') - print(' Recoding...') + print(' Recording...') origin = record_angles_average(scf) print(' Position recorded') diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py index 62a19cd27..384648741 100644 --- a/test/localization/test_lighthouse_geometry_solver.py +++ b/test/localization/test_lighthouse_geometry_solver.py @@ -78,10 +78,10 @@ def test_that_two_bs_poses_in_one_sample_are_estimated(self): def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self): # Fixture # CF_ORIGIN is used in the first sample and will define the global reference frame - bs_id0 = 3 - bs_id1 = 1 - bs_id2 = 2 - bs_id3 = 0 + bs_id0 = 7 + bs_id1 = 2 + bs_id2 = 9 + bs_id3 = 3 matched_samples = [ LhCfPoseSample(angles_calibrated={ bs_id0: self.fixtures.angles_cf_origin_bs0, From 283f0718920ab357a5d5e072a12754d9a8d0cc49 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 9 Jan 2022 11:30:57 +0100 Subject: [PATCH 325/861] Refactoring --- .../lighthouse_geometry_solver.py | 55 +++++++++------- .../lighthouse_initial_estimator.py | 64 ++++++++++--------- cflib/localization/lighthouse_types.py | 18 ++---- .../test_lighthouse_geometry_solver.py | 12 ++-- .../test_lighthouse_initial_estimator.py | 54 ++++++++++++---- 5 files changed, 118 insertions(+), 85 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index d6ea2afe5..317180a54 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -2,6 +2,7 @@ import numpy.typing as npt import scipy.optimize +from cflib.localization.lighthouse_types import LhBsCfPoses from cflib.localization.lighthouse_types import LhCfPoseSample from cflib.localization.lighthouse_types import Pose @@ -45,12 +46,18 @@ def __init__(self) -> None: # The estimated poses of the base stations self.bs_poses: dict[int, Pose] = {} - # True if the sover was terminated due to reaching the maximum nr of alowed iterations - self.max_iter_reached: bool = False + # The estimated poses of the CF samples + self.cf_poses: list[Pose] = [] + + # Estimated error for each base station in each sample + self.estimated_errors: list[dict[int, float]] = [] # Information about errors in the solution self.error_info = {} + # True if the sover was terminated due to reaching the maximum nr of alowed iterations + self.max_iter_reached: bool = False + class LighthouseGeometrySolver: """ @@ -104,7 +111,7 @@ class LighthouseGeometrySolver: """ @classmethod - def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[LhCfPoseSample], + def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> LighthouseGeometrySolution: """ Solve for the pose of base stations and CF samples. @@ -113,7 +120,7 @@ def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[Lh Iteration is terminated after a fixed number of iteration if acceptable solution is found. - :param initial_guess_bs_poses: Initial guess for the base station poses + :param initial_guess_bs_poses: Initial guess for the base station and CF sample poses :param matched_samples: List of matched samples, must have been augmented with initial guesses of the CF poses. The samples will be updated with their estimated pose and error during the process. @@ -122,16 +129,16 @@ def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[Lh """ solution = LighthouseGeometrySolution() - solution.n_bss = len(initial_guess_bs_poses) + solution.n_bss = len(initial_guess.bs_poses) solution.n_cfs = len(matched_samples) solution.n_cfs_in_params = len(matched_samples) - 1 solution.n_sensors = len(sensor_positions) - solution.bs_id_to_index, solution.bs_index_to_id = cls._crate_bs_map(initial_guess_bs_poses) + solution.bs_id_to_index, solution.bs_index_to_id = cls._crate_bs_map(initial_guess.bs_poses) - target_angles = cls._populate_target_angles(matched_samples, solution) + target_angles = cls._populate_target_angles(matched_samples) idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, jac_sparsity = cls._populate_indexes_and_jacobian( matched_samples, solution) - params_bs, params_cfs = cls._populate_initial_guess(initial_guess_bs_poses, matched_samples, solution) + params_bs, params_cfs = cls._populate_initial_guess(initial_guess, solution) # Extra arguments passed on to calc_residual() args = (solution, idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, target_angles, sensor_positions) @@ -153,8 +160,7 @@ def solve(cls, initial_guess_bs_poses: dict[int, Pose], matched_samples: list[Lh return solution @classmethod - def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample], defs: LighthouseGeometrySolution - ) -> npt.NDArray: + def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample]) -> npt.NDArray: """ A np.array of all measured angles, the target angles """ @@ -226,19 +232,19 @@ def _populate_indexes_and_jacobian(cls, matched_samples: list[LhCfPoseSample], d jac_sparsity) @classmethod - def _populate_initial_guess(cls, initial_guess_bs_poses: list[Pose], matched_samples: list[LhCfPoseSample], + def _populate_initial_guess(cls, initial_guess: LhBsCfPoses, defs: LighthouseGeometrySolution) -> tuple[npt.NDArray, npt.NDArray]: """ Generate parameters for base stations and CFs, this is the initial guess we start to iterate from. """ params_bs = np.zeros((defs.n_bss, defs.n_params_per_bs)) - for bs_id, pose in initial_guess_bs_poses.items(): + for bs_id, pose in initial_guess.bs_poses.items(): params_bs[defs.bs_id_to_index[bs_id], :] = cls._pose_to_params(pose) # Skip the first CF pose, it is the definition of the origin and is not a parameter params_cfs = np.zeros((defs.n_cfs_in_params, defs.n_params_per_cf)) - for index, sample in enumerate(matched_samples[1:]): - params_cfs[index, :] = cls._pose_to_params(sample.inital_est_pose) + for index, inital_est_pose in enumerate(initial_guess.cf_poses[1:]): + params_cfs[index, :] = cls._pose_to_params(inital_est_pose) return params_bs, params_cfs @@ -373,35 +379,36 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, matched_samples: list[LhCfPoseSample]) -> None: bss, cf_poses = cls._params_to_struct(lsq_result.x, solution) - # Update matched samples with estimated CF poses - matched_samples[0].estimated_pose = Pose() + # Extract CF pose estimates + solution.cf_poses.append(Pose()) for i, sample in enumerate(matched_samples[1:]): - sample.estimated_pose = cls._params_to_pose(cf_poses[i], solution) + solution.cf_poses.append(cls._params_to_pose(cf_poses[i], solution)) # Extract base station pose estimates - solution.bs_poses = {} for index, pose in enumerate(bss): bs_id = solution.bs_index_to_id[index] solution.bs_poses[bs_id] = cls._params_to_pose(pose, solution) solution.max_iter_reached = lsq_result == 0 - # Extract the final error for each sample. Use the approximate distance from the first sensor to the ray + # Extract the error for each CF pose residuals = lsq_result.fun i = 0 for sample in matched_samples: + sample_errors = {} for bs_id in sorted(sample.angles_calibrated.keys()): - sample.estimated_errors[bs_id] = np.linalg.norm(residuals[i:i + 2]) + sample_errors[bs_id] = np.linalg.norm(residuals[i:i + 2]) i += solution.n_sensors * 2 + solution.estimated_errors.append(sample_errors) - solution.error_info = cls._aggregate_error_info(matched_samples) + solution.error_info = cls._aggregate_error_info(solution.estimated_errors) @classmethod - def _aggregate_error_info(cls, matched_samples: list[LhCfPoseSample]): + def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]): error_per_bs = {} errors = [] - for sample in matched_samples: - for bs_id, error in sample.estimated_errors.items(): + for sample_errors in estimated_errors: + for bs_id, error in sample_errors.items(): if bs_id not in error_per_bs: error_per_bs[bs_id] = [] error_per_bs[bs_id].append(error) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 96d72cf06..043b91923 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -23,6 +23,7 @@ import numpy.typing as npt from .ippe_cf import IppeCf +from cflib.localization.lighthouse_types import LhBsCfPoses from cflib.localization.lighthouse_types import LhCfPoseSample from cflib.localization.lighthouse_types import Pose @@ -35,31 +36,36 @@ class LighthouseInitialEstimator: """ @classmethod - def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> dict[int, Pose]: + def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> LhBsCfPoses: """ - Make a rough estimate of the poses of all base stations found in the samples. + Make a rough estimate of the poses of all base stations and CF poses found in the samples. The pose of the Crazyflie in the first sample is used as a reference and will define the global reference frame. - :param matched_samples: A list of samples with lihghthouse angles. Note: matched_samples is augmented with - more information during the process, including the estimated poses + :param matched_samples: A list of samples with lihghthouse angles. :param sensor_positions: An array with the sensor positions on the lighthouse deck (3D, CF ref frame) + :return: a """ - cls._angles_to_poses(matched_samples, sensor_positions) + bs_poses_ref_cfs = cls._angles_to_poses(matched_samples, sensor_positions) + # Use the first CF pose as the global reference frame + bs_poses: dict[int, Pose] = bs_poses_ref_cfs[0] # TODO Do not use first sample as reference, pass it in as a parameter - bs_poses: dict[int, Pose] = {} - cls._get_reference_bs_poses(matched_samples[0], bs_poses) - cls._calc_remaining_bs_poses(matched_samples, bs_poses) - cls._calc_cf_poses(matched_samples, bs_poses) + cls._calc_remaining_bs_poses(bs_poses_ref_cfs, bs_poses) + cf_poses = cls._calc_cf_poses(bs_poses_ref_cfs, bs_poses) - return bs_poses + return LhBsCfPoses(bs_poses, cf_poses) @classmethod - def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> None: + def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike + ) -> list[dict[int, Pose]]: + """ + Estimate the base station poses in the Crazyflie reference frames, for each sample. + """ + result: list[dict[int, Pose]] = [] for sample in matched_samples: poses: dict[int, Pose] = {} for bs, angles in sample.angles_calibrated.items(): @@ -71,24 +77,15 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position t_vec = np.dot(R_mat, -estimate[0][1]) poses[bs] = Pose(R_mat, t_vec) - sample.initial_est_bs_poses = poses - - @classmethod - def _get_reference_bs_poses(cls, sample: LhCfPoseSample, bs_poses: dict[int, Pose]) -> None: - """ - The Pose of the CF in this sample is defining the global ref frame. - Store the poses for the bases stations that are in the sample. - """ - est_ref_cf = sample.initial_est_bs_poses - for bs, pose in est_ref_cf.items(): - bs_poses[bs] = pose + result.append(poses) + return result @classmethod - def _calc_remaining_bs_poses(cls, matched_sampels: list[LhCfPoseSample], bs_poses: dict[int, Pose]) -> None: + def _calc_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: dict[int, Pose]) -> None: # Find all base stations in the list all_bs = set() - for sample in matched_sampels: - all_bs.update(sample.initial_est_bs_poses.keys()) + for initial_est_bs_poses in bs_poses_ref_cfs: + all_bs.update(initial_est_bs_poses.keys()) # Remove the reference base stations that we already have the poses for to_find = all_bs - bs_poses.keys() @@ -96,8 +93,8 @@ def _calc_remaining_bs_poses(cls, matched_sampels: list[LhCfPoseSample], bs_pose # run through the list of samples until we manage to find them all remaining = len(to_find) while remaining > 0: - for sample in matched_sampels: - bs_poses_in_sample = sample.initial_est_bs_poses + for initial_est_bs_poses in bs_poses_ref_cfs: + bs_poses_in_sample = initial_est_bs_poses unknown = to_find.intersection(bs_poses_in_sample.keys()) known = set(bs_poses.keys()).intersection(bs_poses_in_sample.keys()) @@ -126,16 +123,21 @@ def _calc_remaining_bs_poses(cls, matched_sampels: list[LhCfPoseSample], bs_pose remaining = len(to_find) @classmethod - def _calc_cf_poses(cls, matched_samples: list[LhCfPoseSample], bs_poses: list[Pose]) -> None: - for sample in matched_samples: + def _calc_cf_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: list[Pose]) -> list[Pose]: + cf_poses: list[Pose] = [] + + for initial_est_bs_poses in bs_poses_ref_cfs: # Use the first base station pose as a reference - est_ref_cf = sample.initial_est_bs_poses + est_ref_cf = initial_est_bs_poses ref_bs = list(est_ref_cf.keys())[0] pose_global = bs_poses[ref_bs] pose_cf = est_ref_cf[ref_bs] est_ref_global = cls._map_cf_pos_to_cf_pos(pose_global, pose_cf) - sample.inital_est_pose = est_ref_global + + cf_poses.append(est_ref_global) + + return cf_poses @classmethod def _map_pose_to_ref_frame(cls, pose1_ref1: Pose, pose1_ref2: Pose, pose2_ref2: Pose) -> Pose: diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index 16cb1bb48..8f258d04e 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -100,6 +100,12 @@ class LhMeasurement(NamedTuple): angles: LighthouseBsVectors +class LhBsCfPoses(NamedTuple): + """Represents all poses of base stations and CF samples""" + bs_poses: dict[int, Pose] + cf_poses: list[Pose] + + class LhCfPoseSample: """ Represents a sample of a Crazyflie pose in space, it contains various data related to the pose such as: @@ -118,18 +124,6 @@ def __init__(self, timestamp: float = 0.0, angles_calibrated: dict[int, Lighthou if self.angles_calibrated is None: self.angles_calibrated = {} - # Initial estimates of bs poses for this sample, in the CF reference frame - self.initial_est_bs_poses: dict[int, Pose] = {} - - # The initial estimate of the CF pose for this sample, in the global ref frame - self.inital_est_pose: Pose = None - - # The refined estimate of the CF pose for this sample, in the global ref frame - self.estimated_pose: Pose = None - - # The aprroximate errors in final solution - self.estimated_errors: dict[int, float] = {} - class LhDeck4SensorPositions: """ Positions of the sensors on the Lighthouse 4 deck """ diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py index 384648741..f54c4caa2 100644 --- a/test/localization/test_lighthouse_geometry_solver.py +++ b/test/localization/test_lighthouse_geometry_solver.py @@ -42,11 +42,11 @@ def test_that_one_bs_poses_in_one_sample_is_estimated(self): }), ] - initial_guess_bs_poses = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) # Test actual = LighthouseGeometrySolver.solve( - initial_guess_bs_poses, matched_samples, LhDeck4SensorPositions.positions) + initial_guess, matched_samples, LhDeck4SensorPositions.positions) # Assert bs_poses = actual.bs_poses @@ -64,11 +64,11 @@ def test_that_two_bs_poses_in_one_sample_are_estimated(self): }), ] - initial_guess_bs_poses = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) # Test actual = LighthouseGeometrySolver.solve( - initial_guess_bs_poses, matched_samples, LhDeck4SensorPositions.positions) + initial_guess, matched_samples, LhDeck4SensorPositions.positions) # Assert bs_poses = actual.bs_poses @@ -97,11 +97,11 @@ def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self): }), ] - initial_guess_bs_poses = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) # Test actual = LighthouseGeometrySolver.solve( - initial_guess_bs_poses, matched_samples, LhDeck4SensorPositions.positions) + initial_guess, matched_samples, LhDeck4SensorPositions.positions) # Assert bs_poses = actual.bs_poses diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py index 085f26b0a..3a91e29a7 100644 --- a/test/localization/test_lighthouse_initial_estimator.py +++ b/test/localization/test_lighthouse_initial_estimator.py @@ -46,7 +46,7 @@ def test_that_one_bs_pose_is_found(self): actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) # Assert - self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual[bs_id], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id], places=3) def test_that_two_bs_poses_in_same_sample_are_found(self): # Fixture @@ -64,15 +64,46 @@ def test_that_two_bs_poses_in_same_sample_are_found(self): actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) # Assert - self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual[bs_id0], places=3) - self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual[bs_id1], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual.bs_poses[bs_id1], places=3) def test_that_linked_bs_poses_in_multiple_samples_are_found(self): # Fixture # CF_ORIGIN is used in the first sample and will define the global reference frame - bs_id0 = 3 + bs_id0 = 7 bs_id1 = 1 - bs_id2 = 2 + bs_id2 = 5 + bs_id3 = 0 + samples = [ + LhCfPoseSample(angles_calibrated={ + bs_id0: self.fixtures.angles_cf_origin_bs0, + bs_id1: self.fixtures.angles_cf_origin_bs1, + }), + LhCfPoseSample(angles_calibrated={ + bs_id1: self.fixtures.angles_cf1_bs1, + bs_id2: self.fixtures.angles_cf1_bs2, + }), + LhCfPoseSample(angles_calibrated={ + bs_id2: self.fixtures.angles_cf2_bs2, + bs_id3: self.fixtures.angles_cf2_bs3, + }), + ] + + # Test + actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + + # Assert + self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual.bs_poses[bs_id1], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, actual.bs_poses[bs_id2], places=3) + self.assertPosesAlmostEqual(self.fixtures.BS3_POSE, actual.bs_poses[bs_id3], places=3) + + def test_that_cf_poses_are_estimated(self): + # Fixture + # CF_ORIGIN is used in the first sample and will define the global reference frame + bs_id0 = 7 + bs_id1 = 1 + bs_id2 = 5 bs_id3 = 0 samples = [ LhCfPoseSample(angles_calibrated={ @@ -93,10 +124,9 @@ def test_that_linked_bs_poses_in_multiple_samples_are_found(self): actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) # Assert - self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual[bs_id0], places=3) - self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual[bs_id1], places=3) - self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, actual[bs_id2], places=3) - self.assertPosesAlmostEqual(self.fixtures.BS3_POSE, actual[bs_id3], places=3) + self.assertPosesAlmostEqual(self.fixtures.CF_ORIGIN_POSE, actual.cf_poses[0], places=3) + self.assertPosesAlmostEqual(self.fixtures.CF1_POSE, actual.cf_poses[1], places=3) + self.assertPosesAlmostEqual(self.fixtures.CF2_POSE, actual.cf_poses[2], places=3) def test_that_the_global_ref_frame_is_used(self): # Fixture @@ -120,11 +150,11 @@ def test_that_the_global_ref_frame_is_used(self): # Assert self.assertPosesAlmostEqual( - Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(1.0, 3.0, 3.0)), actual[bs_id0], places=3) + Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(1.0, 3.0, 3.0)), actual.bs_poses[bs_id0], places=3) self.assertPosesAlmostEqual( - Pose.from_rot_vec(R_vec=(0.0, 0.0, 0.0), t_vec=(-2.0, 1.0, 3.0)), actual[bs_id1], places=3) + Pose.from_rot_vec(R_vec=(0.0, 0.0, 0.0), t_vec=(-2.0, 1.0, 3.0)), actual.bs_poses[bs_id1], places=3) self.assertPosesAlmostEqual( - Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(2.0, 1.0, 3.0)), actual[bs_id2], places=3) + Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(2.0, 1.0, 3.0)), actual.bs_poses[bs_id2], places=3) def test_that_raises_for_isolated_bs(self): # Fixture From 08c7c8fe394ed3454a5d6b748bea018d45a20cb7 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 9 Jan 2022 19:44:09 +0100 Subject: [PATCH 326/861] Added rotation of pose to Pose class --- cflib/localization/lighthouse_types.py | 20 +++++++++++++ test/localization/lighthouse_test_base.py | 2 +- test/localization/test_lighthouse_types.py | 35 ++++++++++++++++++---- 3 files changed, 50 insertions(+), 7 deletions(-) diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index 8f258d04e..d1c8f0637 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -92,6 +92,26 @@ def inv_rotate_translate(self, point: npt.ArrayLike) -> npt.NDArray: """ return np.dot(np.transpose(self.rot_matrix), point - self.translation) + def rotate_translate_pose(self, pose: 'Pose') -> 'Pose': + """ + Rotate and translate a pose + """ + t = np.dot(self.rot_matrix, pose.translation) + self.translation + R = np.dot(self.rot_matrix, pose.rot_matrix) + + return Pose(R_matrix=R, t_vec=t) + + def inv_rotate_translate_pose(self, pose: 'Pose') -> 'Pose': + """ + Inverse rotate and translate a point, that is transform from global + to local reference frame + """ + inv_rot_matrix = np.transpose(self.rot_matrix) + t = np.dot(inv_rot_matrix, pose.translation - self.translation) + R = np.dot(inv_rot_matrix, pose.rot_matrix) + + return Pose(R_matrix=R, t_vec=t) + class LhMeasurement(NamedTuple): """Represents a measurement from one base station.""" diff --git a/test/localization/lighthouse_test_base.py b/test/localization/lighthouse_test_base.py index cdc966f4b..403bcb87a 100644 --- a/test/localization/lighthouse_test_base.py +++ b/test/localization/lighthouse_test_base.py @@ -48,7 +48,7 @@ def assertPosesAlmostEqual(self, expected: Pose, actual: Pose, places: int = 5): f'Translation different, expected: {expected.translation}, actual: {actual.translation}') def un_ambiguize(rot_vec): - quat = Rotation.from_rotvec(expected.rot_vec).as_quat() + quat = Rotation.from_rotvec(rot_vec).as_quat() return Rotation.from_quat(quat).as_rotvec() _expected_rot_vec = un_ambiguize(expected.rot_vec) diff --git a/test/localization/test_lighthouse_types.py b/test/localization/test_lighthouse_types.py index 36d64ed4d..dacc2e27b 100644 --- a/test/localization/test_lighthouse_types.py +++ b/test/localization/test_lighthouse_types.py @@ -19,14 +19,14 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import unittest +from test.localization.lighthouse_test_base import LighthouseTestBase import numpy as np from cflib.localization.lighthouse_types import Pose -class TestLighthouseTypes(unittest.TestCase): +class TestLighthouseTypes(LighthouseTestBase): def setUp(self): pass @@ -50,11 +50,11 @@ def test_default_rot_vec_constructor(self): def test_rotate_translate(self): # Fixture - pose = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2), t_vec=(1.0, 0.0, 0.0)) + transform = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2), t_vec=(1.0, 0.0, 0.0)) point = (2.0, 0.0, 0.0) # Test - actual = pose.rotate_translate(point) + actual = transform.rotate_translate(point) # Assert self.assertAlmostEqual(1.0, actual[0]) @@ -63,13 +63,36 @@ def test_rotate_translate(self): def test_rotate_translate_and_back(self): # Fixture - pose = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0), t_vec=(0.1, 0.2, 0.3)) + transform = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0), t_vec=(0.1, 0.2, 0.3)) expected = (2.0, 3.0, 4.0) # Test - actual = pose.inv_rotate_translate(pose.rotate_translate(expected)) + actual = transform.inv_rotate_translate(transform.rotate_translate(expected)) # Assert self.assertAlmostEqual(expected[0], actual[0]) self.assertAlmostEqual(expected[1], actual[1]) self.assertAlmostEqual(expected[2], actual[2]) + + def test_rotate_translate_pose(self): + # Fixture + transform = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2), t_vec=(1.0, 0.0, 0.0)) + pose = Pose(t_vec=(2.0, 0.0, 0.0)) + expected = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2), t_vec=(1.0, 2.0, 0.0)) + + # Test + actual = transform.rotate_translate_pose(pose) + + # Assert + self.assertPosesAlmostEqual(expected, actual) + + def test_rotate_translate_pose_and_back(self): + # Fixture + transform = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0), t_vec=(0.1, 0.2, 0.3)) + expected = Pose(t_vec=(2.0, 3.0, 4.0)) + + # Test + actual = transform.inv_rotate_translate_pose(transform.rotate_translate_pose(expected)) + + # Assert + self.assertPosesAlmostEqual(expected, actual) From a13212eb18b9c0e0bf972910374e06165c0cf820 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 9 Jan 2022 20:02:30 +0100 Subject: [PATCH 327/861] Added reference fram alignment --- .../lighthouse_sweep_angle_reader.py | 2 +- .../localization/lighthouse_system_aligner.py | 123 ++++++++++++++++++ examples/lighthouse/bs_geometry_estimation.py | 52 ++++++-- .../test_lighthouse_system_aligner.py | 91 +++++++++++++ 4 files changed, 257 insertions(+), 11 deletions(-) create mode 100644 cflib/localization/lighthouse_system_aligner.py create mode 100644 test/localization/test_lighthouse_system_aligner.py diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py index ae70de3fe..8c653c4a5 100644 --- a/cflib/localization/lighthouse_sweep_angle_reader.py +++ b/cflib/localization/lighthouse_sweep_angle_reader.py @@ -6,7 +6,7 @@ # | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2021 Bitcraze AB +# Copyright (C) 2021-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 diff --git a/cflib/localization/lighthouse_system_aligner.py b/cflib/localization/lighthouse_system_aligner.py new file mode 100644 index 000000000..0e79ad612 --- /dev/null +++ b/cflib/localization/lighthouse_system_aligner.py @@ -0,0 +1,123 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2021-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, 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 numpy as np +import numpy.typing as npt +import scipy.optimize +from cflib.localization.lighthouse_types import Pose + + +class LighthouseSystemAligner: + @classmethod + def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: list[npt.ArrayLike], + bs_poses: dict[int, Pose]) -> dict[int, Pose]: + """ + Align a coordinate system with the physical world. Finds the transform from the + current reference frame to one that is aligned with measured positions, and transforms base station + poses to the new coordinate system. + + :param origin: The position of the desired origin in the current reference frame + :param x_axis: One or more positions on the desired positive X-axis (X>0, Y=Z=0) in the current + reference frame + :param x_axis: One or more positions in the desired XY-plane (Z=0) in the current reference frame + :param bs_poses: a dictionary with the base station poses in the current reference frame + :return: a dictionary with the base station poses in the desired reference frame + """ + raw_transformation = cls.find_transformation(origin, x_axis, xy_plane) + transformation = cls._de_flip_transformation(raw_transformation, x_axis, bs_poses) + + result: dict[int, Pose] = {} + for bs_id, pose in bs_poses.items(): + result[bs_id] = transformation.rotate_translate_pose(pose) + + return result + + @classmethod + def find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: list[npt.ArrayLike]) -> Pose: + """ + Finds the transformation from the current reference frame to a desired reference frame based on measured positions + of the desired reference frame. + + :param origin: The position of the desired origin in the current reference frame + :param x_axis: One or more positions on the desired positive X-axis (X>0, Y=Z=0) in the current + reference frame + :param x_axis: One or more positions in the desired XY-plane (Z=0) in the current reference frame + :return: The transformation from the current reference frame to the desired reference frame. Note: the solution may + be flipped. + """ + args=(origin, x_axis, xy_plane) + + x0 = np.zeros((6)) + + result = scipy.optimize.least_squares(cls._calc_residual, + x0, verbose=0, + jac_sparsity=None, + x_scale='jac', + ftol=1e-8, + method='trf', + max_nfev=10, + args=args) + return cls._Pose_from_params(result.x) + + @classmethod + def _calc_residual(cls, params, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: list[npt.ArrayLike]): + transform = cls._Pose_from_params(params) + + origin_diff = transform.rotate_translate(origin) + x_axis_diff = map(lambda x: transform.rotate_translate(x), x_axis) + xy_plane_diff = map(lambda x: transform.rotate_translate(x), xy_plane) + + residual_origin = origin_diff + + # Points on X-axis: ignore X + x_axis_residual = list(map(lambda x: x[1:3], x_axis_diff)) + + # Points in the XY-plane: ignore X and Y + xy_plane_residual = list(map(lambda x: x[2], xy_plane_diff)) + + residual = np.concatenate((np.ravel(residual_origin), np.ravel(x_axis_residual), np.ravel(xy_plane_residual))) + return residual + + @classmethod + def _Pose_from_params(cls, params: npt.ArrayLike) -> Pose: + return Pose.from_rot_vec(R_vec=params[:3], t_vec=params[3:]) + + @classmethod + def _de_flip_transformation(cls, raw_transformation: Pose, x_axis: list[npt.ArrayLike], bs_poses: dict[int, Pose]) -> Pose: + """ + Investigats a transformation and flips it if needed. This method assumes that + 1. all base stations are at Z>0 + 2. x_axis samples are taken at x>0 + """ + transformation = raw_transformation + + # X-axis poses should be on the positivie X-axis, check the first one + if raw_transformation.rotate_translate(x_axis[0])[0] < 0.0: + flip_around_z_axis = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi)) + transformation = flip_around_z_axis.rotate_translate_pose(transformation) + + # Base station poses should be above the floor, check the first one + bs_pose = list(bs_poses.values())[0] + if raw_transformation.rotate_translate(bs_pose.translation)[2] < 0.0: + flip_around_x_axis = Pose.from_rot_vec(R_vec=(np.pi, 0.0, 0.0)) + transformation = flip_around_x_axis.rotate_translate_pose(transformation) + + return transformation diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index 923b9fb7d..c5d09509a 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -53,6 +53,7 @@ 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_types import LhCfPoseSample from cflib.localization.lighthouse_types import LhDeck4SensorPositions from cflib.localization.lighthouse_types import LhMeasurement @@ -103,20 +104,29 @@ def parse(recording_time, default): return default -def estimate_geometry(origin, samples): - matched_samples = [origin] + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2) +def estimate_geometry(origin, x_axis, xy_plane, samples): + matched_samples = [origin] + x_axis + xy_plane + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2) initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) + 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)) + bs_aligned_poses = LighthouseSystemAligner.align(origin_pos, x_axis_pos, xy_plane_pos, solution.bs_poses) + print(' Base stations at:') - for bs_id, pose in sorted(solution.bs_poses.items()): + for bs_id, pose in sorted(bs_aligned_poses.items()): pos = pose.translation print(f' {bs_id}: ({pos[0]}, {pos[1]}, {pos[2]})') print(' Solution match per base station:') for bs_id, value in solution.error_info['bs'].items(): print(f' {bs_id}: {value}') - return solution.bs_poses + return bs_aligned_poses def upload_geometry(scf, bs_poses): @@ -152,14 +162,36 @@ def data_written(success): print(f'Step 1. Connecting to the Crazyflie on uri {uri}...') with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: print(' Connected') - print('Step 2. Put the Crazyflie where you want the origin of your coordinate system, ' + - 'oriented with forward in the positive X direction.') + 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.') input('Press return when ready. ') print(' Recording...') origin = record_angles_average(scf) print(' Position recorded') - print('Step 3. We will now record data from the space you plan to fly in and optimize the base station ' + + print('Step 3. Put the Crazyflie somehere on the positive X-axis.') + print('Multiple samples can be recorded if you want to, type "r" before you hit enter to repeat the step.') + x_axis = [] + do_repeat = True + while do_repeat: + do_repeat = 'r' == input('Press return when ready. ').lower() + print(' Recording...') + x_axis.append(record_angles_average(scf)) + print(' Position recorded') + + print('Step 4. Put the Crazyflie somehere in the XY-plane, but not on the X-axis.') + print('Multiple samples can be recorded if you want to, type "r" before you hit enter to repeat the step.') + xy_plane = [] + do_repeat = True + while do_repeat: + do_repeat = 'r' == input('Press return when ready. ').lower() + print(' Recording...') + xy_plane.append(record_angles_average(scf)) + print(' Position recorded') + + 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. ') print('This step does not add anything in a system with only one base station, enter 0 in this case.') @@ -171,11 +203,11 @@ def data_written(success): samples = record_angles_sequence(scf, recording_time_s) print(' Recording ended') - print('Step 4. Estimating geometry...') - bs_poses = estimate_geometry(origin, samples) + print('Step 6. Estimating geometry...') + bs_poses = estimate_geometry(origin, x_axis, xy_plane, samples) print(' Geometry estimated') - print('Step 5. Upload geometry to the Crazyflie') + print('Step 7. Upload geometry to the Crazyflie') input('Press enter to upload geometry. ') upload_geometry(scf, bs_poses) print('Geometry uploaded') diff --git a/test/localization/test_lighthouse_system_aligner.py b/test/localization/test_lighthouse_system_aligner.py new file mode 100644 index 000000000..c1175d843 --- /dev/null +++ b/test/localization/test_lighthouse_system_aligner.py @@ -0,0 +1,91 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 numpy as np +from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner +from cflib.localization.lighthouse_types import Pose +from test.localization.lighthouse_test_base import LighthouseTestBase + + +class TestLighthouseSystemAligner(LighthouseTestBase): + def setUp(self): + pass + + def test_that_transformation_is_found_for_single_points(self): + # Fixture + origin = (1.0, 0.0, 0.0) + x_axis = [(1.0, 1.0, 0.0)] + xy_plane = [(2.0, 1.0, 0.0)] + + # Test + actual = LighthouseSystemAligner.find_transformation(origin, x_axis, xy_plane) + + # Assert + self.assertVectorsAlmostEqual((0.0, 0.0, 0.0), actual.rotate_translate((1.0, 0.0, 0.0))) + self.assertVectorsAlmostEqual((1.0, 0.0, 0.0), actual.rotate_translate((1.0, 1.0, 0.0))) + self.assertVectorsAlmostEqual((0.0, 1.0, 0.0), actual.rotate_translate((0.0, 0.0, 0.0))) + self.assertVectorsAlmostEqual((0.0, 0.0, 1.0), actual.rotate_translate((1.0, 0.0, 1.0))) + + def test_that_transformation_is_found_for_multiple_points(self): + # Fixture + origin = (1.0, 0.0, 0.0) + x_axis = [(1.0, 1.0, 0.0), (1.0, 4.0, 0.0)] + xy_plane = [(2.0, 1.0, 0.0), (3.0, -1.0, 0.0), (5.0, 0.0, 0.0)] + + # Test + actual = LighthouseSystemAligner.find_transformation(origin, x_axis, xy_plane) + + # Assert + self.assertVectorsAlmostEqual((0.0, 0.0, 0.0), actual.rotate_translate((1.0, 0.0, 0.0))) + self.assertVectorsAlmostEqual((1.0, 0.0, 0.0), actual.rotate_translate((1.0, 1.0, 0.0))) + self.assertVectorsAlmostEqual((0.0, 1.0, 0.0), actual.rotate_translate((0.0, 0.0, 0.0))) + self.assertVectorsAlmostEqual((0.0, 0.0, 1.0), actual.rotate_translate((1.0, 0.0, 1.0))) + + def test_that_base_stations_are_rotated(self): + # Fixture + origin = (1.0, 0.0, 0.0) + x_axis = [(1.0, 1.0, 0.0)] + xy_plane = [(2.0, 1.0, 0.0)] + + bs_id = 7 + bs_poses = {bs_id: Pose.from_rot_vec(t_vec=(1.0, 0.0, 1.0))} + + # Test + actual = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) + + # Assert + self.assertPosesAlmostEqual(Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(0.0, 0.0, 1.0)), actual[bs_id]) + + def test_that_solution_is_de_flipped(self): + # Fixture + origin = (0.0, 0.0, 0.0) + x_axis = [(-1.0, 0.0, 0.0)] + xy_plane = [(2.0, 1.0, 0.0)] + + bs_id = 7 + bs_poses = {bs_id: Pose.from_rot_vec(t_vec=(0.0, 0.0, 1.0))} + expected = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(0.0, 0.0, 1.0)) + + # Test + actual = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) + + # Assert + self.assertPosesAlmostEqual(expected, actual[bs_id]) From a39e7c9605cf3a422f971439d34f1b63e67eb3f0 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 9 Jan 2022 20:05:27 +0100 Subject: [PATCH 328/861] Added dependency to scipy --- setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/setup.py b/setup.py index 58183b61b..dab3d77bf 100644 --- a/setup.py +++ b/setup.py @@ -34,6 +34,7 @@ install_requires=[ 'pyusb>=1.0.0b2', + 'scipy>=1.7', ], # $ pip install -e .[dev] From 663cc4fe4e3b82ff8fff9c516f6b481291dc5b3c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 10 Jan 2022 18:12:32 +0100 Subject: [PATCH 329/861] Corrections to work with python 3.7 --- cflib/localization/ippe_cf.py | 2 ++ cflib/localization/lighthouse_bs_geo.py | 2 ++ cflib/localization/lighthouse_bs_vector.py | 4 ++- .../lighthouse_geometry_solver.py | 29 ++++++++++++++++--- .../lighthouse_initial_estimator.py | 2 ++ .../localization/lighthouse_sample_matcher.py | 2 ++ .../localization/lighthouse_system_aligner.py | 5 +++- cflib/localization/lighthouse_types.py | 2 ++ 8 files changed, 42 insertions(+), 6 deletions(-) diff --git a/cflib/localization/ippe_cf.py b/cflib/localization/ippe_cf.py index 60221a2a4..6110d6550 100644 --- a/cflib/localization/ippe_cf.py +++ b/cflib/localization/ippe_cf.py @@ -19,6 +19,8 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +from __future__ import annotations + from collections import namedtuple import numpy as np diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index db63971a6..f5a24e5c8 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -22,6 +22,8 @@ """ Functionality to handle base station geometry in the lighthouse poistioning system """ +from __future__ import annotations + from cflib.localization.lighthouse_bs_vector import LighthouseBsVector from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py index ee14d9de5..0b18db4f0 100644 --- a/cflib/localization/lighthouse_bs_vector.py +++ b/cflib/localization/lighthouse_bs_vector.py @@ -19,6 +19,8 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +from __future__ import annotations + import math import numpy as np @@ -140,7 +142,7 @@ def _q(self): # each sensor # LighthouseBsVectors = list[LighthouseBsVector] -class LighthouseBsVectors(list[LighthouseBsVector]): +class LighthouseBsVectors(list): def projection_pair_list(self) -> npt.NDArray: """ Genereate a list of projection pairs for all vectors diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 317180a54..84cd1664e 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -1,3 +1,26 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +from __future__ import annotations + import numpy as np import numpy.typing as npt import scipy.optimize @@ -120,10 +143,8 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample] Iteration is terminated after a fixed number of iteration if acceptable solution is found. - :param initial_guess_bs_poses: Initial guess for the base station and CF sample poses - :param matched_samples: List of matched samples, must have been augmented with initial guesses of the CF - poses. The samples will be updated with their estimated pose and error during the - process. + :param initial_guess: Initial guess for the base stations and CF sample poses + :param matched_samples: List of matched samples. :param sensor_positions: Sensor positions (3D), in the CF reference frame :return: an instance of LighthouseGeometrySolution """ diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 043b91923..589a927da 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -19,6 +19,8 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +from __future__ import annotations + import numpy as np import numpy.typing as npt diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py index bac20ef62..8b7bee560 100644 --- a/cflib/localization/lighthouse_sample_matcher.py +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -19,6 +19,8 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +from __future__ import annotations + from cflib.localization.lighthouse_types import LhCfPoseSample from cflib.localization.lighthouse_types import LhMeasurement diff --git a/cflib/localization/lighthouse_system_aligner.py b/cflib/localization/lighthouse_system_aligner.py index 0e79ad612..91aaf04eb 100644 --- a/cflib/localization/lighthouse_system_aligner.py +++ b/cflib/localization/lighthouse_system_aligner.py @@ -19,9 +19,12 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +from __future__ import annotations + import numpy as np import numpy.typing as npt import scipy.optimize + from cflib.localization.lighthouse_types import Pose @@ -63,7 +66,7 @@ def find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], :return: The transformation from the current reference frame to the desired reference frame. Note: the solution may be flipped. """ - args=(origin, x_axis, xy_plane) + args = (origin, x_axis, xy_plane) x0 = np.zeros((6)) diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index d1c8f0637..b5ec105bc 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -19,6 +19,8 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +from __future__ import annotations + from typing import NamedTuple import numpy as np From 7389674e9f58bb33365977e28d95d9b831e6bd74 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 11 Jan 2022 11:03:09 +0100 Subject: [PATCH 330/861] Add Link-type to PCAP header So that we could support USB or WIFI in future. --- cflib/crtp/pcap.py | 18 +++++--- cflib/drivers/crazyradio.py | 2 +- wireshark/crtp-dissector.lua | 80 ++++++++++++++++++------------------ 3 files changed, 53 insertions(+), 47 deletions(-) diff --git a/cflib/crtp/pcap.py b/cflib/crtp/pcap.py index 866195b8c..86b571bf3 100644 --- a/cflib/crtp/pcap.py +++ b/cflib/crtp/pcap.py @@ -1,6 +1,7 @@ import os import struct from datetime import datetime +from enum import IntEnum class PCAPLog(): @@ -91,6 +92,11 @@ class PCAPLog(): was captured. If incl_len and orig_len differ, the actually saved packet size was limited by snaplen. """ + # Link type options for CRTP packet + class LinkType(IntEnum): + RADIO = 1 + USB = 2 + # Global header for pcap 2.4 pcap_global_header = ('D4 C3 B2 A1 ' '02 00 ' # major revision (i.e. pcap <2>.4) @@ -121,20 +127,20 @@ def instance(cls): return cls._instance - def logCRTP(self, receive, devid, address, channel, crtp_packet): - length = len(address) + 1 + 1 + 1 + len(crtp_packet) # addr + devid + receive + channel + data + def logCRTP(self, link_type: LinkType, receive, devid, address, channel, crtp_packet): + length = len(address) + 1 + 1 + 1 + 1 + len(crtp_packet) # type + addr + devid + receive + channel + data self._log.write(self._pcap_header(length)) self._log.write( self._assemble_record( - receive, address, channel, devid, crtp_packet + int(link_type), receive, address, channel, devid, crtp_packet ) ) - def _assemble_record(self, receive, address, channel, devid, crtp_packet): + def _assemble_record(self, link_type, receive, address, channel, devid, crtp_packet): return struct.pack( - ' 0: logger = PCAPLog.instance() - logger.logCRTP(receive, devid, address, channel, packet) + logger.logCRTP(logger.LinkType.RADIO, receive, devid, address, channel, packet) except KeyError: pass diff --git a/wireshark/crtp-dissector.lua b/wireshark/crtp-dissector.lua index 34bebd622..30202d546 100644 --- a/wireshark/crtp-dissector.lua +++ b/wireshark/crtp-dissector.lua @@ -116,8 +116,8 @@ function get_crtp_port_channel_names(port, channel) end function format_address(buffer) - addr = buffer(1, 5):bytes():tohex() - port = buffer(6, 1):uint() + addr = buffer(2, 5):bytes():tohex() + port = buffer(7, 1):uint() addr = addr:gsub("..", ":%0"):sub(2) port = tostring(port) @@ -126,7 +126,7 @@ function format_address(buffer) end function format_radio(buffer) - devid = buffer(7, 1):uint() + devid = buffer(8, 1):uint() return "Radio #" .. tostring(devid) end @@ -160,7 +160,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) local port_tree = tree:add(crtp, port_name) - local cmd = buffer(9, 1):uint() + local cmd = buffer(10, 1):uint() local cmd_str = "Unknown" if cmd == Commands.COMMAND_SET_GROUP_MASK then @@ -170,7 +170,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- uint8_t groupMask; // mask for which CFs this should apply to -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(10, 1):uint() + group_mask = buffer(11, 1):uint() end elseif cmd == Commands.COMMAND_TAKEOFF then cmd_str = "Take Off (deprecated)" elseif cmd == Commands.COMMAND_LAND then cmd_str = "Land (deprecated)" @@ -181,7 +181,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- uint8_t groupMask; // mask for which CFs this should apply to -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(10, 1):uint() + group_mask = buffer(11, 1):uint() end elseif cmd == Commands.COMMAND_GO_TO then cmd_str = "Go To" @@ -196,13 +196,13 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float duration; // sec -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(10, 1):uint() - relative = buffer(11, 1):uint() - x = buffer(12, 4):le_float() - y = buffer(16, 4):le_float() - z = buffer(20, 4):le_float() - yaw = buffer(24, 4):le_float() - duration = buffer(28, 4):le_float() + group_mask = buffer(11, 1):uint() + relative = buffer(12, 1):uint() + x = buffer(13, 4):le_float() + y = buffer(17, 4):le_float() + z = buffer(21, 4):le_float() + yaw = buffer(25, 4):le_float() + duration = buffer(29, 4):le_float() end elseif cmd == Commands.COMMAND_START_TRAJECTORY then cmd_str = "Start Trajectory" @@ -215,11 +215,11 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float timescale; // time factor; 1 = original speed; >1: slower; <1: faster -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(10, 1):uint() - relative = buffer(11, 1):uint() - reversed = buffer(12, 1):uint() - id = buffer(13, 1):uint() - timescale = buffer(14):float() + group_mask = buffer(11, 1):uint() + relative = buffer(12, 1):uint() + reversed = buffer(13, 1):uint() + id = buffer(14, 1):uint() + timescale = buffer(15):float() end elseif cmd == Commands.COMMAND_DEFINE_TRAJECTORY then cmd_str = "Define Trajectory" @@ -229,7 +229,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- struct trajectoryDescription description; -- } __attribute__((packed)); if receive == 0 then - id = buffer(10, 1):uint() + id = buffer(11, 1):uint() end elseif cmd == Commands.COMMAND_TAKEOFF_2 then cmd_str = "Take Off" @@ -242,11 +242,11 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float duration; // s (time it should take until target height is reached) -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(10, 1):uint() - height = buffer(11, 4):le_float() - yaw = buffer(15, 4):le_float() - use_yaw = buffer(19, 1):uint() - duration = buffer(20, 4):le_float() + group_mask = buffer(11, 1):uint() + height = buffer(12, 4):le_float() + yaw = buffer(16, 4):le_float() + use_yaw = buffer(20, 1):uint() + duration = buffer(21, 4):le_float() end elseif cmd == Commands.COMMAND_LAND_2 then @@ -259,11 +259,11 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float duration; // s (time it should take until target height is reached) -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(10, 1):uint() - height = buffer(11, 4):le_float() - yaw = buffer(15, 4):le_float() - use_yaw = buffer(19, 1):uint() - duration = buffer(20, 4):le_float() + group_mask = buffer(11, 1):uint() + height = buffer(12, 4):le_float() + yaw = buffer(16, 4):le_float() + use_yaw = buffer(20, 1):uint() + duration = buffer(21, 4):le_float() end elseif cmd == Commands.COMMAND_TAKEOFF_WITH_VELOCITY then cmd_str = "Take Off With Velocity" @@ -272,7 +272,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) port_tree:add_le(f_crtp_setpoint_hl_command, cmd_str) local success = true if receive == 1 then - retval = buffer(12):uint() + retval = buffer(13):uint() local success = (retval == 0) and "Success" or "Failure" port_tree:add_le(f_crtp_setpoint_hl_retval, retval):append_text(" (" .. success .. ")") end @@ -317,17 +317,17 @@ function handle_parameter_port(tree, buffer, channel, size) if (channel == 1 or channel == 2) and size > 2 then -- Add variable id - local var_id = buffer(9, 2):le_uint() + local var_id = buffer(10, 2):le_uint() local port_tree = tree:add(crtp, port_name) port_tree:add_le(f_crtp_parameter_varid, var_id) -- Add value if size > 3 then - port_tree:add_le(f_crtp_parameter_val_uint, buffer(12):le_uint()) - port_tree:add_le(f_crtp_parameter_val_int, buffer(12):le_int()) + port_tree:add_le(f_crtp_parameter_val_uint, buffer(13):le_uint()) + port_tree:add_le(f_crtp_parameter_val_int, buffer(13):le_int()) end if size >= 7 then - port_tree:add_le(f_crtp_parameter_val_float, buffer(12):le_float()) + port_tree:add_le(f_crtp_parameter_val_float, buffer(13):le_float()) end end end @@ -338,9 +338,9 @@ end function crtp.dissector(buffer, pinfo, tree) pinfo.cols.protocol = "CRTP" - if buffer:len() <= 8 then return end + if buffer:len() <= 9 then return end - local receive = buffer(0, 1):uint() + local receive = buffer(1, 1):uint() if receive == 1 then pinfo.cols.dst = format_radio(buffer) pinfo.cols.src = format_address(buffer) @@ -350,18 +350,18 @@ function crtp.dissector(buffer, pinfo, tree) end local subtree = tree:add(crtp, "CRTP Packet") - local header = bit.band(buffer(8, 1):uint(), 0xF3) + local header = bit.band(buffer(9, 1):uint(), 0xF3) local crtp_port = bit.rshift(bit.band(header, 0xF0), 4) local crtp_channel = bit.band(header, 0x03) -- Add CRTP packet size: -- receive_byte + address + channel + devid = 8 -- Rest is CRTP packet - local crtp_size = buffer:len() - 8 + local crtp_size = buffer:len() - 9 subtree:add_le(f_crtp_size, crtp_size) -- Check for safelink packet - if crtp_size == 3 and header == 0xF3 and buffer(9, 1):uint() == 0x05 then + if crtp_size == 3 and header == 0xF3 and buffer(10, 1):uint() == 0x05 then pinfo.cols.info = "SafeLink" return end @@ -385,7 +385,7 @@ function crtp.dissector(buffer, pinfo, tree) -- Console, we can add text if crtp_port == Ports.Console then local port_tree = tree:add(crtp, port_name) - port_tree:add_le(f_crtp_console_text, buffer(9):string()) + port_tree:add_le(f_crtp_console_text, buffer(10):string()) end if crtp_port == Ports.Parameters then From 4abb0ef690b3a42a69284e3c6c763b90f8eb88ea Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 11 Jan 2022 11:41:44 +0100 Subject: [PATCH 331/861] Move wireshark to doc/ folder --- wireshark/crtp-dissector.lua => crtp-dissector.lua | 0 wireshark/README.md => docs/wireshark/wireshark.md | 9 ++++++--- 2 files changed, 6 insertions(+), 3 deletions(-) rename wireshark/crtp-dissector.lua => crtp-dissector.lua (100%) rename wireshark/README.md => docs/wireshark/wireshark.md (92%) diff --git a/wireshark/crtp-dissector.lua b/crtp-dissector.lua similarity index 100% rename from wireshark/crtp-dissector.lua rename to crtp-dissector.lua diff --git a/wireshark/README.md b/docs/wireshark/wireshark.md similarity index 92% rename from wireshark/README.md rename to docs/wireshark/wireshark.md index 07bf20fb6..407f71c78 100644 --- a/wireshark/README.md +++ b/docs/wireshark/wireshark.md @@ -1,14 +1,17 @@ -# How to analyze CFLIB CRTP traffic using Wireshark +--- +title: How to analyze CFLIB CRTP traffic using Wireshark +page_id: wireshark_debugging +--- Wireshark is a free and open-source packet analyzer. It is used for network troubleshooting, analysis, software and communications protocol development, and education. It makes analyzing what is going on with packet based protocols easier. We have written a plugin to Wireshark that can display [CRTP](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/) packets (what this library uses to communicate with Crazyflies). -![Image of CRTP dissector in Wireshark](../docs/images/wireshark-dissector.png?raw=true "CRTP Wireshark Dissector") +![Image of CRTP dissector in Wireshark](../images/wireshark-dissector.png?raw=true "CRTP Wireshark Dissector") In order for Wireshark (and our dissector plugin) to be able to decode CRTP you need to do *two* things: -## 1. Install the plugin [crtp-dissector.lua](crtp-dissector.lua) in Wireshark +## 1. Install the plugin [crtp-dissector.lua](../../crtp-dissector.lua) in Wireshark Wireshark looks for plugins in both a personal plugin folder and a global plugin folder. Lua plugins are stored in the plugin folders; compiled plugins are stored in subfolders of the plugin folders, with the subfolder name being the Wireshark minor version number (X.Y). There is another hierarchical level for each Wireshark plugin type (libwireshark, libwiretap and codecs). So for example the location for a libwireshark plugin foo.so (foo.dll on Windows) would be PLUGINDIR/X.Y/epan (libwireshark used to be called libepan; the other folder names are codecs and wiretap). From 0636a826fcad5cf2ecdb124d28f389afb0af79ab Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 11 Jan 2022 15:23:56 +0100 Subject: [PATCH 332/861] Refactoring --- cflib/localization/lighthouse_geometry_solver.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 84cd1664e..b5937aba3 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -401,8 +401,9 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, bss, cf_poses = cls._params_to_struct(lsq_result.x, solution) # Extract CF pose estimates + # First pose (origin) is not in the parameter list solution.cf_poses.append(Pose()) - for i, sample in enumerate(matched_samples[1:]): + for i in range(len(matched_samples) - 1): solution.cf_poses.append(cls._params_to_pose(cf_poses[i], solution)) # Extract base station pose estimates From 8eb2a7ad6cc6ab840e4644d0a2135b5691a09b76 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 11 Jan 2022 15:26:36 +0100 Subject: [PATCH 333/861] Raise exception if solution does not converge --- cflib/localization/lighthouse_geometry_solver.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index b5937aba3..507e561e5 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -78,9 +78,6 @@ def __init__(self) -> None: # Information about errors in the solution self.error_info = {} - # True if the sover was terminated due to reaching the maximum nr of alowed iterations - self.max_iter_reached: bool = False - class LighthouseGeometrySolver: """ @@ -411,7 +408,8 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, bs_id = solution.bs_index_to_id[index] solution.bs_poses[bs_id] = cls._params_to_pose(pose, solution) - solution.max_iter_reached = lsq_result == 0 + if not lsq_result.success: + raise Exception('Solution did not converge') # Extract the error for each CF pose residuals = lsq_result.fun From 9e23f506c1a8266547c825710e38cda4efd40235 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 12 Jan 2022 11:20:31 +0100 Subject: [PATCH 334/861] wireshark: Support USB as well --- cflib/crtp/pcap.py | 13 ++--- cflib/drivers/cfusb.py | 27 ++++++++++ crtp-dissector.lua | 120 +++++++++++++++++++++++++---------------- 3 files changed, 104 insertions(+), 56 deletions(-) diff --git a/cflib/crtp/pcap.py b/cflib/crtp/pcap.py index 86b571bf3..bef44755f 100644 --- a/cflib/crtp/pcap.py +++ b/cflib/crtp/pcap.py @@ -128,18 +128,13 @@ def instance(cls): return cls._instance def logCRTP(self, link_type: LinkType, receive, devid, address, channel, crtp_packet): - length = len(address) + 1 + 1 + 1 + 1 + len(crtp_packet) # type + addr + devid + receive + channel + data - - self._log.write(self._pcap_header(length)) - self._log.write( - self._assemble_record( - int(link_type), receive, address, channel, devid, crtp_packet - ) - ) + record = self._assemble_record(int(link_type), receive, address, channel, devid, crtp_packet) + self._log.write(self._pcap_header(len(record))) + self._log.write(record) def _assemble_record(self, link_type, receive, address, channel, devid, crtp_packet): return struct.pack( - ' 0: + logger = PCAPLog.instance() + logger.logCRTP( + logger.LinkType.USB, + receive, + id, + bytearray.fromhex(self.get_serial()), + 0, + packet + ) + except KeyError: + pass + # Data transfers def send_packet(self, dataOut): """ Send a packet and receive the ack from the radio dongle @@ -150,6 +172,9 @@ def send_packet(self, dataOut): self.handle.bulkWrite(1, dataOut, 20) else: self.handle.write(endpoint=1, data=dataOut, timeout=20) + + self._log_packet(False, self.dev.port_number, dataOut) + except usb.USBError: pass @@ -174,6 +199,8 @@ def receive_packet(self): # supported, but the "normal" case will work. pass + self._log_packet(True, self.dev.port_number, dataIn) + return dataIn diff --git a/crtp-dissector.lua b/crtp-dissector.lua index 30202d546..90bc8db2a 100644 --- a/crtp-dissector.lua +++ b/crtp-dissector.lua @@ -45,6 +45,15 @@ crtp.fields = { f_crtp_setpoint_hl_id, f_crtp_setpoint_hl_timescale } +local Links = { + UNKNOWN = 0, + RADIO = 1, + USB = 2, +} + +local link = Links.UNKNOWN + +local crtp_start = 0 -- Analye port and channel and figure what service (port name / channel name) -- we are dealing with @@ -61,6 +70,7 @@ local Ports = { Platform = 0xD, All = 0xF } + function get_crtp_port_channel_names(port, channel) local port_name = "Unknown" local channel_name = nil @@ -116,19 +126,28 @@ function get_crtp_port_channel_names(port, channel) end function format_address(buffer) - addr = buffer(2, 5):bytes():tohex() - port = buffer(7, 1):uint() + if link == Links.RADIO then + addr = buffer(2, 5):bytes():tohex() + port = buffer(7, 1):uint() - addr = addr:gsub("..", ":%0"):sub(2) - port = tostring(port) + addr = addr:gsub("..", ":%0"):sub(2) + port = tostring(port) - return addr .. " (" .. port .. ")" + return addr .. " (" .. port .. ")" + elseif link == Links.USB then + return buffer(2, 12):bytes():tohex() + end end -function format_radio(buffer) - devid = buffer(8, 1):uint() +function format_device(buffer) + if link == Links.RADIO then + devid = buffer(8, 1):uint() + return "Radio #" .. tostring(devid) + elseif link == Links.USB then + devid = buffer(15, 1):uint() + return "USB #" .. tostring(devid) + end - return "Radio #" .. tostring(devid) end function handle_setpoint_highlevel(tree, receive, buffer, channel, size) @@ -160,7 +179,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) local port_tree = tree:add(crtp, port_name) - local cmd = buffer(10, 1):uint() + local cmd = buffer(crtp_start + 1, 1):uint() local cmd_str = "Unknown" if cmd == Commands.COMMAND_SET_GROUP_MASK then @@ -181,7 +200,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- uint8_t groupMask; // mask for which CFs this should apply to -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(11, 1):uint() + group_mask = buffer(crtp_start + 2, 1):uint() end elseif cmd == Commands.COMMAND_GO_TO then cmd_str = "Go To" @@ -196,13 +215,13 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float duration; // sec -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(11, 1):uint() - relative = buffer(12, 1):uint() - x = buffer(13, 4):le_float() - y = buffer(17, 4):le_float() - z = buffer(21, 4):le_float() - yaw = buffer(25, 4):le_float() - duration = buffer(29, 4):le_float() + group_mask = buffer(crtp_start + 2, 1):uint() + relative = buffer(crtp_start + 3, 1):uint() + x = buffer(crtp_start + 4, 4):le_float() + y = buffer(crtp_start + 8, 4):le_float() + z = buffer(crtp_start + 12, 4):le_float() + yaw = buffer(crtp_start + 16, 4):le_float() + duration = buffer(crtp_start + 20, 4):le_float() end elseif cmd == Commands.COMMAND_START_TRAJECTORY then cmd_str = "Start Trajectory" @@ -215,11 +234,11 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float timescale; // time factor; 1 = original speed; >1: slower; <1: faster -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(11, 1):uint() - relative = buffer(12, 1):uint() - reversed = buffer(13, 1):uint() - id = buffer(14, 1):uint() - timescale = buffer(15):float() + group_mask = buffer(crtp_start + 2, 1):uint() + relative = buffer(crtp_start + 3, 1):uint() + reversed = buffer(crtp_start + 4, 1):uint() + id = buffer(crtp_start + 5, 1):uint() + timescale = buffer(crtp_start + 6):float() end elseif cmd == Commands.COMMAND_DEFINE_TRAJECTORY then cmd_str = "Define Trajectory" @@ -229,7 +248,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- struct trajectoryDescription description; -- } __attribute__((packed)); if receive == 0 then - id = buffer(11, 1):uint() + id = buffer(crtp_start + 2, 1):uint() end elseif cmd == Commands.COMMAND_TAKEOFF_2 then cmd_str = "Take Off" @@ -242,11 +261,11 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float duration; // s (time it should take until target height is reached) -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(11, 1):uint() - height = buffer(12, 4):le_float() - yaw = buffer(16, 4):le_float() - use_yaw = buffer(20, 1):uint() - duration = buffer(21, 4):le_float() + group_mask = buffer(crtp_start + 2, 1):uint() + height = buffer(crtp_start + 3, 4):le_float() + yaw = buffer(crtp_start + 7, 4):le_float() + use_yaw = buffer(crtp_start + 11, 1):uint() + duration = buffer(crtp_start + 12, 4):le_float() end elseif cmd == Commands.COMMAND_LAND_2 then @@ -259,11 +278,11 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- float duration; // s (time it should take until target height is reached) -- } __attribute__((packed)); if receive == 0 then - group_mask = buffer(11, 1):uint() - height = buffer(12, 4):le_float() - yaw = buffer(16, 4):le_float() - use_yaw = buffer(20, 1):uint() - duration = buffer(21, 4):le_float() + group_mask = buffer(crtp_start + 2, 1):uint() + height = buffer(crtp_start + 3, 4):le_float() + yaw = buffer(crtp_start + 7, 4):le_float() + use_yaw = buffer(crtp_start + 11, 1):uint() + duration = buffer(crtp_start + 12, 4):le_float() end elseif cmd == Commands.COMMAND_TAKEOFF_WITH_VELOCITY then cmd_str = "Take Off With Velocity" @@ -272,7 +291,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) port_tree:add_le(f_crtp_setpoint_hl_command, cmd_str) local success = true if receive == 1 then - retval = buffer(13):uint() + retval = buffer(crtp_start + 4):uint() local success = (retval == 0) and "Success" or "Failure" port_tree:add_le(f_crtp_setpoint_hl_retval, retval):append_text(" (" .. success .. ")") end @@ -317,51 +336,58 @@ function handle_parameter_port(tree, buffer, channel, size) if (channel == 1 or channel == 2) and size > 2 then -- Add variable id - local var_id = buffer(10, 2):le_uint() + local var_id = buffer(crtp_start + 1, 2):le_uint() local port_tree = tree:add(crtp, port_name) port_tree:add_le(f_crtp_parameter_varid, var_id) -- Add value if size > 3 then - port_tree:add_le(f_crtp_parameter_val_uint, buffer(13):le_uint()) - port_tree:add_le(f_crtp_parameter_val_int, buffer(13):le_int()) + port_tree:add_le(f_crtp_parameter_val_uint, buffer(crtp_start + 4):le_uint()) + port_tree:add_le(f_crtp_parameter_val_int, buffer(crtp_start + 4):le_int()) end if size >= 7 then - port_tree:add_le(f_crtp_parameter_val_float, buffer(13):le_float()) + port_tree:add_le(f_crtp_parameter_val_float, buffer(crtp_start + 4):le_float()) end end end -- create a function to dissect it, layout: --- | receive| address | channel | radio devid | crtp header | crtp data | --- | 1 byte | 5 bytes | 1 byte | 1 byte | 1 byte | n bytes | +-- | link_type | receive| address | channel | radio devid | crtp header | crtp data | +-- | 1 byte | 1 byte | 5 or 12 bytes | 1 byte | 1 byte | 1 byte | n bytes | function crtp.dissector(buffer, pinfo, tree) pinfo.cols.protocol = "CRTP" - if buffer:len() <= 9 then return end + link = buffer(0, 1):uint() + if link == Links.RADIO then + crtp_start = 9 + elseif link == Links.USB then + crtp_start = 16 + end + + if buffer:len() <= crtp_start then return end local receive = buffer(1, 1):uint() if receive == 1 then - pinfo.cols.dst = format_radio(buffer) + pinfo.cols.dst = format_device(buffer) pinfo.cols.src = format_address(buffer) else pinfo.cols.dst = format_address(buffer) - pinfo.cols.src = format_radio(buffer) + pinfo.cols.src = format_device(buffer) end local subtree = tree:add(crtp, "CRTP Packet") - local header = bit.band(buffer(9, 1):uint(), 0xF3) + local header = bit.band(buffer(crtp_start, 1):uint(), 0xF3) local crtp_port = bit.rshift(bit.band(header, 0xF0), 4) local crtp_channel = bit.band(header, 0x03) -- Add CRTP packet size: -- receive_byte + address + channel + devid = 8 -- Rest is CRTP packet - local crtp_size = buffer:len() - 9 + local crtp_size = buffer:len() - crtp_start subtree:add_le(f_crtp_size, crtp_size) -- Check for safelink packet - if crtp_size == 3 and header == 0xF3 and buffer(10, 1):uint() == 0x05 then + if crtp_size == 3 and header == 0xF3 and buffer(crtp_start + 1, 1):uint() == 0x05 then pinfo.cols.info = "SafeLink" return end @@ -385,7 +411,7 @@ function crtp.dissector(buffer, pinfo, tree) -- Console, we can add text if crtp_port == Ports.Console then local port_tree = tree:add(crtp, port_name) - port_tree:add_le(f_crtp_console_text, buffer(10):string()) + port_tree:add_le(f_crtp_console_text, buffer(crtp_start + 1):string()) end if crtp_port == Ports.Parameters then From dbe0b2c820e6d95c9d7b2c1205ebb29a367010c5 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 13 Jan 2022 08:56:06 +0100 Subject: [PATCH 335/861] Move wireshark dissector to tools/ --- docs/wireshark/wireshark.md | 2 +- crtp-dissector.lua => tools/crtp-dissector.lua | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename crtp-dissector.lua => tools/crtp-dissector.lua (100%) diff --git a/docs/wireshark/wireshark.md b/docs/wireshark/wireshark.md index 407f71c78..ba555aa79 100644 --- a/docs/wireshark/wireshark.md +++ b/docs/wireshark/wireshark.md @@ -11,7 +11,7 @@ We have written a plugin to Wireshark that can display [CRTP](https://www.bitcra In order for Wireshark (and our dissector plugin) to be able to decode CRTP you need to do *two* things: -## 1. Install the plugin [crtp-dissector.lua](../../crtp-dissector.lua) in Wireshark +## 1. Install the plugin [crtp-dissector.lua](../../tools/crtp-dissector.lua) in Wireshark Wireshark looks for plugins in both a personal plugin folder and a global plugin folder. Lua plugins are stored in the plugin folders; compiled plugins are stored in subfolders of the plugin folders, with the subfolder name being the Wireshark minor version number (X.Y). There is another hierarchical level for each Wireshark plugin type (libwireshark, libwiretap and codecs). So for example the location for a libwireshark plugin foo.so (foo.dll on Windows) would be PLUGINDIR/X.Y/epan (libwireshark used to be called libepan; the other folder names are codecs and wiretap). diff --git a/crtp-dissector.lua b/tools/crtp-dissector.lua similarity index 100% rename from crtp-dissector.lua rename to tools/crtp-dissector.lua From 54b48cf3ed50dc562abcb6c7c16368dadf14f2a5 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 13 Jan 2022 08:56:30 +0100 Subject: [PATCH 336/861] wireshark: Add undecoded field --- tools/crtp-dissector.lua | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/tools/crtp-dissector.lua b/tools/crtp-dissector.lua index 90bc8db2a..57e774db7 100644 --- a/tools/crtp-dissector.lua +++ b/tools/crtp-dissector.lua @@ -5,6 +5,7 @@ crtp = Proto("CrazyRealTimeTProtocol","Crazy Real Time Protocol") local f_crtp_port = ProtoField.uint8("crtp.port", "Port") local f_crtp_channel = ProtoField.uint8("crtp.channel", "Channel") local f_crtp_size = ProtoField.uint8("crtp.size", "Size") +local f_crtp_undecoded = ProtoField.string("crtp.undecoded", "Undecoded") -- Specialized CRTP service fields local f_crtp_console_text = ProtoField.string("crtp.console_text", "Text", base.ASCII) @@ -42,7 +43,7 @@ crtp.fields = { f_crtp_setpoint_hl_groupmask, f_crtp_setpoint_hl_duration, f_crtp_setpoint_hl_height, f_crtp_setpoint_hl_relative, f_crtp_setpoint_hl_x, f_crtp_setpoint_hl_y, f_crtp_setpoint_hl_z, - f_crtp_setpoint_hl_id, f_crtp_setpoint_hl_timescale + f_crtp_setpoint_hl_id, f_crtp_setpoint_hl_timescale, f_crtp_undecoded } local Links = { @@ -52,7 +53,7 @@ local Links = { } local link = Links.UNKNOWN - +local undecoded = 0 local crtp_start = 0 -- Analye port and channel and figure what service (port name / channel name) @@ -202,6 +203,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) if receive == 0 then group_mask = buffer(crtp_start + 2, 1):uint() end + undecoded = undecoded - 1 elseif cmd == Commands.COMMAND_GO_TO then cmd_str = "Go To" @@ -222,6 +224,8 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) z = buffer(crtp_start + 12, 4):le_float() yaw = buffer(crtp_start + 16, 4):le_float() duration = buffer(crtp_start + 20, 4):le_float() + undecoded = undecoded - 24 + end elseif cmd == Commands.COMMAND_START_TRAJECTORY then cmd_str = "Start Trajectory" @@ -239,6 +243,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) reversed = buffer(crtp_start + 4, 1):uint() id = buffer(crtp_start + 5, 1):uint() timescale = buffer(crtp_start + 6):float() + undecoded = undecoded - 10 end elseif cmd == Commands.COMMAND_DEFINE_TRAJECTORY then cmd_str = "Define Trajectory" @@ -249,6 +254,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) -- } __attribute__((packed)); if receive == 0 then id = buffer(crtp_start + 2, 1):uint() + undecoded = undecoded - 1 end elseif cmd == Commands.COMMAND_TAKEOFF_2 then cmd_str = "Take Off" @@ -266,6 +272,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) yaw = buffer(crtp_start + 7, 4):le_float() use_yaw = buffer(crtp_start + 11, 1):uint() duration = buffer(crtp_start + 12, 4):le_float() + undecoded = undecoded - 16 end elseif cmd == Commands.COMMAND_LAND_2 then @@ -283,6 +290,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) yaw = buffer(crtp_start + 7, 4):le_float() use_yaw = buffer(crtp_start + 11, 1):uint() duration = buffer(crtp_start + 12, 4):le_float() + undecoded = undecoded - 16 end elseif cmd == Commands.COMMAND_TAKEOFF_WITH_VELOCITY then cmd_str = "Take Off With Velocity" @@ -294,6 +302,7 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) retval = buffer(crtp_start + 4):uint() local success = (retval == 0) and "Success" or "Failure" port_tree:add_le(f_crtp_setpoint_hl_retval, retval):append_text(" (" .. success .. ")") + undecoded = undecoded - 4 end if id then @@ -348,6 +357,7 @@ function handle_parameter_port(tree, buffer, channel, size) if size >= 7 then port_tree:add_le(f_crtp_parameter_val_float, buffer(crtp_start + 4):le_float()) end + undecoded = 0 end end @@ -386,6 +396,8 @@ function crtp.dissector(buffer, pinfo, tree) local crtp_size = buffer:len() - crtp_start subtree:add_le(f_crtp_size, crtp_size) + undecoded = crtp_size - 1 + -- Check for safelink packet if crtp_size == 3 and header == 0xF3 and buffer(crtp_start + 1, 1):uint() == 0x05 then pinfo.cols.info = "SafeLink" @@ -412,6 +424,7 @@ function crtp.dissector(buffer, pinfo, tree) if crtp_port == Ports.Console then local port_tree = tree:add(crtp, port_name) port_tree:add_le(f_crtp_console_text, buffer(crtp_start + 1):string()) + undecoded = 0 end if crtp_port == Ports.Parameters then @@ -422,6 +435,10 @@ function crtp.dissector(buffer, pinfo, tree) handle_setpoint_highlevel(tree, receive, buffer, crtp_channel, crtp_size) end + if undecoded > 0 then + local from = crtp_start + (crtp_size - undecoded) + subtree:add_le(f_crtp_undecoded, buffer(from, undecoded):bytes():tohex()) + end end wtap_encap = DissectorTable.get("wtap_encap") From d005eb2350965c3510d480112e5f247fd48829d0 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 11 Jan 2022 12:00:52 +0100 Subject: [PATCH 337/861] Use pypi-packaged libusb so/dll Also removes support for pyusb0 --- cflib/drivers/cfusb.py | 85 ++++++++++--------------------------- cflib/drivers/crazyradio.py | 81 +++++++++-------------------------- setup.py | 1 + 3 files changed, 43 insertions(+), 124 deletions(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index 96e30dd4c..9a4f41130 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -29,6 +29,8 @@ import os import usb +import usb.core +import libusb_package try: if os.environ['CRTP_PCAP_LOG'] is not None: @@ -44,18 +46,6 @@ USB_VID = 0x0483 USB_PID = 0x5740 -try: - import usb.core - - pyusb_backend = None - if os.name == 'nt': - import usb.backend.libusb0 as libusb0 - - pyusb_backend = libusb0.get_backend() - pyusb1 = True - -except Exception: - pyusb1 = False def _find_devices(): @@ -66,18 +56,13 @@ def _find_devices(): logger.info('Looking for devices....') - if pyusb1: - for d in usb.core.find(idVendor=USB_VID, idProduct=USB_PID, find_all=1, - backend=pyusb_backend): + devices = usb.core.find(idVendor=USB_VID, idProduct=USB_PID, find_all=1, + backend=libusb_package.get_libusb1_backend()) + if devices: + for d in devices: if d.manufacturer == 'Bitcraze AB': ret.append(d) - else: - busses = usb.busses() - for bus in busses: - for device in bus.devices: - if device.idVendor == USB_VID: - if device.idProduct == USB_PID: - ret += [device, ] + return ret @@ -100,17 +85,11 @@ def __init__(self, device=None, devid=0): self.dev = None if self.dev: - if (pyusb1 is True): - self.dev.set_configuration(1) - self.handle = self.dev - self.version = float( - '{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, - self.dev.bcdDevice & 0x0FF)) - else: - self.handle = self.dev.open() - self.handle.setConfiguration(1) - self.handle.claimInterface(0) - self.version = float(self.dev.deviceVersion) + self.dev.set_configuration(1) + self.handle = self.dev + self.version = float( + '{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, + self.dev.bcdDevice & 0x0FF)) def get_serial(self): # The signature for get_string has changed between versions to 1.0.0b1, @@ -122,12 +101,8 @@ def get_serial(self): return usb.util.get_string(self.dev, self.dev.iSerialNumber) def close(self): - if (pyusb1 is False): - if self.handle: - self.handle.releaseInterface() - else: - if self.dev: - usb.util.dispose_resources(self.dev) + if self.dev: + usb.util.dispose_resources(self.dev) self.handle = None self.dev = None @@ -166,23 +141,15 @@ def send_packet(self, dataOut): The ack contains information about the packet transmission and a data payload if the ack packet contained any """ try: - if (pyusb1 is False): - self.handle.bulkWrite(1, dataOut, 20) - else: - self.handle.write(endpoint=1, data=dataOut, timeout=20) - + self.handle.write(endpoint=1, data=dataOut, timeout=20) self._log_packet(False, self.dev.port_number, dataOut) - except usb.USBError: pass def receive_packet(self): dataIn = () try: - if (pyusb1 is False): - dataIn = self.handle.bulkRead(0x81, 64, 20) - else: - dataIn = self.handle.read(0x81, 64, timeout=20) + dataIn = self.handle.read(0x81, 64, timeout=20) except usb.USBError as e: try: if e.backend_error_code == -7 or e.backend_error_code == -116: @@ -204,19 +171,13 @@ def receive_packet(self): # Private utility functions def _send_vendor_setup(handle, request, value, index, data): - if pyusb1: - handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value, - wIndex=index, timeout=1000, data_or_wLength=data) - else: - handle.controlMsg(usb.TYPE_VENDOR, request, data, value=value, - index=index, timeout=1000) + handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value, + wIndex=index, timeout=1000, data_or_wLength=data) + def _get_vendor_setup(handle, request, value, index, length): - if pyusb1: - return handle.ctrl_transfer(usb.TYPE_VENDOR | 0x80, request, - wValue=value, wIndex=index, timeout=1000, - data_or_wLength=length) - else: - return handle.controlMsg(usb.TYPE_VENDOR | 0x80, request, length, - value=value, index=index, timeout=1000) + return handle.ctrl_transfer(usb.TYPE_VENDOR | 0x80, request, + wValue=value, wIndex=index, timeout=1000, + data_or_wLength=length) + diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 8389627ec..b8b2a6c3e 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -30,6 +30,8 @@ import platform import usb +import usb.core +import libusb_package try: if os.environ['CRTP_PCAP_LOG'] is not None: @@ -59,18 +61,6 @@ SCANN_CHANNELS = 0x21 LAUNCH_BOOTLOADER = 0xFF -try: - import usb.core - - pyusb_backend = None - if os.name == 'nt': - import usb.backend.libusb0 as libusb0 - - pyusb_backend = libusb0.get_backend() - pyusb1 = True -except Exception: - pyusb1 = False - def _find_devices(serial=None): """ @@ -78,21 +68,13 @@ def _find_devices(serial=None): """ ret = [] - if pyusb1: - for d in usb.core.find(idVendor=0x1915, idProduct=0x7777, find_all=1, - backend=pyusb_backend): + devices = usb.core.find(idVendor=0x1915, idProduct=0x7777, find_all=1, + backend=libusb_package.get_libusb1_backend()) + if devices: + for d in devices: if serial is not None and serial == d.serial_number: return d ret.append(d) - else: - busses = usb.busses() - for bus in busses: - for device in bus.devices: - if device.idVendor == CRADIO_VID: - if device.idProduct == CRADIO_PID: - if serial == device.serial_number: - return device - ret += [device, ] return ret @@ -142,16 +124,10 @@ def __init__(self, device=None, devid=0, serial=None): self.dev = device - if (pyusb1 is True): - self.dev.set_configuration(1) - self.handle = self.dev - self.version = float('{0:x}.{1:x}'.format( - self.dev.bcdDevice >> 8, self.dev.bcdDevice & 0x0FF)) - else: - self.handle = self.dev.open() - self.handle.setConfiguration(1) - self.handle.claimInterface(0) - self.version = float(self.dev.deviceVersion) + self.dev.set_configuration(1) + self.handle = self.dev + self.version = float('{0:x}.{1:x}'.format( + self.dev.bcdDevice >> 8, self.dev.bcdDevice & 0x0FF)) if self.version < 0.3: raise 'This driver requires Crazyradio firmware V0.3+' @@ -159,9 +135,6 @@ def __init__(self, device=None, devid=0, serial=None): if self.version < 0.4: logger.warning('You should update to Crazyradio firmware V0.4+') - # Reset the dongle to power up settings - if platform.system() == 'Linux': - self.handle.reset() self.set_data_rate(self.DR_2MPS) self.set_channel(2) self.arc = -1 @@ -183,12 +156,8 @@ def _log_packet(self, receive, devid, address, channel, packet): pass def close(self): - if (pyusb1 is False): - if self.handle: - self.handle.releaseInterface() - else: - if self.dev: - usb.util.dispose_resources(self.dev) + if self.dev: + usb.util.dispose_resources(self.dev) self.handle = None self.dev = None @@ -303,12 +272,8 @@ def send_packet(self, dataOut): ackIn = None data = None try: - if (pyusb1 is False): - self.handle.bulkWrite(1, dataOut, 1000) - data = self.handle.bulkRead(0x81, 64, 1000) - else: - self.handle.write(endpoint=1, data=dataOut, timeout=1000) - data = self.handle.read(0x81, 64, timeout=1000) + self.handle.write(endpoint=1, data=dataOut, timeout=1000) + data = self.handle.read(0x81, 64, timeout=1000) self._log_packet( False, @@ -344,19 +309,11 @@ def send_packet(self, dataOut): # Private utility functions def _send_vendor_setup(handle, request, value, index, data): - if pyusb1: - handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value, - wIndex=index, timeout=1000, data_or_wLength=data) - else: - handle.controlMsg(usb.TYPE_VENDOR, request, data, value=value, - index=index, timeout=1000) + handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value, + wIndex=index, timeout=1000, data_or_wLength=data) def _get_vendor_setup(handle, request, value, index, length): - if pyusb1: - return handle.ctrl_transfer(usb.TYPE_VENDOR | 0x80, request, - wValue=value, wIndex=index, timeout=1000, - data_or_wLength=length) - else: - return handle.controlMsg(usb.TYPE_VENDOR | 0x80, request, length, - value=value, index=index, timeout=1000) + return handle.ctrl_transfer(usb.TYPE_VENDOR | 0x80, request, + wValue=value, wIndex=index, timeout=1000, + data_or_wLength=length) diff --git a/setup.py b/setup.py index cfe0e1d15..60f3ac154 100644 --- a/setup.py +++ b/setup.py @@ -34,6 +34,7 @@ install_requires=[ 'pyusb>=1.0.0b2', + 'libusb-package~=1.0', 'opencv-python-headless~=4.5.1' ], From 8278ae3bcca133cdd30470a7d4fbf4a8b560be4f Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 13 Jan 2022 10:17:53 +0100 Subject: [PATCH 338/861] Force use of libusb0 on Windows Required until libusb1 1.0.25 is released --- cflib/drivers/cfusb.py | 15 ++++++++------- cflib/drivers/crazyradio.py | 14 ++++++++++---- 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index 9a4f41130..b15e0d4e3 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -28,9 +28,9 @@ import logging import os +import libusb_package import usb import usb.core -import libusb_package try: if os.environ['CRTP_PCAP_LOG'] is not None: @@ -47,7 +47,6 @@ USB_PID = 0x5740 - def _find_devices(): """ Returns a list of CrazyRadio devices currently connected to the computer @@ -56,14 +55,18 @@ def _find_devices(): logger.info('Looking for devices....') + if os.name == 'nt': + backend = None + else: + backend = libusb_package.get_libusb1_backend() + devices = usb.core.find(idVendor=USB_VID, idProduct=USB_PID, find_all=1, - backend=libusb_package.get_libusb1_backend()) + backend=backend) if devices: for d in devices: if d.manufacturer == 'Bitcraze AB': ret.append(d) - return ret @@ -89,7 +92,7 @@ def __init__(self, device=None, devid=0): self.handle = self.dev self.version = float( '{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, - self.dev.bcdDevice & 0x0FF)) + self.dev.bcdDevice & 0x0FF)) def get_serial(self): # The signature for get_string has changed between versions to 1.0.0b1, @@ -175,9 +178,7 @@ def _send_vendor_setup(handle, request, value, index, data): wIndex=index, timeout=1000, data_or_wLength=data) - def _get_vendor_setup(handle, request, value, index, length): return handle.ctrl_transfer(usb.TYPE_VENDOR | 0x80, request, wValue=value, wIndex=index, timeout=1000, data_or_wLength=length) - diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index b8b2a6c3e..13e73bcb4 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -27,11 +27,10 @@ """ import logging import os -import platform +import libusb_package import usb import usb.core -import libusb_package try: if os.environ['CRTP_PCAP_LOG'] is not None: @@ -68,8 +67,15 @@ def _find_devices(serial=None): """ ret = [] + if os.name == 'nt': + import usb.backend.libusb0 as libusb0 + + backend = libusb0 + else: + backend = libusb_package.get_libusb1_backend() + devices = usb.core.find(idVendor=0x1915, idProduct=0x7777, find_all=1, - backend=libusb_package.get_libusb1_backend()) + backend=backend) if devices: for d in devices: if serial is not None and serial == d.serial_number: @@ -310,7 +316,7 @@ def send_packet(self, dataOut): # Private utility functions def _send_vendor_setup(handle, request, value, index, data): handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value, - wIndex=index, timeout=1000, data_or_wLength=data) + wIndex=index, timeout=1000, data_or_wLength=data) def _get_vendor_setup(handle, request, value, index, length): From 5c1c376b6c12a7989b5c7cdb91b52b707d499bbc Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 13 Jan 2022 14:23:47 +0100 Subject: [PATCH 339/861] Fix Windows USB regression introduced in #299 --- cflib/drivers/cfusb.py | 4 +++- cflib/drivers/crazyradio.py | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index b15e0d4e3..ab1706fd6 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -56,7 +56,9 @@ def _find_devices(): logger.info('Looking for devices....') if os.name == 'nt': - backend = None + import usb.backend.libusb0 as libusb0 + + backend = libusb0.get_backend() else: backend = libusb_package.get_libusb1_backend() diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 13e73bcb4..f53ea82bc 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -70,7 +70,7 @@ def _find_devices(serial=None): if os.name == 'nt': import usb.backend.libusb0 as libusb0 - backend = libusb0 + backend = libusb0.get_backend() else: backend = libusb_package.get_libusb1_backend() From b34dfaad6e93c2e815e6367615a80b1d6f2ebb93 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 13 Jan 2022 14:56:08 +0100 Subject: [PATCH 340/861] Make sure the initial estimator picks the right solutions --- .../lighthouse_initial_estimator.py | 42 ++++++++++++++++--- examples/lighthouse/bs_geometry_estimation.py | 14 ++++--- 2 files changed, 44 insertions(+), 12 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 589a927da..7bedbc9e4 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -72,16 +72,46 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position poses: dict[int, Pose] = {} for bs, angles in sample.angles_calibrated.items(): Q = angles.projection_pair_list() - estimate = IppeCf.solve(sensor_positions, Q) + estimates_ref_bs = IppeCf.solve(sensor_positions, Q) + estimates_ref_cf = cls._convert_estimates_to_cf_reference_frame(estimates_ref_bs) + bs_pose = cls._solution_picker(estimates_ref_cf) - # Pick the first solution from IPPE and convert to cf ref frame - R_mat = estimate[0][0].transpose() - t_vec = np.dot(R_mat, -estimate[0][1]) - - poses[bs] = Pose(R_mat, t_vec) + poses[bs] = bs_pose result.append(poses) return result + def _convert_estimates_to_cf_reference_frame(estimates_ref_bs: list[IppeCf.Solution]) -> tuple[Pose, Pose]: + """ + Convert the two ippe solutions from the base station reference frame to the CF reference frame + """ + R1 = estimates_ref_bs[0].R.transpose() + t1 = np.dot(R1, -estimates_ref_bs[0].t) + + R2 = estimates_ref_bs[1].R.transpose() + t2 = np.dot(R2, -estimates_ref_bs[1].t) + + return Pose(R1, t1), Pose(R2, t2) + + @classmethod + def _solution_picker(cls, estimates_ref_cf: tuple[Pose, Pose]) -> Pose: + """ + IPPE produces two solutions and we have to pick the right one. + The solutions are on opposite sides of the CF Z-axis and one of them is also flipped up side down. + Assuming that our base stations are pointing downwards and that the CF is fairly flat, we can + find the correct solution by looking at the orientation and pick the one where the Z-axis is pointing + upwards. + + :param estimates_ref_cf: The two possible estimated base station poses + :return: The selected solution + """ + pose1 = estimates_ref_cf[0] + pose2 = estimates_ref_cf[1] + + if np.dot(pose1.rot_matrix, (0.0, 0.0, 1.0))[2] > 0: + return pose1 + else: + return pose2 + @classmethod def _calc_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: dict[int, Pose]) -> None: # Find all base stations in the list diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index c5d09509a..1cd9a88f9 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -36,8 +36,10 @@ This script is a temporary implementation until similar functionality has been added to the client. -Prerequisite: The base station calibration data must have been +Prerequisite: +1. The base station calibration data must have been received by the Crazyflie before this script is executed. +2. Base stations must point downwards, towards the floor. ''' import logging import time @@ -48,6 +50,7 @@ from cflib.crazyflie.mem import LighthouseMemHelper from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +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 @@ -141,12 +144,10 @@ def upload_geometry(scf, bs_poses): event = Event() def data_written(success): - if not success: - raise Exception('Upload failed') event.set() - helper = LighthouseMemHelper(scf.cf) - helper.write_geos(geo_dict, data_written) + helper = LighthouseConfigWriter(scf.cf) + helper.write_and_store_config(data_written, geos=geo_dict) event.wait() @@ -193,7 +194,8 @@ def data_written(success): 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. ') + 'all the base stations are received and do not move too fast. Make sure the Crazyflie is fairly ' + + 'level during the recording.') print('This step does not add anything in a system with only one base station, enter 0 in this case.') default_time = 10 recording_time = input(f'Enter the number of seconds you want to record ({default_time} by default), ' + From 0a0c49ff99d712d88758a5884e2f5b5072b7d7ec Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 13 Jan 2022 14:57:20 +0100 Subject: [PATCH 341/861] Allow up to 100 iterations --- cflib/localization/lighthouse_geometry_solver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 507e561e5..7c1707d28 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -59,7 +59,7 @@ def __init__(self) -> None: self.n_sensors: int = None # The maximum nr of iterations to execute when solving the system - self.max_nr_iter = 10 + self.max_nr_iter = 100 self.bs_id_to_index: dict[int, int] = {} self.bs_index_to_id: dict[int, int] = {} From bde8c25a41400a919bc0379b1a940778d0a35a45 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 13 Jan 2022 15:18:29 +0100 Subject: [PATCH 342/861] Styling --- cflib/localization/lighthouse_system_aligner.py | 14 ++++++++------ examples/lighthouse/bs_geometry_estimation.py | 1 - .../localization/test_lighthouse_system_aligner.py | 7 +++++-- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/cflib/localization/lighthouse_system_aligner.py b/cflib/localization/lighthouse_system_aligner.py index 91aaf04eb..e5c4a22cc 100644 --- a/cflib/localization/lighthouse_system_aligner.py +++ b/cflib/localization/lighthouse_system_aligner.py @@ -54,17 +54,18 @@ def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: lis return result @classmethod - def find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: list[npt.ArrayLike]) -> Pose: + def find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], + xy_plane: list[npt.ArrayLike]) -> Pose: """ - Finds the transformation from the current reference frame to a desired reference frame based on measured positions - of the desired reference frame. + Finds the transformation from the current reference frame to a desired reference frame based on measured + positions of the desired reference frame. :param origin: The position of the desired origin in the current reference frame :param x_axis: One or more positions on the desired positive X-axis (X>0, Y=Z=0) in the current reference frame :param x_axis: One or more positions in the desired XY-plane (Z=0) in the current reference frame - :return: The transformation from the current reference frame to the desired reference frame. Note: the solution may - be flipped. + :return: The transformation from the current reference frame to the desired reference frame. Note: the + solution may be flipped. """ args = (origin, x_axis, xy_plane) @@ -104,7 +105,8 @@ def _Pose_from_params(cls, params: npt.ArrayLike) -> Pose: return Pose.from_rot_vec(R_vec=params[:3], t_vec=params[3:]) @classmethod - def _de_flip_transformation(cls, raw_transformation: Pose, x_axis: list[npt.ArrayLike], bs_poses: dict[int, Pose]) -> Pose: + def _de_flip_transformation(cls, raw_transformation: Pose, x_axis: list[npt.ArrayLike], + bs_poses: dict[int, Pose]) -> Pose: """ Investigats a transformation and flips it if needed. This method assumes that 1. all base stations are at Z>0 diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index 1cd9a88f9..800d74861 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -47,7 +47,6 @@ import cflib.crtp # noqa from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import LighthouseMemHelper from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter diff --git a/test/localization/test_lighthouse_system_aligner.py b/test/localization/test_lighthouse_system_aligner.py index c1175d843..ecdd60469 100644 --- a/test/localization/test_lighthouse_system_aligner.py +++ b/test/localization/test_lighthouse_system_aligner.py @@ -19,10 +19,12 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +from test.localization.lighthouse_test_base import LighthouseTestBase + import numpy as np + from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner from cflib.localization.lighthouse_types import Pose -from test.localization.lighthouse_test_base import LighthouseTestBase class TestLighthouseSystemAligner(LighthouseTestBase): @@ -72,7 +74,8 @@ def test_that_base_stations_are_rotated(self): actual = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) # Assert - self.assertPosesAlmostEqual(Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(0.0, 0.0, 1.0)), actual[bs_id]) + self.assertPosesAlmostEqual(Pose.from_rot_vec( + R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(0.0, 0.0, 1.0)), actual[bs_id]) def test_that_solution_is_de_flipped(self): # Fixture From 077a3d1e9eecd742ab414317c3baac82d82f945e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 13 Jan 2022 15:34:49 +0100 Subject: [PATCH 343/861] Updates after comments from @jonasdn --- cflib/localization/ippe_cf.py | 29 +++++++------------ cflib/localization/lighthouse_bs_vector.py | 2 +- .../lighthouse_initial_estimator.py | 1 - examples/lighthouse/bs_geometry_estimation.py | 6 ++-- 4 files changed, 14 insertions(+), 24 deletions(-) diff --git a/cflib/localization/ippe_cf.py b/cflib/localization/ippe_cf.py index 6110d6550..51472dea0 100644 --- a/cflib/localization/ippe_cf.py +++ b/cflib/localization/ippe_cf.py @@ -57,24 +57,17 @@ def solve(U_cf: npt.ArrayLike, Q_cf: npt.ArrayLike) -> list[Solution]: This is a wrapper function to convert from/to CF coordinate system/array style - Parameters - ---------- - U_cf: Nx3 matrix holding the model points in world coordinates. - - Q_cf: Nx2 matrix holding the points in the image. These are in normalised - pixel coordinates. That is, the effects of the camera's intrinsic matrix - and lens distortion are corrected, so that the Q projects with a perfect - pinhole model. - - First param: Y (positive to the left) - Second param: Z (positive up) - - - Return - --------- - IPPEPoses: A list that contains 2 sets of pose solution from IPPE including rotation matrix - translation matrix, and reprojection error. The first solution in the list has - the smallest reprojection error. + :param U_cf: Nx3 matrix holding the model points in world coordinates. + :param Q_cf: Nx2 matrix holding the points in the image. These are in normalised + pixel coordinates. That is, the effects of the camera's intrinsic matrix + and lens distortion are corrected, so that the Q projects with a perfect + pinhole model. + + First param: Y (positive to the left) + Second param: Z (positive up) + :return: A list that contains 2 sets of pose solution from IPPE including rotation matrix + translation matrix, and reprojection error. The first solution in the list has + the smallest reprojection error. """ U, Q = IppeCf._cf_to_ippe(U_cf, Q_cf) diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py index 0b18db4f0..89537ac3f 100644 --- a/cflib/localization/lighthouse_bs_vector.py +++ b/cflib/localization/lighthouse_bs_vector.py @@ -6,7 +6,7 @@ # | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2021 Bitcraze AB +# Copyright (C) 2021-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 diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 7bedbc9e4..7f2eb0187 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -54,7 +54,6 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A # Use the first CF pose as the global reference frame bs_poses: dict[int, Pose] = bs_poses_ref_cfs[0] - # TODO Do not use first sample as reference, pass it in as a parameter cls._calc_remaining_bs_poses(bs_poses_ref_cfs, bs_poses) cf_poses = cls._calc_cf_poses(bs_poses_ref_cfs, bs_poses) diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index 800d74861..6c328e8fb 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -17,10 +17,8 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . ''' 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 From 7153f3c8946c847cf20fb34f9d15bf0c8662dc7f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 13 Jan 2022 16:01:43 +0100 Subject: [PATCH 344/861] Print base station channel --- examples/lighthouse/bs_geometry_estimation.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index 6c328e8fb..3254e9e2e 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -121,10 +121,10 @@ def estimate_geometry(origin, x_axis, xy_plane, samples): print(' Base stations at:') for bs_id, pose in sorted(bs_aligned_poses.items()): pos = pose.translation - print(f' {bs_id}: ({pos[0]}, {pos[1]}, {pos[2]})') + print(f' {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})') print(' Solution match per base station:') for bs_id, value in solution.error_info['bs'].items(): - print(f' {bs_id}: {value}') + print(f' {bs_id + 1}: {value}') return bs_aligned_poses From aa24c720d3137d8cc330d3088b9110346f9d5999 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 14 Jan 2022 14:17:53 +0100 Subject: [PATCH 345/861] Do not raise exception whe not converging --- cflib/localization/lighthouse_geometry_solver.py | 7 +++++-- examples/lighthouse/bs_geometry_estimation.py | 2 ++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 7c1707d28..7b4f2696c 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -78,6 +78,10 @@ def __init__(self) -> None: # Information about errors in the solution self.error_info = {} + # Indicates if the solution coverged (True). + # If it did not converge, the solution is probably not good enough to use + self.success = False + class LighthouseGeometrySolver: """ @@ -408,8 +412,7 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, bs_id = solution.bs_index_to_id[index] solution.bs_poses[bs_id] = cls._params_to_pose(pose, solution) - if not lsq_result.success: - raise Exception('Solution did not converge') + solution.success = lsq_result.success # Extract the error for each CF pose residuals = lsq_result.fun diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index 3254e9e2e..bed47d26c 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -108,6 +108,8 @@ def estimate_geometry(origin, x_axis, xy_plane, samples): matched_samples = [origin] + x_axis + xy_plane + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2) initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) solution = LighthouseGeometrySolver.solve(initial_guess, 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) From cd32765b7c12f6fede9a1e98fc0d154cad231f74 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 14 Jan 2022 14:20:57 +0100 Subject: [PATCH 346/861] Impoved prints --- examples/lighthouse/bs_geometry_estimation.py | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index bed47d26c..b0fc2dc7f 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -20,6 +20,8 @@ # 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 @@ -78,20 +80,34 @@ def ready_cb(averages): for bs_id, data in recorded_angles.items(): angles_calibrated[bs_id] = data[1] + visible = ', '.join(map(lambda x:str(x + 1), recorded_angles.keys())) + print(f' Position recorded, bs visible: {visible}') + return LhCfPoseSample(angles_calibrated=angles_calibrated) def record_angles_sequence(scf, recording_time_s): result = [] + bs_seen = set() + def ready_cb(bs_id, angles): 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() - time.sleep(recording_time_s) + end_time = time.time() + recording_time_s + + while time.time() < end_time: + time_left = 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 @@ -168,7 +184,6 @@ def data_written(success): input('Press return when ready. ') print(' Recording...') origin = record_angles_average(scf) - print(' Position recorded') print('Step 3. Put the Crazyflie somehere on the positive X-axis.') print('Multiple samples can be recorded if you want to, type "r" before you hit enter to repeat the step.') @@ -178,7 +193,6 @@ def data_written(success): do_repeat = 'r' == input('Press return when ready. ').lower() print(' Recording...') x_axis.append(record_angles_average(scf)) - print(' Position recorded') print('Step 4. Put the Crazyflie somehere in the XY-plane, but not on the X-axis.') print('Multiple samples can be recorded if you want to, type "r" before you hit enter to repeat the step.') @@ -188,7 +202,6 @@ def data_written(success): do_repeat = 'r' == input('Press return when ready. ').lower() print(' Recording...') xy_plane.append(record_angles_average(scf)) - print(' Position recorded') print() print('Step 5. We will now record data from the space you plan to fly in and optimize the base station ' + From 0d6ab99e62141430833125843552b0164a20cf3c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 16 Jan 2022 17:27:43 +0100 Subject: [PATCH 347/861] Updated system aligner --- .../localization/lighthouse_system_aligner.py | 13 ++++----- .../test_lighthouse_system_aligner.py | 27 +++++++++++++++++-- 2 files changed, 32 insertions(+), 8 deletions(-) diff --git a/cflib/localization/lighthouse_system_aligner.py b/cflib/localization/lighthouse_system_aligner.py index e5c4a22cc..755ac8eac 100644 --- a/cflib/localization/lighthouse_system_aligner.py +++ b/cflib/localization/lighthouse_system_aligner.py @@ -44,7 +44,7 @@ def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: lis :param bs_poses: a dictionary with the base station poses in the current reference frame :return: a dictionary with the base station poses in the desired reference frame """ - raw_transformation = cls.find_transformation(origin, x_axis, xy_plane) + raw_transformation = cls._find_transformation(origin, x_axis, xy_plane) transformation = cls._de_flip_transformation(raw_transformation, x_axis, bs_poses) result: dict[int, Pose] = {} @@ -54,8 +54,8 @@ def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: lis return result @classmethod - def find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], - xy_plane: list[npt.ArrayLike]) -> Pose: + def _find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], + xy_plane: list[npt.ArrayLike]) -> Pose: """ Finds the transformation from the current reference frame to a desired reference frame based on measured positions of the desired reference frame. @@ -110,12 +110,13 @@ def _de_flip_transformation(cls, raw_transformation: Pose, x_axis: list[npt.Arra """ Investigats a transformation and flips it if needed. This method assumes that 1. all base stations are at Z>0 - 2. x_axis samples are taken at x>0 + 2. x_axis samples are taken at X>0 """ transformation = raw_transformation - # X-axis poses should be on the positivie X-axis, check the first one - if raw_transformation.rotate_translate(x_axis[0])[0] < 0.0: + # X-axis poses should be on the positivie X-axis, check that the "mean" of the x-axis points ends up at X>0 + x_axis_mean = np.mean(x_axis, axis=0) + if raw_transformation.rotate_translate(x_axis_mean)[0] < 0.0: flip_around_z_axis = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi)) transformation = flip_around_z_axis.rotate_translate_pose(transformation) diff --git a/test/localization/test_lighthouse_system_aligner.py b/test/localization/test_lighthouse_system_aligner.py index ecdd60469..db0660344 100644 --- a/test/localization/test_lighthouse_system_aligner.py +++ b/test/localization/test_lighthouse_system_aligner.py @@ -38,10 +38,11 @@ def test_that_transformation_is_found_for_single_points(self): xy_plane = [(2.0, 1.0, 0.0)] # Test - actual = LighthouseSystemAligner.find_transformation(origin, x_axis, xy_plane) + actual = LighthouseSystemAligner._find_transformation(origin, x_axis, xy_plane) # Assert self.assertVectorsAlmostEqual((0.0, 0.0, 0.0), actual.rotate_translate((1.0, 0.0, 0.0))) + self.assertVectorsAlmostEqual((1.0, 0.0, 0.0), actual.rotate_translate((1.0, 1.0, 0.0))) self.assertVectorsAlmostEqual((0.0, 1.0, 0.0), actual.rotate_translate((0.0, 0.0, 0.0))) self.assertVectorsAlmostEqual((0.0, 0.0, 1.0), actual.rotate_translate((1.0, 0.0, 1.0))) @@ -53,10 +54,11 @@ def test_that_transformation_is_found_for_multiple_points(self): xy_plane = [(2.0, 1.0, 0.0), (3.0, -1.0, 0.0), (5.0, 0.0, 0.0)] # Test - actual = LighthouseSystemAligner.find_transformation(origin, x_axis, xy_plane) + actual = LighthouseSystemAligner._find_transformation(origin, x_axis, xy_plane) # Assert self.assertVectorsAlmostEqual((0.0, 0.0, 0.0), actual.rotate_translate((1.0, 0.0, 0.0))) + self.assertVectorsAlmostEqual((1.0, 0.0, 0.0), actual.rotate_translate((1.0, 1.0, 0.0))) self.assertVectorsAlmostEqual((0.0, 1.0, 0.0), actual.rotate_translate((0.0, 0.0, 0.0))) self.assertVectorsAlmostEqual((0.0, 0.0, 1.0), actual.rotate_translate((1.0, 0.0, 1.0))) @@ -92,3 +94,24 @@ def test_that_solution_is_de_flipped(self): # Assert self.assertPosesAlmostEqual(expected, actual[bs_id]) + + def test_that_is_aligned_for_multiple_points_where_system_is_rotated_and_poins_are_fuzzy(self): + # Fixture + origin = (0.0, 0.0, 0.0) + x_axis = [(-1.0, 0.0 + 0.01, 0.0), (-2.0, 0.0 - 0.02, 0.0)] + xy_plane = [(2.0, 2.0, 0.0 + 0.02), (-3.0, -2.0, 0.0 + 0.01), (5.0, 0.0, 0.0 - 0.01)] + + # Note: Z of base stations must be positive (above the floor) + bs_poses = { + 1: Pose.from_rot_vec(t_vec=(1.0, 0.0, 1.0)), + 2: Pose.from_rot_vec(t_vec=(0.0, 1.0, 1.0)), + 3: Pose.from_rot_vec(t_vec=(0.0, 0.0, 1.0)), + } + + # Test + actual = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) + + # Assert + self.assertVectorsAlmostEqual((-1.0, 0.0, 1.0), actual[1].translation, places=1) + self.assertVectorsAlmostEqual((0.0, -1.0, 1.0), actual[2].translation, places=1) + self.assertVectorsAlmostEqual((0.0, 0.0, 1.0), actual[3].translation, places=1) From 02c9c7ca40a649610ca90b75651c69aa95840240 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 18 Jan 2022 09:51:41 +0100 Subject: [PATCH 348/861] Fixed styling --- examples/lighthouse/bs_geometry_estimation.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/bs_geometry_estimation.py index b0fc2dc7f..497e12cc0 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/bs_geometry_estimation.py @@ -80,7 +80,7 @@ def ready_cb(averages): for bs_id, data in recorded_angles.items(): angles_calibrated[bs_id] = data[1] - visible = ', '.join(map(lambda x:str(x + 1), recorded_angles.keys())) + visible = ', '.join(map(lambda x: str(x + 1), recorded_angles.keys())) print(f' Position recorded, bs visible: {visible}') return LhCfPoseSample(angles_calibrated=angles_calibrated) @@ -125,7 +125,7 @@ def estimate_geometry(origin, x_axis, xy_plane, samples): initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) if not solution.success: - print("Solution did not converge, it might not be good!") + print('Solution did not converge, it might not be good!') start_x_axis = 1 start_xy_plane = 1 + len(x_axis) From 240bcab51491e07068169a8b74817bd234fccee8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 18 Jan 2022 12:58:33 +0100 Subject: [PATCH 349/861] #297 Reverted part of PR to make the client not use new estimator --- cflib/localization/lighthouse_bs_geo.py | 176 +++++++++++++++++++++--- setup.py | 1 + 2 files changed, 158 insertions(+), 19 deletions(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index f5a24e5c8..8771063d2 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -6,7 +6,7 @@ # | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2021-2022 Bitcraze AB +# 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 @@ -22,14 +22,10 @@ """ Functionality to handle base station geometry in the lighthouse poistioning system """ -from __future__ import annotations +import math -from cflib.localization.lighthouse_bs_vector import LighthouseBsVector -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_types import LhDeck4SensorPositions -from cflib.localization.lighthouse_types import LhMeasurement +import cv2 as cv +import numpy as np class LighthouseBsGeoEstimator: @@ -39,10 +35,48 @@ class LighthouseBsGeoEstimator: """ def __init__(self): + self._directions = { + self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), + self._hash_sensor_order([2, 0, 3, 1]): math.radians(25), + self._hash_sensor_order([2, 3, 0, 1]): math.radians(65), + self._hash_sensor_order([3, 2, 0, 1]): math.radians(90), + self._hash_sensor_order([3, 2, 1, 0]): math.radians(115), + self._hash_sensor_order([3, 1, 2, 0]): math.radians(155), + self._hash_sensor_order([1, 3, 2, 0]): math.radians(180), + self._hash_sensor_order([1, 3, 0, 2]): math.radians(205), + self._hash_sensor_order([1, 0, 3, 2]): math.radians(245), + self._hash_sensor_order([0, 1, 3, 2]): math.radians(270), + self._hash_sensor_order([0, 1, 2, 3]): math.radians(295), + self._hash_sensor_order([0, 2, 1, 3]): math.radians(335), + } + + # Sensor distances on the lighthouse deck + sensor_distance_width = 0.015 + sensor_distance_length = 0.03 + + # Sensor positions in world coordinates, open cv style + self._lighthouse_3d = np.float32( + [ + [-sensor_distance_width / 2, 0, -sensor_distance_length / 2], + [sensor_distance_width / 2, 0, -sensor_distance_length / 2], + [-sensor_distance_width / 2, 0, sensor_distance_length / 2], + [sensor_distance_width / 2, 0, sensor_distance_length / 2] + ]) + + # Camera matrix + self._K = np.float64( + [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0] + ]) + + self._dist_coef = np.zeros(4) + # Sanity check maximum pos - self._sanity_max_pos = 15 + self._sanity_max_pos = 10 - def estimate_geometry(self, bs_vectors: list[LighthouseBsVector]): + def estimate_geometry(self, bs_vectors): """ Estimate the full pose of a base station based on angles from the 4 sensors on a lighthouse deck. The result is a rotation matrix and position of the @@ -52,15 +86,11 @@ def estimate_geometry(self, bs_vectors: list[LighthouseBsVector]): :return rot_bs_in_cf_coord: Rotation matrix of the BS in the CFs coordinate system :return pos_bs_in_cf_coord: Position vector of the BS in the CFs coordinate system """ - bs_id = 0 - - samples = [LhMeasurement(timestamp=0.0, base_station_id=bs_id, angles=bs_vectors)] - matched_samples = LighthouseSampleMatcher.match(samples) - initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) - solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) - pose = solution.bs_poses[bs_id] - - return pose.rot_matrix, pose.translation + guess_yaw = self._find_initial_yaw_guess(bs_vectors) + rvec_guess, tvec_guess = self._convert_yaw_to_open_cv(guess_yaw) + rw_ocv, tw_ocv = self._estimate_pose_by_pnp(bs_vectors, rvec_guess, tvec_guess) + rot_bs_in_cf_coord, pos_bs_in_cf_coord = self._opencv_to_cf(rw_ocv, tw_ocv) + return rot_bs_in_cf_coord, pos_bs_in_cf_coord def sanity_check_result(self, pos_bs_in_cf_coord): """ @@ -71,3 +101,111 @@ def sanity_check_result(self, pos_bs_in_cf_coord): if (abs(coord) > self._sanity_max_pos): return False return True + + def _find_initial_yaw_guess(self, bs_vectors): + # Assume bs is faicing slighly downwards and fairly horizontal + # Sort sensors in the order they are hit by the horizontal sweep + # and use the order to figure out roughly the direction to the + # base station + sweeps_x = { + 0: bs_vectors[0].lh_v1_horiz_angle, + 1: bs_vectors[1].lh_v1_horiz_angle, + 2: bs_vectors[2].lh_v1_horiz_angle, + 3: bs_vectors[3].lh_v1_horiz_angle + } + + ordered_map = {k: v for k, v in sorted(sweeps_x.items(), key=lambda item: item[1])} + sensor_order = list(ordered_map.keys()) + + # The base station is roughly in this direction, in CF (world) coordinates + return self._directions[self._hash_sensor_order(sensor_order)] + + def _hash_sensor_order(self, order): + hash = 0 + for i in range(4): + hash += order[i] * 4 ** i + return hash + + def _convert_yaw_to_open_cv(self, yaw): + # Base station height + bs_h = 2.0 + # Distance to base station along the floor + bs_fd = 3.0 + # Distance to base station + bs_dist = math.sqrt(bs_h ** 2 + bs_fd ** 2) + elevation = math.atan2(bs_h, bs_fd) + + # Initial position of the CF in camera coordinate system, open cv style + tvec_start = np.array([0, 0, bs_dist]) + + # Calculate rotation matrix + d_c = math.cos(-yaw + math.pi) + d_s = math.sin(-yaw + math.pi) + R_rot_y = np.array([ + [d_c, 0.0, d_s], + [0.0, 1.0, 0.0], + [-d_s, 0.0, d_c], + ]) + + e_c = math.cos(elevation) + e_s = math.sin(elevation) + R_rot_x = np.array([ + [1.0, 0.0, 0.0], + [0.0, e_c, -e_s], + [0.0, e_s, e_c], + ]) + + R = np.dot(R_rot_x, R_rot_y) + rvec_start, _ = cv.Rodrigues(R) + + return rvec_start, tvec_start + + def _estimate_pose_by_pnp(self, bs_vectors, rvec_start, tvec_start): + # Sensors as seen by the "camera" + lighthouse_image_projection = np.float32( + [ + [-math.tan(bs_vectors[0].lh_v1_horiz_angle), -math.tan(bs_vectors[0].lh_v1_vert_angle)], + [-math.tan(bs_vectors[1].lh_v1_horiz_angle), -math.tan(bs_vectors[1].lh_v1_vert_angle)], + [-math.tan(bs_vectors[2].lh_v1_horiz_angle), -math.tan(bs_vectors[2].lh_v1_vert_angle)], + [-math.tan(bs_vectors[3].lh_v1_horiz_angle), -math.tan(bs_vectors[3].lh_v1_vert_angle)] + ]) + + _ret, rvec_est, tvec_est = cv.solvePnP( + self._lighthouse_3d, + lighthouse_image_projection, + self._K, + self._dist_coef, + flags=cv.SOLVEPNP_ITERATIVE, + rvec=rvec_start, + tvec=tvec_start, + useExtrinsicGuess=True) + + if not _ret: + raise Exception('No solution found') + + Rw_ocv, Tw_ocv = self._cam_to_world(rvec_est, tvec_est) + return Rw_ocv, Tw_ocv + + def _cam_to_world(self, rvec_c, tvec_c): + R_c, _ = cv.Rodrigues(rvec_c) + R_w = np.linalg.inv(R_c) + tvec_w = -np.matmul(R_w, tvec_c) + return R_w, tvec_w + + def _opencv_to_cf(self, R_cv, t_cv): + R_opencv_to_cf = np.array([ + [0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + ]) + + R_cf_to_opencv = np.array([ + [0.0, -1.0, 0.0], + [0.0, 0.0, -1.0], + [1.0, 0.0, 0.0], + ]) + + t_cf = np.dot(R_opencv_to_cf, t_cv) + R_cf = np.dot(R_opencv_to_cf, np.dot(R_cv, R_cf_to_opencv)) + + return R_cf, t_cf diff --git a/setup.py b/setup.py index 9f6adc303..154fd66cc 100644 --- a/setup.py +++ b/setup.py @@ -36,6 +36,7 @@ 'pyusb>=1.0.0b2', 'libusb-package~=1.0', 'scipy>=1.7', + 'opencv-python-headless~=4.5.1', ], # $ pip install -e .[dev] From c4bf617021e3696b3986f835c7233783d033c804 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 20 Jan 2022 11:17:23 +0100 Subject: [PATCH 350/861] crazyradio: Re-add reset on Linux It seems to be needed. --- cflib/drivers/crazyradio.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index f53ea82bc..a96def4ce 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -27,6 +27,7 @@ """ import logging import os +import platform import libusb_package import usb @@ -141,6 +142,9 @@ def __init__(self, device=None, devid=0, serial=None): if self.version < 0.4: logger.warning('You should update to Crazyradio firmware V0.4+') + if platform.system() == 'Linux': + self.handle.reset() + self.set_data_rate(self.DR_2MPS) self.set_channel(2) self.arc = -1 From 3c16fa9405f9005b916d7d6b8c3888b16b8aa716 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 20 Jan 2022 11:36:39 +0100 Subject: [PATCH 351/861] Bump minimum Python version to 3.7 (#302) --- .github/workflows/dependency_check.yml | 4 ++-- docs/installation/install.md | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 6c74c5ea1..eb825e556 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -17,10 +17,10 @@ jobs: - name: install lib run: pip install -e . - minimum-python: + minimum-python37: runs-on: ubuntu-latest container: - image: python:3.6 + image: python:3.7 steps: - uses: actions/checkout@v2 diff --git a/docs/installation/install.md b/docs/installation/install.md index 2da50af98..ad37fceca 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -3,6 +3,10 @@ title: Installation page_id: install --- +## Requirements + +This project requires Python 3.7+. +See below sections for more platform-specific requirements. ## Development ### Developing for the cflib * [Fork the cflib](https://help.github.com/articles/fork-a-repo/) @@ -12,7 +16,7 @@ page_id: install * [Uninstall the cflib if you don't want it any more](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib` -Note: If you are developing for the cflib you must use python3. On Ubuntu (16.04, 18.08) use `pip3` instead of `pip`. +Note: If you are developing for the cflib you must use python3. On Ubuntu (20.04+) use `pip3` instead of `pip`. ### Linux, OSX, Windows From 125a44a4aa051ef51bf29f8d86bac6e2e515ca2a Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 20 Jan 2022 13:08:41 +0100 Subject: [PATCH 352/861] Update version to 0.1.17 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 154fd66cc..226cdd71c 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.16', + version='0.1.17', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 407b877b356aef799ce8521673761f7f8077996f Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 20 Jan 2022 14:22:26 +0100 Subject: [PATCH 353/861] wireshark.md: Make doc proper for website --- docs/wireshark/wireshark.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/wireshark/wireshark.md b/docs/wireshark/wireshark.md index ba555aa79..f73c091b8 100644 --- a/docs/wireshark/wireshark.md +++ b/docs/wireshark/wireshark.md @@ -7,11 +7,11 @@ Wireshark is a free and open-source packet analyzer. It is used for network trou We have written a plugin to Wireshark that can display [CRTP](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/) packets (what this library uses to communicate with Crazyflies). -![Image of CRTP dissector in Wireshark](../images/wireshark-dissector.png?raw=true "CRTP Wireshark Dissector") +![Image of CRTP dissector in Wireshark](../images/wireshark-dissector.png) In order for Wireshark (and our dissector plugin) to be able to decode CRTP you need to do *two* things: -## 1. Install the plugin [crtp-dissector.lua](../../tools/crtp-dissector.lua) in Wireshark +## 1. Install the plugin crtp-dissector.lua Wireshark looks for plugins in both a personal plugin folder and a global plugin folder. Lua plugins are stored in the plugin folders; compiled plugins are stored in subfolders of the plugin folders, with the subfolder name being the Wireshark minor version number (X.Y). There is another hierarchical level for each Wireshark plugin type (libwireshark, libwiretap and codecs). So for example the location for a libwireshark plugin foo.so (foo.dll on Windows) would be PLUGINDIR/X.Y/epan (libwireshark used to be called libepan; the other folder names are codecs and wiretap). @@ -24,7 +24,7 @@ On Unix-like systems: - The personal plugin folder is ~/.local/lib/wireshark/plugins. -Copy the crtp-dissector.lua file into the personal plugin folder and restart Wireshark or hit `CTRL+SHIFT+L` +Copy the `tools/crtp-dissector.lua` file into the personal plugin folder and restart Wireshark or hit `CTRL+SHIFT+L` ## 2. Generate a [PCAP](https://en.wikipedia.org/wiki/Pcap) file From 9787e4180ae192c9177505767d2a5ec6f18cb552 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 21 Jan 2022 15:23:03 +0100 Subject: [PATCH 354/861] Updated criteria in solution picker --- cflib/localization/lighthouse_initial_estimator.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 7f2eb0187..d84e43942 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -79,7 +79,8 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position result.append(poses) return result - def _convert_estimates_to_cf_reference_frame(estimates_ref_bs: list[IppeCf.Solution]) -> tuple[Pose, Pose]: + @classmethod + def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf.Solution]) -> tuple[Pose, Pose]: """ Convert the two ippe solutions from the base station reference frame to the CF reference frame """ @@ -106,7 +107,7 @@ def _solution_picker(cls, estimates_ref_cf: tuple[Pose, Pose]) -> Pose: pose1 = estimates_ref_cf[0] pose2 = estimates_ref_cf[1] - if np.dot(pose1.rot_matrix, (0.0, 0.0, 1.0))[2] > 0: + if np.dot(pose1.rot_matrix, (0.0, 0.0, 1.0))[2] > np.dot(pose2.rot_matrix, (0.0, 0.0, 1.0))[2]: return pose1 else: return pose2 From fa6974a8d0ef08c7aa1dd1561a401066196dbe99 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 24 Jan 2022 11:05:09 +0100 Subject: [PATCH 355/861] Added minimum numpy version --- setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/setup.py b/setup.py index 226cdd71c..7b027e310 100644 --- a/setup.py +++ b/setup.py @@ -36,6 +36,7 @@ 'pyusb>=1.0.0b2', 'libusb-package~=1.0', 'scipy>=1.7', + 'numpy>=1.20,<1.25', 'opencv-python-headless~=4.5.1', ], From 0a9a39b468d376e8f77132778c71e9ea278edaea Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 24 Jan 2022 11:05:55 +0100 Subject: [PATCH 356/861] Changed scipy dependency --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 7b027e310..cea1dc1ca 100644 --- a/setup.py +++ b/setup.py @@ -35,7 +35,7 @@ install_requires=[ 'pyusb>=1.0.0b2', 'libusb-package~=1.0', - 'scipy>=1.7', + 'scipy~=1.7', 'numpy>=1.20,<1.25', 'opencv-python-headless~=4.5.1', ], From 08c37ddfe14c42100017e5632e8852444dd1862a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 24 Jan 2022 15:01:15 +0100 Subject: [PATCH 357/861] fixed parsing xml qtm --- examples/qualisys/qualisys_hl_commander.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 3498dc6e3..1df23cafc 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -105,8 +105,8 @@ async def _connect(self): params = await self.connection.get_parameters(parameters=['6d']) xml = ET.fromstring(params) - self.qtm_6DoF_labels = [label.text for label in xml.iter('Name')] - print(self.qtm_6DoF_labels) + self.qtm_6DoF_labels = [label.text.strip() for index, label in enumerate(xml.findall("*/Body/Name"))] + await self.connection.stream_frames( components=['6D'], From 6ae2cdea2493d946f234a4445a4528b626ff649b Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 24 Jan 2022 15:05:51 +0100 Subject: [PATCH 358/861] remove double quotes --- examples/qualisys/qualisys_hl_commander.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 1df23cafc..4215fc0f8 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -105,7 +105,7 @@ async def _connect(self): 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"))] + self.qtm_6DoF_labels = [label.text.strip() for index, label in enumerate(xml.findall('*/Body/Name'))] await self.connection.stream_frames( From 863a89d8e85c148715ed086a9d83469d2ca5b52c Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 24 Jan 2022 15:10:53 +0100 Subject: [PATCH 359/861] Fix warning in radio_test example when no RSSI When no RSSI measurement are available, the radio_test example was hitting a numpy warning when averaging an empty array. This commit fixes this. --- examples/radio/radio-test.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/radio/radio-test.py b/examples/radio/radio-test.py index 9e4d811e3..e94755e28 100644 --- a/examples/radio/radio-test.py +++ b/examples/radio/radio-test.py @@ -82,8 +82,12 @@ temp.append(pk.data[2]) ack_rate = count / TRY - rssi_avg = np.mean(temp) - std = np.std(temp) + 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) From 58c91d00707d447cf5b670e561156439d0827c0e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 24 Jan 2022 15:20:53 +0100 Subject: [PATCH 360/861] fixed autopep8 --- examples/qualisys/qualisys_hl_commander.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 4215fc0f8..bafef9ad2 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -107,7 +107,6 @@ async def _connect(self): 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) From 0314cccf2619c8fd7166e12c3ee14961fbcb4157 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 24 Jan 2022 15:39:31 +0100 Subject: [PATCH 361/861] Improve description of the basicLedTiming example --- examples/basicLedTiming.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/basicLedTiming.py b/examples/basicLedTiming.py index 94d428e15..f4fcddcb6 100644 --- a/examples/basicLedTiming.py +++ b/examples/basicLedTiming.py @@ -22,9 +22,9 @@ # 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. +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. """ From 91012b32ae37d14232b5b65d91e8b1622ad99492 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Mon, 24 Jan 2022 15:53:46 +0100 Subject: [PATCH 362/861] param.py: avoid eval when converting to number --- cflib/crazyflie/param.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 37650f31b..7a5bad532 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -339,9 +339,9 @@ def set_value(self, complete_name, value): pk.data = struct.pack(' Date: Mon, 24 Jan 2022 16:23:51 +0100 Subject: [PATCH 363/861] Remove note about custom firmware to OW examples A custom nRF firmware is not needed anymore to write the OW memory. --- examples/memory/erase-ow.py | 3 --- examples/memory/write-ow.py | 8 -------- 2 files changed, 11 deletions(-) diff --git a/examples/memory/erase-ow.py b/examples/memory/erase-ow.py index 05070035e..15a529647 100644 --- a/examples/memory/erase-ow.py +++ b/examples/memory/erase-ow.py @@ -122,9 +122,6 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - print('This example will not work with the BLE version of the nRF51' - ' firmware (flashed on production units). See https://github.com' - '/bitcraze/crazyflie-clients-python/issues/166') # Initialize the low-level drivers cflib.crtp.init_drivers() diff --git a/examples/memory/write-ow.py b/examples/memory/write-ow.py index c7b27ef93..02ff94d2f 100644 --- a/examples/memory/write-ow.py +++ b/examples/memory/write-ow.py @@ -24,10 +24,6 @@ """ This example connects to the first Crazyflie that it finds and writes to the one wire memory. - -Note: this example will not work with the BLE version of the nRF51 firmware -(flashed on production units). -See https://github.com/bitcraze/crazyflie-clients-python/issues/166 """ import logging import sys @@ -134,10 +130,6 @@ def _disconnected(self, link_uri): if __name__ == '__main__': - print('This example will not work with the BLE version of the nRF51' - ' firmware (flashed on production units). See https://github.com' - '/bitcraze/crazyflie-clients-python/issues/166') - # Initialize the low-level drivers cflib.crtp.init_drivers() From 0578f6c5328d31f06664fbe82113c45ee9f3fecb Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 24 Jan 2022 16:26:50 +0100 Subject: [PATCH 364/861] Make write-ow example write boggus data by default The previous behavior was to write the Crazyflie production test deck which effectively bricked the Crazyflie unless this driver was commented out in the firmware. Write data for a dummy deck instead. --- examples/memory/write-ow.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/memory/write-ow.py b/examples/memory/write-ow.py index 02ff94d2f..64e38ba65 100644 --- a/examples/memory/write-ow.py +++ b/examples/memory/write-ow.py @@ -73,13 +73,14 @@ def _connected(self, link_uri): print('Writing test configuration to' ' memory {}'.format(mems[0].id)) - mems[0].vid = 0xBC - mems[0].pid = 0xFF + # 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] = 'Test board' + mems[0].elements[board_name_id] = 'Hello deck' mems[0].elements[board_rev_id] = 'A' mems[0].write_data(self._data_written) From abefa31e655f830b6a74941eb0143b054801ab30 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 25 Jan 2022 09:28:12 +0100 Subject: [PATCH 365/861] crtp-dissector.lua: Fix parsing parameter values --- tools/crtp-dissector.lua | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/tools/crtp-dissector.lua b/tools/crtp-dissector.lua index 57e774db7..bdd161e9c 100644 --- a/tools/crtp-dissector.lua +++ b/tools/crtp-dissector.lua @@ -344,6 +344,12 @@ function handle_parameter_port(tree, buffer, channel, size) -- Read or Write if (channel == 1 or channel == 2) and size > 2 then + if channel == 1 then + value_start = 4 + else + value_start = 3 + end + -- Add variable id local var_id = buffer(crtp_start + 1, 2):le_uint() local port_tree = tree:add(crtp, port_name) @@ -351,11 +357,11 @@ function handle_parameter_port(tree, buffer, channel, size) -- Add value if size > 3 then - port_tree:add_le(f_crtp_parameter_val_uint, buffer(crtp_start + 4):le_uint()) - port_tree:add_le(f_crtp_parameter_val_int, buffer(crtp_start + 4):le_int()) + port_tree:add_le(f_crtp_parameter_val_uint, buffer(crtp_start + value_start):le_uint()) + port_tree:add_le(f_crtp_parameter_val_int, buffer(crtp_start + value_start):le_int()) end if size >= 7 then - port_tree:add_le(f_crtp_parameter_val_float, buffer(crtp_start + 4):le_float()) + port_tree:add_le(f_crtp_parameter_val_float, buffer(crtp_start + value_start):le_float()) end undecoded = 0 end From 06c237dc957b0feb72063d4eb202195fdf2b8aeb Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Tue, 25 Jan 2022 09:52:39 +0100 Subject: [PATCH 366/861] PositionHLCommander: Do not set controller by default Closes: #310 --- cflib/positioning/position_hl_commander.py | 7 ++++--- docs/user-guides/python_api.md | 2 +- examples/autonomy/position_commander_demo.py | 8 ++++++-- examples/positioning/matrix_light_printer.py | 2 +- test/positioning/test_position_hl_commander.py | 2 +- 5 files changed, 13 insertions(+), 8 deletions(-) diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index 75e9bd94a..ab6ad34e6 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -50,7 +50,7 @@ def __init__(self, crazyflie, x=0.0, y=0.0, z=0.0, default_velocity=0.5, default_height=0.5, - controller=CONTROLLER_PID, + controller=None, default_landing_height=0.0): """ Construct an instance of a PositionHlCommander @@ -282,8 +282,9 @@ def _activate_high_level_commander(self): self._cf.param.set_value('commander.enHighLevel', '1') def _activate_controller(self): - value = str(self._controller) - self._cf.param.set_value('stabilizer.controller', value) + if self._controller is not None: + value = str(self._controller) + self._cf.param.set_value('stabilizer.controller', value) def _velocity(self, velocity): if velocity is self.DEFAULT: diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index d1302ca40..1ec10fc1b 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -508,7 +508,7 @@ to be used with a positioning system such as LPS, the lighthouse or a mocap syst ``` python with SyncCrazyflie(URI) as scf: - with PositionHlCommander(scf) as pc: + with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: # Go to the coordinate (0, 0, 1) pc.go_to(0.0, 0.0, 1.0) # The Crazyflie lands when leaving this "with" section diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py index 6f751a2c6..49af08705 100644 --- a/examples/autonomy/position_commander_demo.py +++ b/examples/autonomy/position_commander_demo.py @@ -67,7 +67,11 @@ def slightly_more_complex_usage(): def land_on_elevated_surface(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2, default_landing_height=0.35) as pc: + 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) @@ -76,7 +80,7 @@ def land_on_elevated_surface(): def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - with PositionHlCommander(scf) as pc: + with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.forward(1.0) pc.left(1.0) pc.back(1.0) diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index a2b0207d8..8aa37b447 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -110,5 +110,5 @@ def matrix_print(cf, pc, image_def): image_def = ImageDef('monalisa.png') with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - with PositionHlCommander(scf, default_height=0.5) as pc: + with PositionHlCommander(scf, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID) as pc: matrix_print(scf.cf, pc, image_def) diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index 4d40ae7de..afec5377d 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -103,7 +103,7 @@ def test_that_it_goes_up_on_take_off( def test_that_it_goes_up_to_default_height( self, sleep_mock): # Fixture - sut = PositionHlCommander(self.cf_mock, default_height=0.4) + sut = PositionHlCommander(self.cf_mock, default_height=0.4, controller=PositionHlCommander.CONTROLLER_PID) # Test sut.take_off(velocity=0.6) From 00db775216dec2064d4303ebc3343fd962a31392 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 25 Jan 2022 14:29:27 +0100 Subject: [PATCH 367/861] Fix USB connection to CF (#312) Adding a USB reset seems to fix the USB connection to the Crazyflie when trying to connect multiple times. Fixes #264 --- cflib/crtp/usbdriver.py | 7 +++---- cflib/drivers/cfusb.py | 6 +++++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index 4e1e26ab8..859244438 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -70,7 +70,9 @@ def connect(self, uri, link_quality_callback, link_error_callback): """ # check if the URI is a radio URI - if not re.search('^usb://', uri): + uri_data = re.search('^usb://([0-9]+)$', + uri) + if not uri_data: raise WrongUriType('Not a radio URI') # Open the USB dongle @@ -78,9 +80,6 @@ def connect(self, uri, link_quality_callback, link_error_callback): uri): raise WrongUriType('Wrong radio URI format!') - uri_data = re.search('^usb://([0-9]+)$', - uri) - self.uri = uri if self.cfusb is None: diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index ab1706fd6..5667974bb 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -27,6 +27,7 @@ """ import logging import os +import platform import libusb_package import usb @@ -90,6 +91,9 @@ def __init__(self, device=None, devid=0): self.dev = None if self.dev: + if platform.system() == 'Linux': + self.dev.reset() + self.dev.set_configuration(1) self.handle = self.dev self.version = float( @@ -118,7 +122,7 @@ def scan(self): return [('usb://0', '')] return [] - def set_crtp_to_usb(self, crtp_to_usb): + def set_crtp_to_usb(self, crtp_to_usb: bool): if crtp_to_usb: _send_vendor_setup(self.handle, 0x01, 0x01, 1, ()) else: From 7a44d2fb8a99cf05fe6f9e42a68be981aa8a5975 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 24 Jan 2022 16:27:06 +0100 Subject: [PATCH 368/861] Fix onewire memory erase --- cflib/crazyflie/mem/ow_element.py | 3 +-- examples/memory/write-ow.py | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/mem/ow_element.py b/cflib/crazyflie/mem/ow_element.py index 4cb7db66c..c8f615575 100644 --- a/cflib/crazyflie/mem/ow_element.py +++ b/cflib/crazyflie/mem/ow_element.py @@ -19,7 +19,6 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import array import logging import struct from binascii import crc32 @@ -134,7 +133,7 @@ def write_data(self, write_finished_cb): self._write_finished_cb = write_finished_cb def erase(self, write_finished_cb): - erase_data = array('B', [0xFF] * 112) + erase_data = bytes([0xFF] * 112) self.mem_handler.write(self, 0x00, struct.unpack('B' * len(erase_data), erase_data)) diff --git a/examples/memory/write-ow.py b/examples/memory/write-ow.py index 64e38ba65..ebadd3508 100644 --- a/examples/memory/write-ow.py +++ b/examples/memory/write-ow.py @@ -73,7 +73,7 @@ def _connected(self, link_uri): 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 + # Setting VID:PID to 00:00 will make the Crazyflie match driver to the board name mems[0].vid = 0x00 mems[0].pid = 0x00 From 42e33c7f0957d6ed832258148571f1bd0ab687a7 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 25 Jan 2022 15:18:50 +0100 Subject: [PATCH 369/861] Remove BLE firmware warning from flash-memory.py Acked-by: Kristoffer Richardsson --- examples/memory/flash-memory.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/examples/memory/flash-memory.py b/examples/memory/flash-memory.py index f79a4844d..bd86bf943 100644 --- a/examples/memory/flash-memory.py +++ b/examples/memory/flash-memory.py @@ -131,14 +131,6 @@ def choose(items, title_text, question_text): if __name__ == '__main__': - # Show info about bug 166 - print('\n###\n' - 'Please make sure that your NRF firmware is compiled without\n' - 'BLE support for this to work.\n' - 'See ' - 'https://github.com/bitcraze/crazyflie-clients-python/issues/166\n' - '###\n') - # Initialize the low-level drivers cflib.crtp.init_drivers() From 41562be6d57d7137390e063424e1796e0e1608de Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 08:51:15 +0100 Subject: [PATCH 370/861] menu.yml: Add wireshark debugging to doc menu --- docs/_data/menu.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index 38d7a3b73..0138f49e3 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -14,6 +14,7 @@ - title: Development subs: - {page_id: api_reference} + - {page_id: wireshark_debugging} - {page_id: matlab} - {page_id: eeprom} - {page_id: uart_communication} From abc638a1123759884dfe92952c3d2098a9f4fe7c Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 09:04:06 +0100 Subject: [PATCH 371/861] wireshark.md: Set better title for menu --- docs/wireshark/wireshark.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/wireshark/wireshark.md b/docs/wireshark/wireshark.md index f73c091b8..cf18c8ead 100644 --- a/docs/wireshark/wireshark.md +++ b/docs/wireshark/wireshark.md @@ -1,5 +1,5 @@ --- -title: How to analyze CFLIB CRTP traffic using Wireshark +title: Debugging CRTP using Wireshark page_id: wireshark_debugging --- From 0f393e674f0361a358247dbf47306452951ac99d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 10:56:46 +0100 Subject: [PATCH 372/861] sbs_motion_commander.py: Use Event instead of boolean To avoid magic sleeps --- examples/step-by-step/sbs_motion_commander.py | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index d93507f37..9e3b420cb 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -22,7 +22,9 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie @@ -36,7 +38,7 @@ DEFAULT_HEIGHT = 0.5 BOX_LIMIT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -94,15 +96,13 @@ def log_pos_callback(timestamp, data, logconf): position_estimate[1] = data['stateEstimate.y'] -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') @@ -121,10 +121,13 @@ def param_deck_flow(name, value_str): scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) - # take_off_simple(scf) - # move_linear_simple(scf) - # move_box_limit(scf) - # logconf.stop() + logconf.start() + + # take_off_simple(scf) + # move_linear_simple(scf) + # move_box_limit(scf) + # logconf.stop() From 44811ba0ea71d4532458cd3890634b6ce3bc065d Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 10:57:07 +0100 Subject: [PATCH 373/861] sbs_motion_commander.md: Update to keep in sync --- docs/user-guides/sbs_motion_commander.md | 161 ++++++++++++++--------- 1 file changed, 102 insertions(+), 59 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 75ddcebf7..7d44985e1 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -19,16 +19,22 @@ Since you should have installed cflib in the previous step by step tutorial, you ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper -URI = 'radio://0/80/2M/E7E7E7E7E7' -logging.basicConfig(level=logging.ERROR) +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 if __name__ == '__main__': @@ -58,24 +64,22 @@ We want to know if the deck is correctly attached before flying, therefore we wi Above `__main__`, start a parameter callback function: ```python -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') ``` -The `is_deck_attached` is a global variable which should be defined under `URI`. Note that the value type that the `param_deck_flow()` is a string type, so you will need to convert it to a number first before you can do any operations with it. +The `deck_attached_event` is a global variable which should be defined under `URI`. Note that the value type that the `param_deck_flow()` is a string type, so you will need to convert it to a number first before you can do any operations with it. ```python ... -URI = 'radio://0/80/2M/E7E7E7E7E7' -is_deck_attached = False +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') +deck_attached_event = Event() ``` Try to run the script now, and try to see if it is able to detect that the flowdeck (or any other positioning deck), is correctly attached. Also try to remove it to see if it can detect it missing as well. @@ -83,28 +87,30 @@ Try to run the script now, and try to see if it is able to detect that the flowd This is the full script as we are: ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper -URI = 'radio://0/80/2M/E7E7E7E7E7' +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') @@ -126,17 +132,21 @@ So now we are going to start up the SyncCrazyflie and start a function in the `_ ```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - if is_deck_attached: - take_off_simple(scf) + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + take_off_simple(scf) ``` -See that we are now using `is_deck_attached`? If this is false, the function will not be called and the crazyflie will not take off. +See that we are now using `deck_attached_event.wait()`? If this returns false, the function will not be called and the crazyflie will not take off. Now make the function `take_off_simple(..)` above `__main__`, which will contain the motion commander instance. ```python def take_off_simple(scf): - with MotionCommander(scf) as mc: + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) + mc.stop() ``` If you run the python script, you will see the crazyflie connect and immediately take off. After flying for 3 seconds it will land again. @@ -152,6 +162,7 @@ Change the following line in `take_off_simple(...)`: with MotionCommander(scf) as mc: mc.up(0.3) time.sleep(3) + mc.stop() ``` Run the script again. The crazyflie will first take off to 0.3 meters and then goes up for another 0.3 meters. @@ -172,24 +183,29 @@ Double check if your script is the same as beneath and run it again to check ```python import logging +import sys import time +from threading import Event 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 +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) def take_off_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) + mc.stop() + def param_deck_flow(name, value_str): ... @@ -203,8 +219,11 @@ if __name__ == '__main__': cb=param_deck_flow) time.sleep(1) - if is_deck_attached: - take_off_simple(scf) + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + take_off_simple(scf) ``` @@ -244,19 +263,21 @@ Double check if your code code is still correct: ```python import logging +import sys import time +from threading import Event 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 +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 -is_deck_attached = False - +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -287,8 +308,11 @@ if __name__ == '__main__': cb=param_deck_flow) time.sleep(1) - if is_deck_attached: - move_linear_simple(scf) + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + move_linear_simple(scf) ``` # Step 4: Logging while flying @@ -302,16 +326,18 @@ Let's integrate some logging to this as well. Add the following log config right logconf = LogConfig(name='Position', period_in_ms=10) logconf.add_variable('stateEstimate.x', 'float') logconf.add_variable('stateEstimate.y', 'float') - cf = scf.cf - cf.log.add_config(logconf) + scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) - move_linear_simple(scf) + logconf.start() - logconf.stop() + move_linear_simple(scf) + + logconf.stop() ``` Don't forget to add `from cflib.crazyflie.log import LogConfig` at the imports (we don't need the sync logger since we are going to use the callback). Make the function `log_pos_callback` above param_deck_flow: @@ -330,19 +356,23 @@ Just double check that everything has been implemented correctly and then run th *You can replace the print function in the callback with a plotter if you would like to try that out, like with the python lib matplotlib :)* ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper + +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -387,11 +417,14 @@ if __name__ == '__main__': scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) - move_linear_simple(scf) - logconf.stop() + logconf.start() + + move_linear_simple(scf) + logconf.stop() ``` @@ -439,20 +472,23 @@ You probably also noticed that we are using `mc.start_back()` and `mc.start_forw ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 BOX_LIMIT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -494,19 +530,22 @@ if __name__ == '__main__': scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() - move_box_limit(scf) - logconf.stop() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + logconf.start() + move_box_limit(scf) + logconf.stop() ``` ## Bouncing in a bounding box Let's take it up a notch! Replace the content in the while loop with the following: ```python - body_x_cmd = 0.2; - body_y_cmd = 0.1; - max_vel = 0.2; + body_x_cmd = 0.2 + body_y_cmd = 0.1 + max_vel = 0.2 while (1): if position_estimate[0] > BOX_LIMIT: @@ -531,20 +570,23 @@ This will now start a linear motion into a certain direction, and makes the Craz Check out if your code still matches the full code and run the script! ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 BOX_LIMIT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -590,15 +632,13 @@ def log_pos_callback(timestamp, data, logconf): position_estimate[1] = data['stateEstimate.y'] -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') @@ -617,10 +657,13 @@ if __name__ == '__main__': scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() - move_box_limit(scf) - logconf.stop() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + logconf.start() + move_box_limit(scf) + logconf.stop() ``` You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. From 642dc17e90a3229433d8c0e92ab1f0f29467e977 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 11:00:48 +0100 Subject: [PATCH 374/861] PID_controller_tuner: handle velMax For now treat xVelMax and yVelMax as old xyVelMax. We might want to re-visit this example sometime. --- examples/tuning/PID_controller_tuner.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 5e575402f..c58a8c592 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -152,7 +152,10 @@ def __init__(self, pid_gui, scf): self.cf.param.add_update_callback( group='posCtlPid', name='zKd', cb=self.param_updated_callback_Kd) self.cf.param.add_update_callback( - group='posCtlPid', name='xyVelMax', + group='posCtlPid', name='xVelMax', + cb=self.param_updated_callback_vMax) + self.cf.param.add_update_callback( + group='posCtlPid', name='yVelMax', cb=self.param_updated_callback_vMax) self.current_value_kp = 0 From 15bf0755fd94c0cd288e5db52495092c44462d32 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 15:26:00 +0100 Subject: [PATCH 375/861] build-docs: set -e to detect errors --- tools/build-docs/build-docs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs index 53f9cf578..eaff02538 100755 --- a/tools/build-docs/build-docs +++ b/tools/build-docs/build-docs @@ -1,5 +1,7 @@ #!/usr/bin/env bash +set -e + scriptDir=$(dirname $0) cflibDir=$scriptDir/../../cflib apiRefDir=$scriptDir/../../docs/api From ee2f0d87aa787023ea35dbffb246e8ec87752a89 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 15:33:58 +0100 Subject: [PATCH 376/861] PID_controller_tuner.py: Fix additional xyVelMax usages --- examples/tuning/PID_controller_tuner.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index c58a8c592..449d68639 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -166,7 +166,8 @@ def __init__(self, pid_gui, scf): self.cf.param.request_param_update('posCtlPid.zKp') self.cf.param.request_param_update('posCtlPid.zKi') self.cf.param.request_param_update('posCtlPid.zKd') - self.cf.param.request_param_update('posCtlPid.xyVelMax') + self.cf.param.request_param_update('posCtlPid.xVelMax') + self.cf.param.request_param_update('posCtlPid.yVelMax') time.sleep(0.1) @@ -197,7 +198,8 @@ def send_pid_gains(self): 'Ki', self.pid_gui.scale_Ki.get()) cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + 'Kd', self.pid_gui.scale_Kd.get()) - cf.param.set_value('posCtlPid.xyVelMax', self.pid_gui.scale_vMax.get()) + cf.param.set_value('posCtlPid.xVelMax', self.pid_gui.scale_vMax.get()) + cf.param.set_value('posCtlPid.yVelMax', self.pid_gui.scale_vMax.get()) time.sleep(0.1) From 36de7a9e1814ee305c85a413e1bc59e66cdbddde Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 16:46:52 +0100 Subject: [PATCH 377/861] build-docs: Explicity set PYTHONPATH To find --user installed modules. --- tools/build-docs/build-docs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs index eaff02538..f9a2103bb 100755 --- a/tools/build-docs/build-docs +++ b/tools/build-docs/build-docs @@ -2,6 +2,9 @@ set -e +# This hack is needed to find --user installed modules +export PYTHONPATH=$PYTHONPATH:$scriptDir/.local/lib/python3.7/site-packages + scriptDir=$(dirname $0) cflibDir=$scriptDir/../../cflib apiRefDir=$scriptDir/../../docs/api @@ -22,7 +25,7 @@ mkdir -p $tempDir # The rewriting of HOME is a hack to make this work in a bare-bone Docker # environment. # -pip3 show cflib > /dev/null 2>&1 || { # this will run if cflib is not found +python3 -m pip show cflib || { # this will run if cflib is not found HOME=$scriptDir pip3 install --user --upgrade pip HOME=$scriptDir pip3 install --user -e $scriptDir/../../ } From 2c54358b1d79c4e892463328021f3cef269effb0 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 26 Jan 2022 16:49:18 +0100 Subject: [PATCH 378/861] build-docs: fix typo in --- tools/build-docs/build-docs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs index f9a2103bb..6ca7a555f 100755 --- a/tools/build-docs/build-docs +++ b/tools/build-docs/build-docs @@ -2,9 +2,6 @@ set -e -# This hack is needed to find --user installed modules -export PYTHONPATH=$PYTHONPATH:$scriptDir/.local/lib/python3.7/site-packages - scriptDir=$(dirname $0) cflibDir=$scriptDir/../../cflib apiRefDir=$scriptDir/../../docs/api @@ -14,6 +11,9 @@ tempDir=$scriptDir/.tmp mkdir -p $apiRefDir mkdir -p $tempDir +# This hack is needed to find --user installed modules +export PYTHONPATH=$PYTHONPATH:$scriptDir/.local/lib/python3.7/site-packages + [ -n "$API_REF_BASE" ] || { export API_REF_BASE=$(readlink -fn $tempDir) } From 5213c5f67ebf1ae747bb618187a668dc77384d07 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 27 Jan 2022 15:13:41 +0100 Subject: [PATCH 379/861] some wiki links still hanging around that are removed --- cflib/drivers/crazyradio.py | 3 ++- docs/installation/install.md | 2 +- setup_linux.sh | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index a96def4ce..0599adac7 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -49,7 +49,8 @@ CRADIO_PID = 0x7777 # Dongle configuration requests -# See http://wiki.bitcraze.se/projects:crazyradio:protocol for documentation +# See https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/functional-areas/usb_radio_protocol/ +# for documentation SET_RADIO_CHANNEL = 0x01 SET_RADIO_ADDRESS = 0x02 SET_DATA_RATE = 0x03 diff --git a/docs/installation/install.md b/docs/installation/install.md index ad37fceca..22c7fbfd4 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -53,7 +53,7 @@ This will run the lint checkers defined in `.pre-commit-config-yaml` on your pro ### With docker and the toolbelt For information and installation of the -[toolbelt.](https://wiki.bitcraze.io/projects:dockerbuilderimage:index) +[toolbelt.](https://github.com/bitcraze/toolbelt) * Check to see if you pass tests: `tb test` * Check to see if you pass style guidelines: `tb verify` diff --git a/setup_linux.sh b/setup_linux.sh index 9ea51d5f6..07fc9e10b 100755 --- a/setup_linux.sh +++ b/setup_linux.sh @@ -3,8 +3,8 @@ # by the current user immediately # Caution! This installs the Crazyflie PC client as root to your Python # site-packages directory. If you wish to install it as a normal user, use the -# instructions on the Wiki at -# http://wiki.bitcraze.se/projects:crazyflie:pc_utils:install +# instructions in the documentation at +# https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/ # After installation, the Crazyflie PC client can be started with `cfclient`. # @author Daniel Lee 2013 From 792bc431371b4492c3a37ef2281c3e2c38610b28 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 27 Jan 2022 15:21:41 +0100 Subject: [PATCH 380/861] fix autopep8 --- cflib/drivers/crazyradio.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/drivers/crazyradio.py b/cflib/drivers/crazyradio.py index 0599adac7..6c83c2788 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -49,7 +49,7 @@ CRADIO_PID = 0x7777 # Dongle configuration requests -# See https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/functional-areas/usb_radio_protocol/ +# See https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/functional-areas/usb_radio_protocol/ # for documentation SET_RADIO_CHANNEL = 0x01 SET_RADIO_ADDRESS = 0x02 From 3e03e9d588354cd320368373eb59107ac2dad834 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 27 Jan 2022 16:03:40 +0100 Subject: [PATCH 381/861] Update version to 0.1.17.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index cea1dc1ca..bdf09eea3 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.17', + version='0.1.17.1', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 2129d43d00113f9a0e5a31ef2c00611daebd6e3b Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 10 Feb 2022 09:53:17 +0100 Subject: [PATCH 382/861] Bootloader: Make flash progress_cb ascii friendly --- cflib/bootloader/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index be93fc837..d66daccdc 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -441,7 +441,7 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): progress_cb = self.progress_cb if not progress_cb: def progress_cb(msg: str, percent: int): - frames = ['◢', '◣', '◤', '◥'] + frames = ['|', '/', '-', '\\'] frame = frames[int(percent) % 4] print('{} {}% {}'.format(frame, percent, msg)) From 2391446020b3a8fc45694061c72cc3fd1f846ab3 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Thu, 10 Feb 2022 09:51:44 +0100 Subject: [PATCH 383/861] wireshark: Add toc handling to parameters This adds dissection of the toc channel of the parameter port which enables us to know the group and name for parameter reads and writes. You can now filter on the name of a group or a name and follow it through your session. And also check the type of the parameter. --- tools/crtp-dissector.lua | 115 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 110 insertions(+), 5 deletions(-) diff --git a/tools/crtp-dissector.lua b/tools/crtp-dissector.lua index bdd161e9c..2f176f503 100644 --- a/tools/crtp-dissector.lua +++ b/tools/crtp-dissector.lua @@ -10,7 +10,11 @@ local f_crtp_undecoded = ProtoField.string("crtp.undecoded", "Undecoded") -- Specialized CRTP service fields local f_crtp_console_text = ProtoField.string("crtp.console_text", "Text", base.ASCII) local f_crtp_parameter_varid = ProtoField.uint16("crtp.parameter_varid", "Variable Id") - +local f_crtp_parameter_type = ProtoField.string("crtp.parameter_type", "Parameter Type") +local f_crtp_parameter_group = ProtoField.string("crtp.parameter_group", "Parameter Group") +local f_crtp_parameter_name = ProtoField.string("crtp.parameter_name", "Parameter Name") +local f_crtp_parameter_count = ProtoField.uint16("crtp.parameter_count", "Parameter Count") +local f_crtp_parameter_crc = ProtoField.string("crtp.parameter_crc", "Parameter CRC") local f_crtp_parameter_val_uint = ProtoField.uint32("crtp.parameter_val_uint", "Value uint") local f_crtp_parameter_val_int = ProtoField.int32("crtp.parameter_val_int", "Value int") local f_crtp_parameter_val_float = ProtoField.float("crtp.parameter_val_float", "Value float") @@ -38,6 +42,8 @@ crtp.fields = { f_crtp_port, f_crtp_channel, f_crtp_console_text, f_crtp_size, f_crtp_parameter_varid, f_crtp_parameter_val_uint, f_crtp_parameter_val_int, f_crtp_parameter_val_float, + f_crtp_parameter_name, f_crtp_parameter_group, f_crtp_parameter_type, + f_crtp_parameter_count, f_crtp_parameter_crc, f_crtp_setpoint_hl_command, f_crtp_setpoint_hl_retval, f_crtp_setpoint_hl_use_yaw, f_crtp_setpoint_hl_yaw, f_crtp_setpoint_hl_groupmask, f_crtp_setpoint_hl_duration, @@ -46,6 +52,8 @@ crtp.fields = { f_crtp_setpoint_hl_id, f_crtp_setpoint_hl_timescale, f_crtp_undecoded } +local param_toc = {} + local Links = { UNKNOWN = 0, RADIO = 1, @@ -340,8 +348,62 @@ function handle_setpoint_highlevel(tree, receive, buffer, channel, size) end end -function handle_parameter_port(tree, buffer, channel, size) - -- Read or Write +function append_param_type(str, type) + if #str == 0 then + return type + else + return str .. " | " .. type + end +end + +function get_param_types(byte) + type = "" + + local ext = bit.lshift(1, 4) + local core = bit.lshift(1, 5) + local ronly = bit.lshift(1, 6) + local group = bit.lshift(1, 7) + + num_type = bit.band(byte, 0x0F) + if num_type == 0 then + type = "PARAM_INT8" + elseif num_type == bit.lshift(0x1, 3) then + type = "PARAM_UINT8" + elseif num_type == bit.bor(0x1, bit.lshift(0x1, 3)) then + type = "PARAM_UINT16" + elseif num_type == 0x1 then + type = "PARAM_INT16" + elseif num_type == 0x2 then + type = "PARAM_INT32" + elseif num_type == bit.bor(0x2, bit.lshift(0x1, 3)) then + type = "PARAM_UINT32" + elseif num_type == bit.bor(0x2, bit.lshift(0x1, 2)) then + type = "PARAM_FLOAT" + end + + if bit.band(byte, core) ~= 0 then + type = append_param_type(type, "PARAM_CORE") + end + + if bit.band(byte, ronly) ~= 0 then + type = append_param_type(type, "PARAM_RONLY") + end + + if bit.band(byte, group) ~= 0 then + type = append_param_type(type, "PARAM_GROUP") + end + + if bit.band(byte, ext) ~= 0 then + type = append_param_type(type, "PARAM_EXTENDED") + end + + return byte .. " (" .. type .. ")" +end + +function handle_parameter_port(tree, receive, buffer, channel, size) + local port_tree = tree:add(crtp, port_name) + + -- Read or Write if (channel == 1 or channel == 2) and size > 2 then if channel == 1 then @@ -352,9 +414,12 @@ function handle_parameter_port(tree, buffer, channel, size) -- Add variable id local var_id = buffer(crtp_start + 1, 2):le_uint() - local port_tree = tree:add(crtp, port_name) port_tree:add_le(f_crtp_parameter_varid, var_id) + item = param_toc[var_id] + port_tree:add_le(f_crtp_parameter_group, item["group"]) + port_tree:add_le(f_crtp_parameter_name, item["name"]) + -- Add value if size > 3 then port_tree:add_le(f_crtp_parameter_val_uint, buffer(crtp_start + value_start):le_uint()) @@ -365,6 +430,46 @@ function handle_parameter_port(tree, buffer, channel, size) end undecoded = 0 end + -- TOC + if channel == 0 then + message_id = buffer(crtp_start + 1, 1):le_uint() + if message_id == 3 and receive == 1 then + port_tree:add_le(f_crtp_parameter_count, buffer(crtp_start + 2, 2):le_uint()) + port_tree:add_le(f_crtp_parameter_crc, buffer(crtp_start + 4):bytes():tohex()) + end + if message_id == 2 then + -- GET_ITEM + item = {} + item["varid"] = buffer(crtp_start + 2, 2):le_uint() + port_tree:add_le(f_crtp_parameter_varid, item["varid"]) + + if receive == 1 then + item["type"] = get_param_types(buffer(crtp_start + 4, 1):le_uint()) + item["group"] = "" + item["name"] = "" + full_name = buffer(crtp_start + 5):string() + + stored_group = false + for i = 1, #full_name do + local c = full_name:sub(i, i) + if string.byte(c) == 0 and not stored_group then + stored_group = true + else + if stored_group then + item["name"] = item["name"] .. c + else + item["group"] = item["group"] .. c + end + end + end + + param_toc[item["varid"]] = item + port_tree:add_le(f_crtp_parameter_type, item["type"]) + port_tree:add_le(f_crtp_parameter_group, item["group"]) + port_tree:add_le(f_crtp_parameter_name, item["name"]) + end + end + end end -- create a function to dissect it, layout: @@ -434,7 +539,7 @@ function crtp.dissector(buffer, pinfo, tree) end if crtp_port == Ports.Parameters then - handle_parameter_port(tree, buffer, crtp_channel, crtp_size) + handle_parameter_port(tree, receive, buffer, crtp_channel, crtp_size) end if crtp_port == Ports.Setpoint_Highlevel then From 6a95884ea1054a839331bc86806da1f7ee0dc1a1 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 11 Feb 2022 06:53:54 +0100 Subject: [PATCH 384/861] wireshark: run formatter on LUA code --- tools/crtp-dissector.lua | 970 ++++++++++++++++++++------------------- 1 file changed, 501 insertions(+), 469 deletions(-) diff --git a/tools/crtp-dissector.lua b/tools/crtp-dissector.lua index 2f176f503..e0ec9faee 100644 --- a/tools/crtp-dissector.lua +++ b/tools/crtp-dissector.lua @@ -1,5 +1,5 @@ -- declare our protocol -crtp = Proto("CrazyRealTimeTProtocol","Crazy Real Time Protocol") +crtp = Proto("CrazyRealTimeTProtocol", "Crazy Real Time Protocol") -- General CRTP packet fields local f_crtp_port = ProtoField.uint8("crtp.port", "Port") @@ -36,28 +36,43 @@ local f_crtp_setpoint_hl_x = ProtoField.float("crtp.setpoint_hl_x", "X") local f_crtp_setpoint_hl_y = ProtoField.float("crtp.setpoint_hl_y", "Y") local f_crtp_setpoint_hl_z = ProtoField.float("crtp.setpoint_hl_z", "Z") - -- All possible fields registred crtp.fields = { - f_crtp_port, f_crtp_channel, f_crtp_console_text, f_crtp_size, - f_crtp_parameter_varid, f_crtp_parameter_val_uint, - f_crtp_parameter_val_int, f_crtp_parameter_val_float, - f_crtp_parameter_name, f_crtp_parameter_group, f_crtp_parameter_type, - f_crtp_parameter_count, f_crtp_parameter_crc, - f_crtp_setpoint_hl_command, f_crtp_setpoint_hl_retval, - f_crtp_setpoint_hl_use_yaw, f_crtp_setpoint_hl_yaw, - f_crtp_setpoint_hl_groupmask, f_crtp_setpoint_hl_duration, - f_crtp_setpoint_hl_height, f_crtp_setpoint_hl_relative, - f_crtp_setpoint_hl_x, f_crtp_setpoint_hl_y, f_crtp_setpoint_hl_z, - f_crtp_setpoint_hl_id, f_crtp_setpoint_hl_timescale, f_crtp_undecoded + f_crtp_port, + f_crtp_channel, + f_crtp_console_text, + f_crtp_size, + f_crtp_parameter_varid, + f_crtp_parameter_val_uint, + f_crtp_parameter_val_int, + f_crtp_parameter_val_float, + f_crtp_parameter_name, + f_crtp_parameter_group, + f_crtp_parameter_type, + f_crtp_parameter_count, + f_crtp_parameter_crc, + f_crtp_setpoint_hl_command, + f_crtp_setpoint_hl_retval, + f_crtp_setpoint_hl_use_yaw, + f_crtp_setpoint_hl_yaw, + f_crtp_setpoint_hl_groupmask, + f_crtp_setpoint_hl_duration, + f_crtp_setpoint_hl_height, + f_crtp_setpoint_hl_relative, + f_crtp_setpoint_hl_x, + f_crtp_setpoint_hl_y, + f_crtp_setpoint_hl_z, + f_crtp_setpoint_hl_id, + f_crtp_setpoint_hl_timescale, + f_crtp_undecoded, } local param_toc = {} local Links = { - UNKNOWN = 0, - RADIO = 1, - USB = 2, + UNKNOWN = 0, + RADIO = 1, + USB = 2, } local link = Links.UNKNOWN @@ -68,488 +83,505 @@ local crtp_start = 0 -- we are dealing with local Ports = { - Console = 0x0, - Parameters = 0x2, - Commander = 0x3, - Memory = 0x4, - Logging = 0x5, - Localization = 0x6, - Commander_Generic = 0x7, - Setpoint_Highlevel = 0x8, - Platform = 0xD, - All = 0xF + Console = 0x0, + Parameters = 0x2, + Commander = 0x3, + Memory = 0x4, + Logging = 0x5, + Localization = 0x6, + Commander_Generic = 0x7, + Setpoint_Highlevel = 0x8, + Platform = 0xD, + All = 0xF, } function get_crtp_port_channel_names(port, channel) - local port_name = "Unknown" - local channel_name = nil - - if port == Ports.Console and channel == 0 then port_name = "Console" - - elseif port == 0x02 then - port_name = "Parameters" - if channel == 0 then channel_name = "Table Of Contents" - elseif channel == 1 then channel_name = "Read" - elseif channel == 2 then channel_name = "Write" - elseif channel == 3 then channel_name = "Misc" - end - - elseif port == 0x03 then port_name = "Commander" - - elseif port == 0x04 then - port_name = "Memory" - if channel == 0 then channel_name = "Information" - elseif channel == 1 then channel_name = "Read" - elseif channel == 2 then channel_name = "Write" - end - - elseif port == 0x05 then - port_name = "Logging" - if channel == 0 then channel_name = "Table Of Contents" - elseif channel == 1 then channel_name = "Settings" - elseif channel == 2 then channel_name = "Log data" - end - - elseif port == 0x06 then port_name = "Localization" - if channel == 0 then channel_name = "Position" - elseif channel == 1 then channel_name = "Generic" - end - - elseif port == 0x07 then port_name = "Commander Generic" - elseif port == 0x08 then port_name = "Setpoint Highlevel" - elseif port == 0x0D then - port_name = "Platform" - if channel == 0 then channel_name = "Platform Command" - elseif channel == 1 then channel_name = "Version Command" - elseif channel == 2 then channel_name = "App Layer" - end - - elseif port == 0x0F then - port_name = "Link Control" - if channel == 1 then channel_name = "Link Service Source" - end - - elseif port == 0xFF then port_name = "ALL" end - - return port_name, channel_name + local port_name = "Unknown" + local channel_name = nil + + if port == Ports.Console and channel == 0 then + port_name = "Console" + elseif port == 0x02 then + port_name = "Parameters" + if channel == 0 then + channel_name = "Table Of Contents" + elseif channel == 1 then + channel_name = "Read" + elseif channel == 2 then + channel_name = "Write" + elseif channel == 3 then + channel_name = "Misc" + end + elseif port == 0x03 then + port_name = "Commander" + elseif port == 0x04 then + port_name = "Memory" + if channel == 0 then + channel_name = "Information" + elseif channel == 1 then + channel_name = "Read" + elseif channel == 2 then + channel_name = "Write" + end + elseif port == 0x05 then + port_name = "Logging" + if channel == 0 then + channel_name = "Table Of Contents" + elseif channel == 1 then + channel_name = "Settings" + elseif channel == 2 then + channel_name = "Log data" + end + elseif port == 0x06 then + port_name = "Localization" + if channel == 0 then + channel_name = "Position" + elseif channel == 1 then + channel_name = "Generic" + end + elseif port == 0x07 then + port_name = "Commander Generic" + elseif port == 0x08 then + port_name = "Setpoint Highlevel" + elseif port == 0x0D then + port_name = "Platform" + if channel == 0 then + channel_name = "Platform Command" + elseif channel == 1 then + channel_name = "Version Command" + elseif channel == 2 then + channel_name = "App Layer" + end + elseif port == 0x0F then + port_name = "Link Control" + if channel == 1 then + channel_name = "Link Service Source" + end + elseif port == 0xFF then + port_name = "ALL" + end + + return port_name, channel_name end function format_address(buffer) - if link == Links.RADIO then - addr = buffer(2, 5):bytes():tohex() - port = buffer(7, 1):uint() + if link == Links.RADIO then + addr = buffer(2, 5):bytes():tohex() + port = buffer(7, 1):uint() - addr = addr:gsub("..", ":%0"):sub(2) - port = tostring(port) + addr = addr:gsub("..", ":%0"):sub(2) + port = tostring(port) - return addr .. " (" .. port .. ")" - elseif link == Links.USB then - return buffer(2, 12):bytes():tohex() - end + return addr .. " (" .. port .. ")" + elseif link == Links.USB then + return buffer(2, 12):bytes():tohex() + end end function format_device(buffer) - if link == Links.RADIO then - devid = buffer(8, 1):uint() - return "Radio #" .. tostring(devid) - elseif link == Links.USB then - devid = buffer(15, 1):uint() - return "USB #" .. tostring(devid) - end - + if link == Links.RADIO then + devid = buffer(8, 1):uint() + return "Radio #" .. tostring(devid) + elseif link == Links.USB then + devid = buffer(15, 1):uint() + return "USB #" .. tostring(devid) + end end function handle_setpoint_highlevel(tree, receive, buffer, channel, size) - local Commands = { - COMMAND_SET_GROUP_MASK = 0, - COMMAND_TAKEOFF = 1, - COMMAND_LAND = 2, - COMMAND_STOP = 3, - COMMAND_GO_TO = 4, - COMMAND_START_TRAJECTORY = 5, - COMMAND_DEFINE_TRAJECTORY = 6, - COMMAND_TAKEOFF_2 = 7, - COMMAND_LAND_2 = 8, - COMMAND_TAKEOFF_WITH_VELOCITY = 9, - COMMAND_LAND_WITH_VELOCITY = 10, - } - - local height = nil - local duration = nil - local yaw = nil - local group_mask = nil - local use_yaw = nil - local relative = nil - local x = nil - local y = nil - local z = nil - local id = nil - local timescale = nil - - local port_tree = tree:add(crtp, port_name) - - local cmd = buffer(crtp_start + 1, 1):uint() - local cmd_str = "Unknown" - - if cmd == Commands.COMMAND_SET_GROUP_MASK then - cmd_str = "Set Group Mask" - - -- struct data_set_group_mask { - -- uint8_t groupMask; // mask for which CFs this should apply to - -- } __attribute__((packed)); - if receive == 0 then - group_mask = buffer(11, 1):uint() - end - elseif cmd == Commands.COMMAND_TAKEOFF then cmd_str = "Take Off (deprecated)" - elseif cmd == Commands.COMMAND_LAND then cmd_str = "Land (deprecated)" - elseif cmd == Commands.COMMAND_STOP then - cmd_str = "Stop" - - -- struct data_stop { - -- uint8_t groupMask; // mask for which CFs this should apply to - -- } __attribute__((packed)); - if receive == 0 then - group_mask = buffer(crtp_start + 2, 1):uint() - end - undecoded = undecoded - 1 - elseif cmd == Commands.COMMAND_GO_TO then - cmd_str = "Go To" - - -- struct data_go_to { - -- uint8_t groupMask; // mask for which CFs this should apply to - -- uint8_t relative; // set to true, if position/yaw are relative to current setpoint - -- float x; // m - -- float y; // m - -- float z; // m - -- float yaw; // rad - -- float duration; // sec - -- } __attribute__((packed)); - if receive == 0 then - group_mask = buffer(crtp_start + 2, 1):uint() - relative = buffer(crtp_start + 3, 1):uint() - x = buffer(crtp_start + 4, 4):le_float() - y = buffer(crtp_start + 8, 4):le_float() - z = buffer(crtp_start + 12, 4):le_float() - yaw = buffer(crtp_start + 16, 4):le_float() - duration = buffer(crtp_start + 20, 4):le_float() - undecoded = undecoded - 24 - - end - elseif cmd == Commands.COMMAND_START_TRAJECTORY then - cmd_str = "Start Trajectory" - - -- struct data_start_trajectory { - -- uint8_t groupMask; // mask for which CFs this should apply to - -- uint8_t relative; // set to true, if trajectory should be shifted to current setpoint - -- uint8_t reversed; // set to true, if trajectory should be executed in reverse - -- uint8_t trajectoryId; // id of the trajectory (previously defined by COMMAND_DEFINE_TRAJECTORY) - -- float timescale; // time factor; 1 = original speed; >1: slower; <1: faster - -- } __attribute__((packed)); - if receive == 0 then - group_mask = buffer(crtp_start + 2, 1):uint() - relative = buffer(crtp_start + 3, 1):uint() - reversed = buffer(crtp_start + 4, 1):uint() - id = buffer(crtp_start + 5, 1):uint() - timescale = buffer(crtp_start + 6):float() - undecoded = undecoded - 10 - end - elseif cmd == Commands.COMMAND_DEFINE_TRAJECTORY then - cmd_str = "Define Trajectory" - - -- struct data_define_trajectory { - -- uint8_t trajectoryId; - -- struct trajectoryDescription description; - -- } __attribute__((packed)); - if receive == 0 then - id = buffer(crtp_start + 2, 1):uint() - undecoded = undecoded - 1 - end - elseif cmd == Commands.COMMAND_TAKEOFF_2 then - cmd_str = "Take Off" - - -- struct data_takeoff_2 { - -- uint8_t groupMask; // mask for which CFs this should apply to - -- float height; // m (absolute) - -- float yaw; // rad - -- bool useCurrentYaw; // If true, use the current yaw (ignore the yaw parameter) - -- float duration; // s (time it should take until target height is reached) - -- } __attribute__((packed)); - if receive == 0 then - group_mask = buffer(crtp_start + 2, 1):uint() - height = buffer(crtp_start + 3, 4):le_float() - yaw = buffer(crtp_start + 7, 4):le_float() - use_yaw = buffer(crtp_start + 11, 1):uint() - duration = buffer(crtp_start + 12, 4):le_float() - undecoded = undecoded - 16 - end - - elseif cmd == Commands.COMMAND_LAND_2 then - cmd_str = "Land" - -- struct data_land_2 { - -- uint8_t groupMask; // mask for which CFs this should apply to - -- float height; // m (absolute) - -- float yaw; // rad - -- bool useCurrentYaw; // If true, use the current yaw (ignore the yaw parameter) - -- float duration; // s (time it should take until target height is reached) - -- } __attribute__((packed)); - if receive == 0 then - group_mask = buffer(crtp_start + 2, 1):uint() - height = buffer(crtp_start + 3, 4):le_float() - yaw = buffer(crtp_start + 7, 4):le_float() - use_yaw = buffer(crtp_start + 11, 1):uint() - duration = buffer(crtp_start + 12, 4):le_float() - undecoded = undecoded - 16 - end - - elseif cmd == Commands.COMMAND_TAKEOFF_WITH_VELOCITY then cmd_str = "Take Off With Velocity" - elseif cmd == Commands.COMMAND_LAND_WITH_VELOCITY then cmd_str = "Land With Velocity" end - - port_tree:add_le(f_crtp_setpoint_hl_command, cmd_str) - local success = true - if receive == 1 then - retval = buffer(crtp_start + 4):uint() - local success = (retval == 0) and "Success" or "Failure" - port_tree:add_le(f_crtp_setpoint_hl_retval, retval):append_text(" (" .. success .. ")") - undecoded = undecoded - 4 - end - - if id then - port_tree:add_le(f_crtp_setpoint_hl_id, id) - end - if timescale then - port_tree:add_le(f_crtp_setpoint_hl_timescale, timescale) - end - if group_mask then - port_tree:add_le(f_crtp_setpoint_hl_groupmask, group_mask) - end - if relative then - port_tree:add_le(f_crtp_setpoint_hl_relative, relative) - end - if height then - port_tree:add_le(f_crtp_setpoint_hl_height, height):append_text(" (m)") - end - if x then - port_tree:add_le(f_crtp_setpoint_hl_x, x) - end - if y then - port_tree:add_le(f_crtp_setpoint_hl_y, y) - end - if z then - port_tree:add_le(f_crtp_setpoint_hl_z, z) - end - if yaw then - port_tree:add_le(f_crtp_setpoint_hl_yaw, yaw) - end - if use_yaw then - port_tree:add_le(f_crtp_setpoint_hl_use_yaw, use_yaw) - end - if duration then - port_tree:add_le(f_crtp_setpoint_hl_duration, duration):append_text(" (s)") - end + local Commands = { + COMMAND_SET_GROUP_MASK = 0, + COMMAND_TAKEOFF = 1, + COMMAND_LAND = 2, + COMMAND_STOP = 3, + COMMAND_GO_TO = 4, + COMMAND_START_TRAJECTORY = 5, + COMMAND_DEFINE_TRAJECTORY = 6, + COMMAND_TAKEOFF_2 = 7, + COMMAND_LAND_2 = 8, + COMMAND_TAKEOFF_WITH_VELOCITY = 9, + COMMAND_LAND_WITH_VELOCITY = 10, + } + + local height = nil + local duration = nil + local yaw = nil + local group_mask = nil + local use_yaw = nil + local relative = nil + local x = nil + local y = nil + local z = nil + local id = nil + local timescale = nil + + local port_tree = tree:add(crtp, port_name) + + local cmd = buffer(crtp_start + 1, 1):uint() + local cmd_str = "Unknown" + + if cmd == Commands.COMMAND_SET_GROUP_MASK then + cmd_str = "Set Group Mask" + + -- struct data_set_group_mask { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(11, 1):uint() + end + elseif cmd == Commands.COMMAND_TAKEOFF then + cmd_str = "Take Off (deprecated)" + elseif cmd == Commands.COMMAND_LAND then + cmd_str = "Land (deprecated)" + elseif cmd == Commands.COMMAND_STOP then + cmd_str = "Stop" + + -- struct data_stop { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(crtp_start + 2, 1):uint() + end + undecoded = undecoded - 1 + elseif cmd == Commands.COMMAND_GO_TO then + cmd_str = "Go To" + + -- struct data_go_to { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- uint8_t relative; // set to true, if position/yaw are relative to current setpoint + -- float x; // m + -- float y; // m + -- float z; // m + -- float yaw; // rad + -- float duration; // sec + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(crtp_start + 2, 1):uint() + relative = buffer(crtp_start + 3, 1):uint() + x = buffer(crtp_start + 4, 4):le_float() + y = buffer(crtp_start + 8, 4):le_float() + z = buffer(crtp_start + 12, 4):le_float() + yaw = buffer(crtp_start + 16, 4):le_float() + duration = buffer(crtp_start + 20, 4):le_float() + undecoded = undecoded - 24 + end + elseif cmd == Commands.COMMAND_START_TRAJECTORY then + cmd_str = "Start Trajectory" + + -- struct data_start_trajectory { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- uint8_t relative; // set to true, if trajectory should be shifted to current setpoint + -- uint8_t reversed; // set to true, if trajectory should be executed in reverse + -- uint8_t trajectoryId; // id of the trajectory (previously defined by COMMAND_DEFINE_TRAJECTORY) + -- float timescale; // time factor; 1 = original speed; >1: slower; <1: faster + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(crtp_start + 2, 1):uint() + relative = buffer(crtp_start + 3, 1):uint() + reversed = buffer(crtp_start + 4, 1):uint() + id = buffer(crtp_start + 5, 1):uint() + timescale = buffer(crtp_start + 6):float() + undecoded = undecoded - 10 + end + elseif cmd == Commands.COMMAND_DEFINE_TRAJECTORY then + cmd_str = "Define Trajectory" + + -- struct data_define_trajectory { + -- uint8_t trajectoryId; + -- struct trajectoryDescription description; + -- } __attribute__((packed)); + if receive == 0 then + id = buffer(crtp_start + 2, 1):uint() + undecoded = undecoded - 1 + end + elseif cmd == Commands.COMMAND_TAKEOFF_2 then + cmd_str = "Take Off" + + -- struct data_takeoff_2 { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- float height; // m (absolute) + -- float yaw; // rad + -- bool useCurrentYaw; // If true, use the current yaw (ignore the yaw parameter) + -- float duration; // s (time it should take until target height is reached) + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(crtp_start + 2, 1):uint() + height = buffer(crtp_start + 3, 4):le_float() + yaw = buffer(crtp_start + 7, 4):le_float() + use_yaw = buffer(crtp_start + 11, 1):uint() + duration = buffer(crtp_start + 12, 4):le_float() + undecoded = undecoded - 16 + end + elseif cmd == Commands.COMMAND_LAND_2 then + cmd_str = "Land" + -- struct data_land_2 { + -- uint8_t groupMask; // mask for which CFs this should apply to + -- float height; // m (absolute) + -- float yaw; // rad + -- bool useCurrentYaw; // If true, use the current yaw (ignore the yaw parameter) + -- float duration; // s (time it should take until target height is reached) + -- } __attribute__((packed)); + if receive == 0 then + group_mask = buffer(crtp_start + 2, 1):uint() + height = buffer(crtp_start + 3, 4):le_float() + yaw = buffer(crtp_start + 7, 4):le_float() + use_yaw = buffer(crtp_start + 11, 1):uint() + duration = buffer(crtp_start + 12, 4):le_float() + undecoded = undecoded - 16 + end + elseif cmd == Commands.COMMAND_TAKEOFF_WITH_VELOCITY then + cmd_str = "Take Off With Velocity" + elseif cmd == Commands.COMMAND_LAND_WITH_VELOCITY then + cmd_str = "Land With Velocity" + end + + port_tree:add_le(f_crtp_setpoint_hl_command, cmd_str) + local success = true + if receive == 1 then + retval = buffer(crtp_start + 4):uint() + local success = (retval == 0) and "Success" or "Failure" + port_tree:add_le(f_crtp_setpoint_hl_retval, retval):append_text(" (" .. success .. ")") + undecoded = undecoded - 4 + end + + if id then + port_tree:add_le(f_crtp_setpoint_hl_id, id) + end + if timescale then + port_tree:add_le(f_crtp_setpoint_hl_timescale, timescale) + end + if group_mask then + port_tree:add_le(f_crtp_setpoint_hl_groupmask, group_mask) + end + if relative then + port_tree:add_le(f_crtp_setpoint_hl_relative, relative) + end + if height then + port_tree:add_le(f_crtp_setpoint_hl_height, height):append_text(" (m)") + end + if x then + port_tree:add_le(f_crtp_setpoint_hl_x, x) + end + if y then + port_tree:add_le(f_crtp_setpoint_hl_y, y) + end + if z then + port_tree:add_le(f_crtp_setpoint_hl_z, z) + end + if yaw then + port_tree:add_le(f_crtp_setpoint_hl_yaw, yaw) + end + if use_yaw then + port_tree:add_le(f_crtp_setpoint_hl_use_yaw, use_yaw) + end + if duration then + port_tree:add_le(f_crtp_setpoint_hl_duration, duration):append_text(" (s)") + end end function append_param_type(str, type) - if #str == 0 then - return type - else - return str .. " | " .. type - end + if #str == 0 then + return type + else + return str .. " | " .. type + end end function get_param_types(byte) - type = "" - - local ext = bit.lshift(1, 4) - local core = bit.lshift(1, 5) - local ronly = bit.lshift(1, 6) - local group = bit.lshift(1, 7) - - num_type = bit.band(byte, 0x0F) - if num_type == 0 then - type = "PARAM_INT8" - elseif num_type == bit.lshift(0x1, 3) then - type = "PARAM_UINT8" - elseif num_type == bit.bor(0x1, bit.lshift(0x1, 3)) then - type = "PARAM_UINT16" - elseif num_type == 0x1 then - type = "PARAM_INT16" - elseif num_type == 0x2 then - type = "PARAM_INT32" - elseif num_type == bit.bor(0x2, bit.lshift(0x1, 3)) then - type = "PARAM_UINT32" - elseif num_type == bit.bor(0x2, bit.lshift(0x1, 2)) then - type = "PARAM_FLOAT" - end - - if bit.band(byte, core) ~= 0 then - type = append_param_type(type, "PARAM_CORE") - end - - if bit.band(byte, ronly) ~= 0 then - type = append_param_type(type, "PARAM_RONLY") - end - - if bit.band(byte, group) ~= 0 then - type = append_param_type(type, "PARAM_GROUP") - end - - if bit.band(byte, ext) ~= 0 then - type = append_param_type(type, "PARAM_EXTENDED") - end - - return byte .. " (" .. type .. ")" + type = "" + + local ext = bit.lshift(1, 4) + local core = bit.lshift(1, 5) + local ronly = bit.lshift(1, 6) + local group = bit.lshift(1, 7) + + num_type = bit.band(byte, 0x0F) + if num_type == 0 then + type = "PARAM_INT8" + elseif num_type == bit.lshift(0x1, 3) then + type = "PARAM_UINT8" + elseif num_type == bit.bor(0x1, bit.lshift(0x1, 3)) then + type = "PARAM_UINT16" + elseif num_type == 0x1 then + type = "PARAM_INT16" + elseif num_type == 0x2 then + type = "PARAM_INT32" + elseif num_type == bit.bor(0x2, bit.lshift(0x1, 3)) then + type = "PARAM_UINT32" + elseif num_type == bit.bor(0x2, bit.lshift(0x1, 2)) then + type = "PARAM_FLOAT" + end + + if bit.band(byte, core) ~= 0 then + type = append_param_type(type, "PARAM_CORE") + end + + if bit.band(byte, ronly) ~= 0 then + type = append_param_type(type, "PARAM_RONLY") + end + + if bit.band(byte, group) ~= 0 then + type = append_param_type(type, "PARAM_GROUP") + end + + if bit.band(byte, ext) ~= 0 then + type = append_param_type(type, "PARAM_EXTENDED") + end + + return byte .. " (" .. type .. ")" end function handle_parameter_port(tree, receive, buffer, channel, size) - local port_tree = tree:add(crtp, port_name) - - -- Read or Write - if (channel == 1 or channel == 2) and size > 2 then - - if channel == 1 then - value_start = 4 - else - value_start = 3 - end - - -- Add variable id - local var_id = buffer(crtp_start + 1, 2):le_uint() - port_tree:add_le(f_crtp_parameter_varid, var_id) - - item = param_toc[var_id] - port_tree:add_le(f_crtp_parameter_group, item["group"]) - port_tree:add_le(f_crtp_parameter_name, item["name"]) - - -- Add value - if size > 3 then - port_tree:add_le(f_crtp_parameter_val_uint, buffer(crtp_start + value_start):le_uint()) - port_tree:add_le(f_crtp_parameter_val_int, buffer(crtp_start + value_start):le_int()) - end - if size >= 7 then - port_tree:add_le(f_crtp_parameter_val_float, buffer(crtp_start + value_start):le_float()) - end - undecoded = 0 - end - -- TOC - if channel == 0 then - message_id = buffer(crtp_start + 1, 1):le_uint() - if message_id == 3 and receive == 1 then - port_tree:add_le(f_crtp_parameter_count, buffer(crtp_start + 2, 2):le_uint()) - port_tree:add_le(f_crtp_parameter_crc, buffer(crtp_start + 4):bytes():tohex()) - end - if message_id == 2 then - -- GET_ITEM - item = {} - item["varid"] = buffer(crtp_start + 2, 2):le_uint() - port_tree:add_le(f_crtp_parameter_varid, item["varid"]) - - if receive == 1 then - item["type"] = get_param_types(buffer(crtp_start + 4, 1):le_uint()) - item["group"] = "" - item["name"] = "" - full_name = buffer(crtp_start + 5):string() - - stored_group = false - for i = 1, #full_name do - local c = full_name:sub(i, i) - if string.byte(c) == 0 and not stored_group then - stored_group = true - else - if stored_group then - item["name"] = item["name"] .. c - else - item["group"] = item["group"] .. c - end - end - end - - param_toc[item["varid"]] = item - port_tree:add_le(f_crtp_parameter_type, item["type"]) - port_tree:add_le(f_crtp_parameter_group, item["group"]) - port_tree:add_le(f_crtp_parameter_name, item["name"]) - end - end - end + local port_tree = tree:add(crtp, port_name) + + -- Read or Write + if (channel == 1 or channel == 2) and size > 2 then + if channel == 1 then + value_start = 4 + else + value_start = 3 + end + + -- Add variable id + local var_id = buffer(crtp_start + 1, 2):le_uint() + port_tree:add_le(f_crtp_parameter_varid, var_id) + + item = param_toc[var_id] + port_tree:add_le(f_crtp_parameter_group, item["group"]) + port_tree:add_le(f_crtp_parameter_name, item["name"]) + + -- Add value + if size > 3 then + port_tree:add_le(f_crtp_parameter_val_uint, buffer(crtp_start + value_start):le_uint()) + port_tree:add_le(f_crtp_parameter_val_int, buffer(crtp_start + value_start):le_int()) + end + if size >= 7 then + port_tree:add_le(f_crtp_parameter_val_float, buffer(crtp_start + value_start):le_float()) + end + undecoded = 0 + end + -- TOC + if channel == 0 then + message_id = buffer(crtp_start + 1, 1):le_uint() + if message_id == 3 and receive == 1 then + port_tree:add_le(f_crtp_parameter_count, buffer(crtp_start + 2, 2):le_uint()) + port_tree:add_le(f_crtp_parameter_crc, buffer(crtp_start + 4):bytes():tohex()) + end + if message_id == 2 then + -- GET_ITEM + item = {} + item["varid"] = buffer(crtp_start + 2, 2):le_uint() + port_tree:add_le(f_crtp_parameter_varid, item["varid"]) + + if receive == 1 then + item["type"] = get_param_types(buffer(crtp_start + 4, 1):le_uint()) + item["group"] = "" + item["name"] = "" + full_name = buffer(crtp_start + 5):string() + + stored_group = false + for i = 1, #full_name do + local c = full_name:sub(i, i) + if string.byte(c) == 0 and not stored_group then + stored_group = true + else + if stored_group then + item["name"] = item["name"] .. c + else + item["group"] = item["group"] .. c + end + end + end + + param_toc[item["varid"]] = item + port_tree:add_le(f_crtp_parameter_type, item["type"]) + port_tree:add_le(f_crtp_parameter_group, item["group"]) + port_tree:add_le(f_crtp_parameter_name, item["name"]) + end + end + end end -- create a function to dissect it, layout: -- | link_type | receive| address | channel | radio devid | crtp header | crtp data | -- | 1 byte | 1 byte | 5 or 12 bytes | 1 byte | 1 byte | 1 byte | n bytes | function crtp.dissector(buffer, pinfo, tree) - pinfo.cols.protocol = "CRTP" - - link = buffer(0, 1):uint() - if link == Links.RADIO then - crtp_start = 9 - elseif link == Links.USB then - crtp_start = 16 - end - - if buffer:len() <= crtp_start then return end - - local receive = buffer(1, 1):uint() - if receive == 1 then - pinfo.cols.dst = format_device(buffer) - pinfo.cols.src = format_address(buffer) - else - pinfo.cols.dst = format_address(buffer) - pinfo.cols.src = format_device(buffer) - end - - local subtree = tree:add(crtp, "CRTP Packet") - local header = bit.band(buffer(crtp_start, 1):uint(), 0xF3) - local crtp_port = bit.rshift(bit.band(header, 0xF0), 4) - local crtp_channel = bit.band(header, 0x03) - - -- Add CRTP packet size: - -- receive_byte + address + channel + devid = 8 - -- Rest is CRTP packet - local crtp_size = buffer:len() - crtp_start - subtree:add_le(f_crtp_size, crtp_size) - - undecoded = crtp_size - 1 - - -- Check for safelink packet - if crtp_size == 3 and header == 0xF3 and buffer(crtp_start + 1, 1):uint() == 0x05 then - pinfo.cols.info = "SafeLink" - return - end - - -- Get port and channel name - port_name, channel_name = get_crtp_port_channel_names(crtp_port, crtp_channel) - - -- Display port in info column - pinfo.cols.info = port_name - - -- Add to CRTP tree - subtree:add_le(f_crtp_port, crtp_port):append_text(" (" .. port_name .. ")") - if channel_name then - subtree:add_le(f_crtp_channel, crtp_channel):append_text(" (" .. channel_name .. ")") - else - subtree:add_le(f_crtp_channel, crtp_channel) - end - - -- Check for special handling - - -- Console, we can add text - if crtp_port == Ports.Console then - local port_tree = tree:add(crtp, port_name) - port_tree:add_le(f_crtp_console_text, buffer(crtp_start + 1):string()) - undecoded = 0 - end - - if crtp_port == Ports.Parameters then - handle_parameter_port(tree, receive, buffer, crtp_channel, crtp_size) - end - - if crtp_port == Ports.Setpoint_Highlevel then - handle_setpoint_highlevel(tree, receive, buffer, crtp_channel, crtp_size) - end - - if undecoded > 0 then - local from = crtp_start + (crtp_size - undecoded) - subtree:add_le(f_crtp_undecoded, buffer(from, undecoded):bytes():tohex()) - end + pinfo.cols.protocol = "CRTP" + + link = buffer(0, 1):uint() + if link == Links.RADIO then + crtp_start = 9 + elseif link == Links.USB then + crtp_start = 16 + end + + if buffer:len() <= crtp_start then + return + end + + local receive = buffer(1, 1):uint() + if receive == 1 then + pinfo.cols.dst = format_device(buffer) + pinfo.cols.src = format_address(buffer) + else + pinfo.cols.dst = format_address(buffer) + pinfo.cols.src = format_device(buffer) + end + + local subtree = tree:add(crtp, "CRTP Packet") + local header = bit.band(buffer(crtp_start, 1):uint(), 0xF3) + local crtp_port = bit.rshift(bit.band(header, 0xF0), 4) + local crtp_channel = bit.band(header, 0x03) + + -- Add CRTP packet size: + -- receive_byte + address + channel + devid = 8 + -- Rest is CRTP packet + local crtp_size = buffer:len() - crtp_start + subtree:add_le(f_crtp_size, crtp_size) + + undecoded = crtp_size - 1 + + -- Check for safelink packet + if crtp_size == 3 and header == 0xF3 and buffer(crtp_start + 1, 1):uint() == 0x05 then + pinfo.cols.info = "SafeLink" + return + end + + -- Get port and channel name + port_name, channel_name = get_crtp_port_channel_names(crtp_port, crtp_channel) + + -- Display port in info column + pinfo.cols.info = port_name + + -- Add to CRTP tree + subtree:add_le(f_crtp_port, crtp_port):append_text(" (" .. port_name .. ")") + if channel_name then + subtree:add_le(f_crtp_channel, crtp_channel):append_text(" (" .. channel_name .. ")") + else + subtree:add_le(f_crtp_channel, crtp_channel) + end + + -- Check for special handling + + -- Console, we can add text + if crtp_port == Ports.Console then + local port_tree = tree:add(crtp, port_name) + port_tree:add_le(f_crtp_console_text, buffer(crtp_start + 1):string()) + undecoded = 0 + end + + if crtp_port == Ports.Parameters then + handle_parameter_port(tree, receive, buffer, crtp_channel, crtp_size) + end + + if crtp_port == Ports.Setpoint_Highlevel then + handle_setpoint_highlevel(tree, receive, buffer, crtp_channel, crtp_size) + end + + if undecoded > 0 then + local from = crtp_start + (crtp_size - undecoded) + subtree:add_le(f_crtp_undecoded, buffer(from, undecoded):bytes():tohex()) + end end wtap_encap = DissectorTable.get("wtap_encap") From 37ce9fa689f0fc94c78804e73122d327a07f96e3 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Fri, 11 Feb 2022 12:39:02 +0100 Subject: [PATCH 385/861] wireshark: Add log TOC dissection --- tools/crtp-dissector.lua | 99 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 99 insertions(+) diff --git a/tools/crtp-dissector.lua b/tools/crtp-dissector.lua index e0ec9faee..0de981cb3 100644 --- a/tools/crtp-dissector.lua +++ b/tools/crtp-dissector.lua @@ -19,6 +19,13 @@ local f_crtp_parameter_val_uint = ProtoField.uint32("crtp.parameter_val_uint", " local f_crtp_parameter_val_int = ProtoField.int32("crtp.parameter_val_int", "Value int") local f_crtp_parameter_val_float = ProtoField.float("crtp.parameter_val_float", "Value float") +local f_crtp_log_varid = ProtoField.uint16("crtp.log_varid", "Variable Id") +local f_crtp_log_type = ProtoField.string("crtp.log_type", "Log Type") +local f_crtp_log_group = ProtoField.string("crtp.log_group", "Log Group") +local f_crtp_log_name = ProtoField.string("crtp.log_name", "Log Name") +local f_crtp_log_count = ProtoField.uint16("crtp.log_count", "Log Count") +local f_crtp_log_crc = ProtoField.string("crtp.log_crc", "Log CRC") + local f_crtp_setpoint_hl_command = ProtoField.string("crtp.setpoint_hl_command", "Command") local f_crtp_setpoint_hl_retval = ProtoField.uint8("crtp.setpoint_hl_retval", "Return Value") @@ -51,6 +58,12 @@ crtp.fields = { f_crtp_parameter_type, f_crtp_parameter_count, f_crtp_parameter_crc, + f_crtp_log_varid, + f_crtp_log_name, + f_crtp_log_group, + f_crtp_log_type, + f_crtp_log_count, + f_crtp_log_crc, f_crtp_setpoint_hl_command, f_crtp_setpoint_hl_retval, f_crtp_setpoint_hl_use_yaw, @@ -68,6 +81,7 @@ crtp.fields = { } local param_toc = {} +local log_toc = {} local Links = { UNKNOWN = 0, @@ -387,6 +401,42 @@ function append_param_type(str, type) end end +function get_log_types(byte) + type = "" + + local core = 0x20 + local byfunc = 0x40 + local group = 0x80 + + num_types_map = { + "LOG_UINT8", + "LOG_UINT16", + "LOG_UINT32", + "LOG_INT8", + "LOG_INT16", + "LOG_INT32", + "LOG_FLOAT", + "LOG_FP16", + } + + num_type = bit.band(byte, 0x0F) + type = num_types_map[num_type] + + if bit.band(byte, core) ~= 0 then + type = append_param_type(type, "LOG_CORE") + end + + if bit.band(byte, byfunc) ~= 0 then + type = append_param_type(type, "LOG_BYFUNC") + end + + if bit.band(byte, group) ~= 0 then + type = append_param_type(type, "LOG_GROUP") + end + + return byte .. " (" .. type .. ")" +end + function get_param_types(byte) type = "" @@ -431,6 +481,51 @@ function get_param_types(byte) return byte .. " (" .. type .. ")" end +function handle_logging_port(tree, receive, buffer, channel, size) + local port_tree = tree:add(crtp, port_name) + + -- TOC + if channel == 0 then + message_id = buffer(crtp_start + 1, 1):le_uint() + if message_id == 3 and receive == 1 then + port_tree:add_le(f_crtp_log_count, buffer(crtp_start + 2, 2):le_uint()) + port_tree:add_le(f_crtp_log_crc, buffer(crtp_start + 4):bytes():tohex()) + end + if message_id == 2 then + -- GET_ITEM + item = {} + item["varid"] = buffer(crtp_start + 2, 2):le_uint() + port_tree:add_le(f_crtp_log_varid, item["varid"]) + + if receive == 1 then + item["type"] = get_log_types(buffer(crtp_start + 4, 1):le_uint()) + item["group"] = "" + item["name"] = "" + full_name = buffer(crtp_start + 5):string() + + stored_group = false + for i = 1, #full_name do + local c = full_name:sub(i, i) + if string.byte(c) == 0 and not stored_group then + stored_group = true + else + if stored_group then + item["name"] = item["name"] .. c + else + item["group"] = item["group"] .. c + end + end + end + + log_toc[item["varid"]] = item + port_tree:add_le(f_crtp_log_type, item["type"]) + port_tree:add_le(f_crtp_log_group, item["group"]) + port_tree:add_le(f_crtp_log_name, item["name"]) + end + end + end +end + function handle_parameter_port(tree, receive, buffer, channel, size) local port_tree = tree:add(crtp, port_name) @@ -574,6 +669,10 @@ function crtp.dissector(buffer, pinfo, tree) handle_parameter_port(tree, receive, buffer, crtp_channel, crtp_size) end + if crtp_port == Ports.Logging then + handle_logging_port(tree, receive, buffer, crtp_channel, crtp_size) + end + if crtp_port == Ports.Setpoint_Highlevel then handle_setpoint_highlevel(tree, receive, buffer, crtp_channel, crtp_size) end From 36142938fcc421933b8b4f124f200958fc594229 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 16 Feb 2022 10:54:47 +0100 Subject: [PATCH 386/861] Updated read-ow example --- examples/memory/read-ow.py | 130 +++++++++---------------------------- 1 file changed, 31 insertions(+), 99 deletions(-) diff --git a/examples/memory/read-ow.py b/examples/memory/read-ow.py index 785c14d1c..b94f59832 100644 --- a/examples/memory/read-ow.py +++ b/examples/memory/read-ow.py @@ -22,16 +22,17 @@ # 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 +Simple example that connects to a Crazyflie, looks for 1-wire memories and lists its contents. """ import logging import sys -import time +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') @@ -39,114 +40,45 @@ # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) +update_event = Event() -class OWExample: - """ - Simple example listing the 1-wire memories found and lists its contents. - """ - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ +def read_ow_mems(cf): + mems = cf.mem.get_mems(MemoryElement.TYPE_1W) + print(f'Found {len(mems)} 1-wire memories') - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() + for m in mems: + update_event.clear() - # Keep track on if ow memory was detected - self.read_ow = False + 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) - # 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) +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 : ') - # Try to connect to the Crazyflie - self._cf.open_link(link_uri) + for key, element in mem.elements.items(): + print(f'\t\t{key}={element}') - # Variable used to keep main loop occupied until disconnect - self.is_connected = True - self._mems_to_update = 0 - - 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) - self._mems_to_update = len(mems) - print('Found {} 1-wire memories'.format(len(mems))) - - if len(mems) == 0: - self.read_ow = True - self._cf.close_link() - - 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('\tAddr : {}'.format(mem.addr)) - 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._mems_to_update -= 1 - if self._mems_to_update == 0: - self.read_ow = True - self._cf.close_link() - - 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 + update_event.set() if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() - le = OWExample(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, or timeout. - timeout = 5 # seconds - ts = time.time() - try: - while le.is_connected and (time.time() - ts < timeout): - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) - - if not le.read_ow: - sys.exit(1) + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + read_ow_mems(scf.cf) From 56ed29fc183d6850c42b6dc268b3d74df0502537 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Feb 2022 11:24:08 +0100 Subject: [PATCH 387/861] Delete CONTRIBUTING.md --- CONTRIBUTING.md | 55 ------------------------------------------------- 1 file changed, 55 deletions(-) delete mode 100644 CONTRIBUTING.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md deleted file mode 100644 index 1ace13f44..000000000 --- a/CONTRIBUTING.md +++ /dev/null @@ -1,55 +0,0 @@ -Contribution guide -================== - -👍🎉 Thanks a lot for considering contributing 🎉👍 - -We welcome and encourage contribution. There is many way to contribute: you can -write bug report, contribute code or documentation. -You can also go to the [bitcraze forum](https://forum.bitcraze.io) and help others. - -## Reporting issues - -When reporting issues the more information you can supply the better. -Since the lib runs on many different OSes, can connect to multiple versions of the Crazyflie and you could use our official releases or clone directly from Git, it can be hard to figure out what's happening. - - - **Information about the environment:** - - What OS are your running on - - What version of Python are you using - - If relevant, what version of the Crazyflie firmware are you using - - **How to reproduce the issue:** Step-by-step guide on how the issue can be reproduced (or at least how you reproduce it). - Include everything you think might be useful, the more information the better. - -## Improvements request and proposal - -We and the community are continuously working to improve the lib. -Feel free to make an issue to request a new functionality. - -## Contributing code/Pull-Request - -We welcome code contribution, this can be done by starting a pull-request. - -If the change is big, typically if the change span to more than one file, consider starting an issue first to discuss the improvement. -This will makes it much easier to make the change fit well into the lib. - -There is some basic requirement for us to merge a pull request: - - Describe the change - - Refer to any issues it effects - - Separate one pull request per functionality: if you start writing "and" in the feature description consider if it could be separated in two pull requests. - - The pull request must pass the automated test (see test section bellow) - -In your code: -- Don't include name, date or information about the change in the code. That's what Git is for. -- CamelCase classes, but not functions and variables -- Private variables and functions should start with _ -- 4 spaces indentation -- When catching exceptions try to make it as specific as possible, it makes it harder for bugs to hide -- Short variable and function names are OK if the scope is small -- The code should pass flake8 - -### Run test - -In order to run the tests you can run: -``` -python tools/build/build -python3 tools/build/build -``` From fd9021ab943dab805dc580d030566ec6b381acde Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Feb 2022 11:24:24 +0100 Subject: [PATCH 388/861] Update README.md --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 80dd87be6..ddbabeb12 100644 --- a/README.md +++ b/README.md @@ -15,5 +15,9 @@ See the [installation instructions](docs/installation/install.md) in the github Check out the [Bitcraze crazyflie-lib-python documentation](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/) on our website. ## Contribute +Go to the [contribute page](https://www.bitcraze.io/contribute/) on our website to learn more. -Everyone is encouraged to contribute to the CrazyFlie library by forking the Github repository and making a pull request or opening an issue. +### Test code for contribution +Run the automated build locally to test your code + + python 3 tools/build/build From 7b1fc38358690fe887866347bf0e502f14d4e4fc Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Feb 2022 11:24:31 +0100 Subject: [PATCH 389/861] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ddbabeb12..6165ad52e 100644 --- a/README.md +++ b/README.md @@ -20,4 +20,4 @@ Go to the [contribute page](https://www.bitcraze.io/contribute/) on our website ### Test code for contribution Run the automated build locally to test your code - python 3 tools/build/build + python3 tools/build/build From 9ddf85ca688a3fb95e773aa05ff1d0d259adfc79 Mon Sep 17 00:00:00 2001 From: Jonas Danielsson Date: Wed, 16 Feb 2022 14:00:24 +0100 Subject: [PATCH 390/861] crtp-dissector: Add dissection for Echo packets --- tools/crtp-dissector.lua | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/tools/crtp-dissector.lua b/tools/crtp-dissector.lua index 0de981cb3..051010dff 100644 --- a/tools/crtp-dissector.lua +++ b/tools/crtp-dissector.lua @@ -43,6 +43,8 @@ local f_crtp_setpoint_hl_x = ProtoField.float("crtp.setpoint_hl_x", "X") local f_crtp_setpoint_hl_y = ProtoField.float("crtp.setpoint_hl_y", "Y") local f_crtp_setpoint_hl_z = ProtoField.float("crtp.setpoint_hl_z", "Z") +local f_crtp_echo_data = ProtoField.uint32("crtp.echo_data", "Echo Data") + -- All possible fields registred crtp.fields = { f_crtp_port, @@ -77,6 +79,7 @@ crtp.fields = { f_crtp_setpoint_hl_z, f_crtp_setpoint_hl_id, f_crtp_setpoint_hl_timescale, + f_crtp_echo_data, f_crtp_undecoded, } @@ -106,7 +109,8 @@ local Ports = { Commander_Generic = 0x7, Setpoint_Highlevel = 0x8, Platform = 0xD, - All = 0xF, + LinkControl = 0xF, + ALL = 0xFF } function get_crtp_port_channel_names(port, channel) @@ -168,7 +172,9 @@ function get_crtp_port_channel_names(port, channel) end elseif port == 0x0F then port_name = "Link Control" - if channel == 1 then + if channel == 0 then + channel_name = "Echo" + elseif channel == 1 then channel_name = "Link Service Source" end elseif port == 0xFF then @@ -665,6 +671,14 @@ function crtp.dissector(buffer, pinfo, tree) undecoded = 0 end + if crtp_port == Ports.LinkControl then + if crtp_channel == 0 then -- Echo + local port_tree = tree:add(crtp, channel_name) + port_tree:add_le(f_crtp_echo_data, buffer(crtp_start + 1):le_uint()) + undecoded = 0 + end + end + if crtp_port == Ports.Parameters then handle_parameter_port(tree, receive, buffer, crtp_channel, crtp_size) end From 4b5dfc2ef7e5ecb91094185a5d9ec6a97242ba5d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 11 Mar 2022 17:29:17 +0100 Subject: [PATCH 391/861] Improved multi bs estimation --- cflib/localization/lighthouse_bs_vector.py | 7 +- .../lighthouse_geometry_solver.py | 13 +- .../lighthouse_initial_estimator.py | 307 ++++++++++++++---- .../localization/lighthouse_sample_matcher.py | 3 + .../localization/lighthouse_system_aligner.py | 7 +- .../localization/lighthouse_system_scaler.py | 131 ++++++++ cflib/localization/lighthouse_types.py | 23 ++ ...ion.py => multi_bs_geometry_estimation.py} | 223 ++++++++++--- .../test_lighthouse_geometry_solver.py | 20 -- .../test_lighthouse_initial_estimator.py | 9 +- .../test_lighthouse_system_aligner.py | 6 +- 11 files changed, 607 insertions(+), 142 deletions(-) create mode 100644 cflib/localization/lighthouse_system_scaler.py rename examples/lighthouse/{bs_geometry_estimation.py => multi_bs_geometry_estimation.py} (53%) diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py index 89537ac3f..67e035964 100644 --- a/cflib/localization/lighthouse_bs_vector.py +++ b/cflib/localization/lighthouse_bs_vector.py @@ -138,11 +138,10 @@ def _q(self): return math.tan(self._lh_v1_vert_angle) / math.sqrt(1 + math.tan(self._lh_v1_horiz_angle) ** 2) -# A LighthouseBsVectors is a list of 4 LighthouseBsVector, one for -# each sensor -# LighthouseBsVectors = list[LighthouseBsVector] - class LighthouseBsVectors(list): + """A list of 4 LighthouseBsVector, one for each sensor. + LighthouseBsVectors is essentially the same as list[LighthouseBsVector]""" + def projection_pair_list(self) -> npt.NDArray: """ Genereate a list of projection pairs for all vectors diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 7b4f2696c..a58082e02 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -131,7 +131,6 @@ class LighthouseGeometrySolver: cf2/bs3/sens1/ang0 X X cf2/bs3/sens1/ang1 X X ... - """ @classmethod @@ -141,8 +140,8 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample] Solve for the pose of base stations and CF samples. The pose of the CF in sample 0 defines the global reference frame. - Iteration is terminated after a fixed number of iteration if acceptable solution - is found. + Iteration is terminated acceptable solution is found. If no solution is found after a fixed number of iterations + the solver is terminated. The success member of the result will indicate if a solution was found or not. :param initial_guess: Initial guess for the base stations and CF sample poses :param matched_samples: List of matched samples. @@ -155,7 +154,7 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample] solution.n_cfs = len(matched_samples) solution.n_cfs_in_params = len(matched_samples) - 1 solution.n_sensors = len(sensor_positions) - solution.bs_id_to_index, solution.bs_index_to_id = cls._crate_bs_map(initial_guess.bs_poses) + solution.bs_id_to_index, solution.bs_index_to_id = cls._create_bs_map(initial_guess.bs_poses) target_angles = cls._populate_target_angles(matched_samples) idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, jac_sparsity = cls._populate_indexes_and_jacobian( @@ -330,7 +329,7 @@ def _calc_angle_pairs(cls, bs_p_a, cf_p_a, sens_pos_p_a, defs: LighthouseGeometr :param bs_p_a: Poses base stations :param cf_p_a: Poses CFs - :paran sens_pos_p_a: Sensor positions + :param sens_pos_p_a: Sensor positions :return: angle pairs All lists are equally long, one entry per output angle pair @@ -378,7 +377,7 @@ def _params_to_pose(cls, params: npt.ArrayLike, defs: LighthouseGeometrySolution return Pose.from_rot_vec(R_vec=r_vec, t_vec=t) @classmethod - def _crate_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[int, int], dict[int, int]]: + def _create_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[int, int], dict[int, int]]: """ We might have gaps in the list of base station ids that is used in the system, use an index instead when refering to a base station. This method creates dictionaries to go from index to base station id, @@ -427,7 +426,7 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, solution.error_info = cls._aggregate_error_info(solution.estimated_errors) @classmethod - def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]): + def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]) -> dict[str, float]: error_per_bs = {} errors = [] for sample_errors in estimated_errors: diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index d84e43942..2f020de6e 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -32,9 +32,9 @@ class LighthouseInitialEstimator: """ - Make initial estimates of base station and CF poses useing IPPE (analytical solution). + Make initial estimates of base station and CF poses using IPPE (analytical solution). The estimates are not good enough to use for flight but is a starting point for further - Calculations. + calculations. """ @classmethod @@ -45,75 +45,241 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A The pose of the Crazyflie in the first sample is used as a reference and will define the global reference frame. - :param matched_samples: A list of samples with lihghthouse angles. + :param matched_samples: A list of samples with lighthouse angles. :param sensor_positions: An array with the sensor positions on the lighthouse deck (3D, CF ref frame) :return: a """ - bs_poses_ref_cfs = cls._angles_to_poses(matched_samples, sensor_positions) + bs_positions = cls._find_solutions(matched_samples, sensor_positions) + # bs_positions is a map from bs-id-pair to position, where the position is the position of the second + # bs, as seen from the first bs (in the first bs ref frame). - # Use the first CF pose as the global reference frame - bs_poses: dict[int, Pose] = bs_poses_ref_cfs[0] + bs_poses_ref_cfs = cls._angles_to_poses(matched_samples, sensor_positions, bs_positions) - cls._calc_remaining_bs_poses(bs_poses_ref_cfs, bs_poses) - cf_poses = cls._calc_cf_poses(bs_poses_ref_cfs, bs_poses) + # Use the first CF pose as the global reference frame. The pose of the first base station (as estimated by ippe) + # is used as the "true" position (reference) + reference_bs_pose = None + for bs_pose_ref_cfs in bs_poses_ref_cfs: + if len(bs_pose_ref_cfs) > 0: + bs_id, reference_bs_pose = list(bs_pose_ref_cfs.items())[0] + break + + if reference_bs_pose is None: + raise Exception('Too little data, no reference') + bs_poses: dict[int, Pose] = {bs_id: reference_bs_pose} + + # Calculate the pose of the remaining base stations, based on the pose of the first CF + cls._estimate_remaining_bs_poses(bs_poses_ref_cfs, bs_poses) + + # Now that we have estimated the base station poses, estimate the poses of the CF in all the samples + cf_poses = cls._estimate_cf_poses(bs_poses_ref_cfs, bs_poses) return LhBsCfPoses(bs_poses, cf_poses) @classmethod - def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike - ) -> list[dict[int, Pose]]: + def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike + ) -> dict[tuple(int, int), npt.NDArray]: + """ + Find the pose of all base stations, in the reference frame of other base stations. + + Ippe finds two mirror solutions for each sample and the bulk of this method is geared towards finding the + correct one. The outline of the process is: + 1. For each sample collect all possible permutations (4) of the base station positions + 2. Aggregate the possible positions of all samples in clusters. This is done per base station pair. + 3. Pick the "best" cluster as the correct permutation. The idea is that the mirror solutions will be spread + out in space, while the correct one will end up more or less in the same spot for all samples. + + :param matched_samples: List of matched samples + :param sensor_positions: list of sensor positions on the lighthouse deck, CF reference frame + :return: Base stations poses in the reference frame of the other base stations. The data is organized as a + dictionary of tuples with base station id pairs, mapped to positions. For instance the entry with key + (2, 1) contains the position of base station 1, in the base station 2 reference frame. + """ + + position_permutations: dict[tuple(int, int), list[list[npt.ArrayLike]]] = {} + for sample in matched_samples: + solutions: dict[int, tuple[Pose, Pose]] = {} + for bs, angles in sample.angles_calibrated.items(): + projections = angles.projection_pair_list() + estimates_ref_bs = IppeCf.solve(sensor_positions, projections) + estimates_ref_cf = cls._convert_estimates_to_cf_reference_frame(estimates_ref_bs) + solutions[bs] = estimates_ref_cf + + cls._add_solution_permutations(solutions, position_permutations) + + return cls._find_most_likely_positions(position_permutations) + + @classmethod + def _add_solution_permutations(cls, solutions: dict[int, tuple[Pose, Pose]], + position_permutations: dict[tuple[int, int], list[list[npt.ArrayLike]]]): + """ + Add the possible permutations of base station positions for a sample to a collection of aggregated positions. + The aggregated collection contains base station positions in the reference frame of other base stations. + + :param solutions: All possible positions of the base stations, in the reference frame of the Crazyflie in one + sample + :param position_permutations: Aggregated possible solutions. A dictionary with base staion pairs as keys, mapped + to lists of lists of possible positions. For instance, the entry for (2, 1) would + contain a list of lists with 4 positions each, for where base station 1 might be + located in the base station 2 reference frame. + """ + ids = sorted(solutions.keys()) + + for i, id_i in enumerate(ids): + solution_i = solutions[id_i] + + for j in range(i + 1, len(ids)): + id_j = ids[j] + + solution_j = solutions[id_j] + + pose1 = solution_i[0].inv_rotate_translate_pose(solution_j[0]) + pose2 = solution_i[0].inv_rotate_translate_pose(solution_j[1]) + pose3 = solution_i[1].inv_rotate_translate_pose(solution_j[0]) + pose4 = solution_i[1].inv_rotate_translate_pose(solution_j[1]) + + pair = (id_i, id_j) + if pair not in position_permutations: + position_permutations[pair] = [] + position_permutations[pair].append([pose1.translation, pose2.translation, + pose3.translation, pose4.translation]) + + @classmethod + def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike, + bs_positions: dict[tuple(int, int), npt.NDArray]) -> list[dict[int, Pose]]: """ Estimate the base station poses in the Crazyflie reference frames, for each sample. + + Use Ippe again to find the possible poses of the bases stations and pick the one that best matches the position + in bs_positions. + + :param matched_samples: List of samples + :param sensor_positions: Positions of the sensors on the lighthouse deck (CF ref frame) + :param bs_positions: Dictionary of base station positions (other base station ref frame) + :return: A list of dictionaries from base station to Pose of all base stations, for each sample """ result: list[dict[int, Pose]] = [] + for sample in matched_samples: - poses: dict[int, Pose] = {} + solutions: dict[int, tuple[Pose, Pose]] = {} for bs, angles in sample.angles_calibrated.items(): - Q = angles.projection_pair_list() - estimates_ref_bs = IppeCf.solve(sensor_positions, Q) + projections = angles.projection_pair_list() + estimates_ref_bs = IppeCf.solve(sensor_positions, projections) estimates_ref_cf = cls._convert_estimates_to_cf_reference_frame(estimates_ref_bs) - bs_pose = cls._solution_picker(estimates_ref_cf) + solutions[bs] = estimates_ref_cf + + poses: dict[int, Pose] = {} + ids = sorted(solutions.keys()) + first = ids[0] + for other in ids[1:]: + pair = (first, other) + expected = bs_positions[pair] + poses[first], poses[other] = cls._choose_solutions(solutions[first], solutions[other], expected) - poses[bs] = bs_pose result.append(poses) + return result @classmethod - def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf.Solution]) -> tuple[Pose, Pose]: + def _choose_solutions(cls, solutions_1: tuple[Pose, Pose], solutions_2: tuple[Pose, Pose], + expected: npt.ArrayLike) -> tuple[Pose, Pose]: + """Pick the base pose solutions for a pair of base stations, based on the position in expected""" + + min_dist = 100000.0 + best1 = None + best2 = None + + for solution_1 in solutions_1: + for solution_2 in solutions_2: + pose_second_bs_ref_fr_first = solution_1.inv_rotate_translate_pose(solution_2) + dist = np.linalg.norm(expected - pose_second_bs_ref_fr_first.translation) + if dist < min_dist: + min_dist = dist + best1 = solution_1 + best2 = solution_2 + + if min_dist > 0.5: + print('large error:', min_dist) + + return best1, best2 + + @classmethod + def _find_most_likely_positions(cls, position_permutations: dict[tuple(int, int), + list[list[npt.ArrayLike]]]) -> dict[tuple(int, int), npt.NDArray]: """ - Convert the two ippe solutions from the base station reference frame to the CF reference frame + Find the most likely base station positions from all the possible permutations. + + Sort the permutations into buckets based on how close they are to the solutions in the first sample. Solutions + that are "too" far away and distcarded. The bucket with the most samples in, is considerred the best. """ - R1 = estimates_ref_bs[0].R.transpose() - t1 = np.dot(R1, -estimates_ref_bs[0].t) + result: dict[tuple(int, int), npt.NDArray] = {} - R2 = estimates_ref_bs[1].R.transpose() - t2 = np.dot(R2, -estimates_ref_bs[1].t) + for pair, position_lists in position_permutations.items(): + # Use first as reference to map the others to + bucket_ref_positions = position_lists[0] + buckets: list[list[npt.NDArray]] = [[], [], [], []] - return Pose(R1, t1), Pose(R2, t2) + cls._map_positions_to_ref(bucket_ref_positions, position_lists, buckets) + best_pos = cls._find_best_position_bucket(buckets) + result[pair] = best_pos + + return result @classmethod - def _solution_picker(cls, estimates_ref_cf: tuple[Pose, Pose]) -> Pose: + def _map_positions_to_ref(cls, bucket_ref_positions: list[npt.ArrayLike], position_lists: list[list[npt.ArrayLike]], + buckets: list[list[npt.ArrayLike]]) -> None: + """ + Sort solution into buckets based on the distance to the reference position. If no bucket is close enough, + the solution is discarded. """ - IPPE produces two solutions and we have to pick the right one. - The solutions are on opposite sides of the CF Z-axis and one of them is also flipped up side down. - Assuming that our base stations are pointing downwards and that the CF is fairly flat, we can - find the correct solution by looking at the orientation and pick the one where the Z-axis is pointing - upwards. - :param estimates_ref_cf: The two possible estimated base station poses - :return: The selected solution + accept_radius = 0.8 + + for pos_list in position_lists: + for pos in pos_list: + for i, ref in enumerate(bucket_ref_positions): + if np.linalg.norm(pos - ref) < accept_radius: + buckets[i].append(pos) + break + + @classmethod + def _find_best_position_bucket(cls, buckets: list[list[npt.ArrayLike]]) -> npt.NDArray: + """ + Find the bucket with the most solutions in, this is considered to be the correct solution. + The final result is the mean of the solution in the bucket. + """ + max_len = 0 + max_i = 0 + for i, bucket in enumerate(buckets): + if len(bucket) > max_len: + max_len = len(bucket) + max_i = i + + pos = np.mean(buckets[max_i], axis=0) + return pos + + @classmethod + def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf.Solution]) -> tuple[Pose, Pose]: + """ + Convert the two ippe solutions from the base station reference frame to the CF reference frame """ - pose1 = estimates_ref_cf[0] - pose2 = estimates_ref_cf[1] + rot_1 = estimates_ref_bs[0].R.transpose() + t_1 = np.dot(rot_1, -estimates_ref_bs[0].t) + + rot_2 = estimates_ref_bs[1].R.transpose() + t_2 = np.dot(rot_2, -estimates_ref_bs[1].t) - if np.dot(pose1.rot_matrix, (0.0, 0.0, 1.0))[2] > np.dot(pose2.rot_matrix, (0.0, 0.0, 1.0))[2]: - return pose1 - else: - return pose2 + return Pose(rot_1, t_1), Pose(rot_2, t_2) @classmethod - def _calc_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: dict[int, Pose]) -> None: + def _estimate_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: dict[int, Pose]) -> None: + """ + Based on one base station pose, estimate the other base staion poses. + + The process is iterative and runs until all poses are found. Assume we know the pose of base station 0, and we + have information of base station pairs (0, 2) and (2, 3), from this we can first derive the pose of 2 and after + that the pose of 3. + """ # Find all base stations in the list all_bs = set() for initial_est_bs_poses in bs_poses_ref_cfs: @@ -125,8 +291,8 @@ def _calc_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_po # run through the list of samples until we manage to find them all remaining = len(to_find) while remaining > 0: - for initial_est_bs_poses in bs_poses_ref_cfs: - bs_poses_in_sample = initial_est_bs_poses + buckets: dict[int, list[Pose]] = {} + for bs_poses_in_sample in bs_poses_ref_cfs: unknown = to_find.intersection(bs_poses_in_sample.keys()) known = set(bs_poses.keys()).intersection(bs_poses_in_sample.keys()) @@ -139,35 +305,64 @@ def _calc_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_po # The known BS pose in the CF reference frame (of this sample) known_cf = bs_poses_in_sample[known_bs] - for bs in unknown: + for bs_id in unknown: # The unknown BS pose in the CF reference frame (of this sample) - unknown_cf = bs_poses_in_sample[bs] + unknown_cf = bs_poses_in_sample[bs_id] # Finally we can calculate the BS pose in the global reference frame - bs_poses[bs] = cls._map_pose_to_ref_frame(known_global, known_cf, unknown_cf) + bs_pose = cls._map_pose_to_ref_frame(known_global, known_cf, unknown_cf) + if bs_id not in buckets: + buckets[bs_id] = [] + buckets[bs_id].append(bs_pose) - to_find = all_bs - bs_poses.keys() - if len(to_find) == 0: - break + # Average over poses and add to bs_poses + for bs_id, poses in buckets.items(): + bs_poses[bs_id] = cls._avarage_poses(poses) + + to_find = all_bs - bs_poses.keys() + if len(to_find) == 0: + break if len(to_find) == remaining: - raise Exception('Can not link positions between all base stations') + raise RuntimeError('Can not link positions between all base stations') remaining = len(to_find) @classmethod - def _calc_cf_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: list[Pose]) -> list[Pose]: - cf_poses: list[Pose] = [] + def _avarage_poses(cls, poses: list[Pose]) -> Pose: + """ + Averaging of quaternions to get the "average" orientation of multiple samples. + From https://stackoverflow.com/a/61013769 + """ + def q_average(Q, W=None): + if W is not None: + Q *= W[:, None] + eigvals, eigvecs = np.linalg.eig(Q.T@Q) + return eigvecs[:, eigvals.argmax()] - for initial_est_bs_poses in bs_poses_ref_cfs: - # Use the first base station pose as a reference - est_ref_cf = initial_est_bs_poses - ref_bs = list(est_ref_cf.keys())[0] + positions = map(lambda x: x.translation, poses) + average_pos = np.average(np.array(list(positions)), axis=0) + + quats = map(lambda x: x.rot_quat, poses) + average_quaternion = q_average(np.array(list(quats))) + + return Pose.from_quat(R_quat=average_quaternion, t_vec=average_pos) + + @classmethod + def _estimate_cf_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: list[Pose]) -> list[Pose]: + """ + Find the pose of the Crazyflie in all samples, based on the base station poses. + """ + cf_poses: list[Pose] = [] - pose_global = bs_poses[ref_bs] - pose_cf = est_ref_cf[ref_bs] - est_ref_global = cls._map_cf_pos_to_cf_pos(pose_global, pose_cf) + for est_ref_cf in bs_poses_ref_cfs: + # Average the global pose based on all base stations + poses = [] + for bs_id, pose_cf in est_ref_cf.items(): + pose_global = bs_poses[bs_id] + est_ref_global = cls._map_cf_pos_to_cf_pos(pose_global, pose_cf) + poses.append(est_ref_global) - cf_poses.append(est_ref_global) + cf_poses.append(cls._avarage_poses(poses)) return cf_poses diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py index 8b7bee560..d16fb338e 100644 --- a/cflib/localization/lighthouse_sample_matcher.py +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -36,6 +36,9 @@ class LighthouseSampleMatcher: @classmethod def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.010, min_nr_of_bs_in_match: int = 0) -> list[LhCfPoseSample]: + """ + Aggregate samples close in time into lists + """ result = [] current: LhCfPoseSample = None diff --git a/cflib/localization/lighthouse_system_aligner.py b/cflib/localization/lighthouse_system_aligner.py index 755ac8eac..3ec964a0a 100644 --- a/cflib/localization/lighthouse_system_aligner.py +++ b/cflib/localization/lighthouse_system_aligner.py @@ -29,9 +29,10 @@ class LighthouseSystemAligner: + """This class is used to align a lighthouse system to a few sampled positions""" @classmethod def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: list[npt.ArrayLike], - bs_poses: dict[int, Pose]) -> dict[int, Pose]: + bs_poses: dict[int, Pose]) -> tuple[dict[int, Pose], Pose]: """ Align a coordinate system with the physical world. Finds the transform from the current reference frame to one that is aligned with measured positions, and transforms base station @@ -42,7 +43,7 @@ def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: lis reference frame :param x_axis: One or more positions in the desired XY-plane (Z=0) in the current reference frame :param bs_poses: a dictionary with the base station poses in the current reference frame - :return: a dictionary with the base station poses in the desired reference frame + :return: a dictionary with the base station poses in the desired reference frame and the transformation """ raw_transformation = cls._find_transformation(origin, x_axis, xy_plane) transformation = cls._de_flip_transformation(raw_transformation, x_axis, bs_poses) @@ -51,7 +52,7 @@ def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: lis for bs_id, pose in bs_poses.items(): result[bs_id] = transformation.rotate_translate_pose(pose) - return result + return result, transformation @classmethod def _find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], diff --git a/cflib/localization/lighthouse_system_scaler.py b/cflib/localization/lighthouse_system_scaler.py new file mode 100644 index 000000000..a873dba03 --- /dev/null +++ b/cflib/localization/lighthouse_system_scaler.py @@ -0,0 +1,131 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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, 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 . +from __future__ import annotations + +import copy + +import numpy as np +import numpy.typing as npt + +from cflib.localization.lighthouse_bs_vector import LighthouseBsVector +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import Pose + + +class LighthouseSystemScaler: + """This class is used to re-scale a system based on various measurements.""" + @classmethod + def scale_fixed_point(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose], expected: npt.ArrayLike, + actual: Pose) -> tuple[dict[int, Pose], list[Pose], float]: + """ + Scale a system based on a position in the physical world in relation to where it is in the estimated system + geometry. Assume the system is aligned and simply use the distance to the points for scaling. + + :param bs_poses: a dictionary with the base station poses in the current reference frame + :param cf_poses: List of CF poses + :param expected: The real world position to use as reference + :param actual: The estimated position in the current system geometry + :return: a tuple containing a dictionary with the base station poses in the scaled system, + a list of Crazyflie poses in the scaled system and the scaling factor + """ + expected_distance = np.linalg.norm(expected) + actual_distance = np.linalg.norm(actual.translation) + scale_factor = expected_distance / actual_distance + return cls._scale_system(bs_poses, cf_poses, scale_factor) + + @classmethod + def scale_diagonals(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose], matched_samples: list[LhCfPoseSample], + expected_diagonal: float) -> tuple[dict[int, Pose], list[Pose], float]: + """ + Scale a system based on where base station "rays" intersects the lighthouse deck in relation to sensor + positions. Calculates the intersection points for all samples and scales the system to match the expected + distance between sensors on the deck. + + :param bs_poses: a dictionary with the base station poses in the current reference frame + :param cf_poses: List of CF poses + :param matched_samples: List of samples. Length must be the same as cf_poses. + :return: a tuple containing a dictionary with the base station poses in the scaled system, + a list of Crazyflie poses in the scaled system and the scaling factor + """ + + estimated_diagonal = cls._calculate_mean_diagonal(bs_poses, cf_poses, matched_samples) + scale_factor = expected_diagonal / estimated_diagonal + return cls._scale_system(bs_poses, cf_poses, scale_factor) + + @classmethod + def _scale_system(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose], + scale_factor: float) -> tuple[dict[int, Pose], list[Pose], float]: + """ + Scale poses of base stations and crazyflie samples. + """ + bs_scaled = {bs_id: copy.copy(pose) for bs_id, pose in bs_poses.items()} + for pose in bs_scaled.values(): + pose.scale(scale_factor) + + cf_scaled = [copy.copy(pose) for pose in cf_poses] + for pose in cf_scaled: + pose.scale(scale_factor) + + return bs_scaled, cf_scaled, scale_factor + + @classmethod + def _calculate_mean_diagonal(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose], + matched_samples: list[LhCfPoseSample]) -> float: + """ + Calculate the average diagonal sensor distance based on where the rays intersect the lighthouse deck + """ + diagonals: list[float] = [] + + for cf_pose, sample in zip(cf_poses, matched_samples): + for bs_id, vectors in sample.angles_calibrated.items(): + diagonals.append(cls.calc_intersection_distance(vectors[0], vectors[3], bs_poses[bs_id], cf_pose)) + diagonals.append(cls.calc_intersection_distance(vectors[1], vectors[2], bs_poses[bs_id], cf_pose)) + + estimated_diagonal = np.mean(diagonals) + + return estimated_diagonal + + @classmethod + def calc_intersection_distance(cls, vector1: LighthouseBsVector, vector2: LighthouseBsVector, + bs_pose: Pose, cf_pose: Pose) -> float: + """Calculate distance between intersection points of rays on the plane defined by the lighthouse deck""" + + intersection1 = cls.calc_intersection_point(vector1, bs_pose, cf_pose) + intersection2 = cls.calc_intersection_point(vector2, bs_pose, cf_pose) + distance = np.linalg.norm(intersection1 - intersection2) + return distance + + @classmethod + def calc_intersection_point(cls, vector: LighthouseBsVector, bs_pose: Pose, cf_pose: Pose) -> npt.NDArray: + """Calculate the intersetion point of a lines and a plane. + The line is the intersection of the two light planes from a base station, while the + plane is defined by the lighthouse deck of the Crazyflie.""" + + plane_base = cf_pose.translation + plane_normal = np.dot(cf_pose.rot_matrix, (0.0, 0.0, 1.0)) + + line_base = bs_pose.translation + line_vector = np.dot(bs_pose.rot_matrix, vector.cart) + + dist_on_line = np.dot((plane_base - line_base), plane_normal) / np.dot(line_vector, plane_normal) + + return line_base + line_vector * dist_on_line diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index b5ec105bc..ec8d34c5f 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -36,6 +36,7 @@ class Pose: _NO_ROTATION_MTX = np.identity(3) _NO_ROTATION_VCT = np.array((0.0, 0.0, 0.0)) + _NO_ROTATION_QUAT = np.array((0.0, 0.0, 0.0, 0.0)) _ORIGIN = np.array((0.0, 0.0, 0.0)) def __init__(self, R_matrix: npt.ArrayLike = _NO_ROTATION_MTX, t_vec: npt.ArrayLike = _ORIGIN) -> None: @@ -52,6 +53,19 @@ def from_rot_vec(cls, R_vec: npt.ArrayLike = _NO_ROTATION_VCT, t_vec: npt.ArrayL """ return Pose(Rotation.from_rotvec(R_vec).as_matrix(), t_vec) + @classmethod + def from_quat(cls, R_quat: npt.ArrayLike = _NO_ROTATION_QUAT, t_vec: npt.ArrayLike = _ORIGIN) -> 'Pose': + """ + Create a Pose from a quaternion and translation vector + """ + return Pose(Rotation.from_quat(R_quat).as_matrix(), t_vec) + + def scale(self, scale) -> None: + """ + quiet + """ + self._t_vec = self._t_vec * scale + @property def rot_matrix(self) -> npt.NDArray: """ @@ -66,6 +80,13 @@ def rot_vec(self) -> npt.NDArray: """ return Rotation.from_matrix(self._R_matrix).as_rotvec() + @property + def rot_quat(self) -> npt.NDArray: + """ + Get the quaternion of the pose + """ + return Rotation.from_matrix(self._R_matrix).as_quat() + @property def translation(self) -> npt.NDArray: """ @@ -159,3 +180,5 @@ class LhDeck4SensorPositions: (-_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0), (_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0), (_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0)]) + + diagonal_distance = np.sqrt(_sensor_distance_length ** 2 + _sensor_distance_length ** 2) diff --git a/examples/lighthouse/bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py similarity index 53% rename from examples/lighthouse/bs_geometry_estimation.py rename to examples/lighthouse/multi_bs_geometry_estimation.py index 497e12cc0..3bdc4c630 100644 --- a/examples/lighthouse/bs_geometry_estimation.py +++ b/examples/lighthouse/multi_bs_geometry_estimation.py @@ -30,25 +30,28 @@ The execution of the script takes you through a number of steps, please follow the instructions. -This script supports large systems with more than 2 base stations (if -supported by the CF firmware). +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. -Prerequisite: +Prerequisites: 1. The base station calibration data must have been received by the Crazyflie before this script is executed. -2. Base stations must point downwards, towards the floor. + +2. 2 or more base stations ''' import logging 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 @@ -56,42 +59,54 @@ 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): + +def record_angles_average(scf: SyncCrazyflie) -> LhCfPoseSample: + """Record angles and average over the samples to reduce noise""" recorded_angles = None - isReady = Event() + is_ready = Event() def ready_cb(averages): nonlocal recorded_angles recorded_angles = averages - isReady.set() + is_ready.set() reader = LighthouseSweepAngleAverageReader(scf.cf, ready_cb) reader.start_angle_collection() - isReady.wait() + is_ready.wait() 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, bs visible: {visible}') + print(f' Position recorded, base station ids visible: {visible}') - return LhCfPoseSample(angles_calibrated=angles_calibrated) + 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, recording_time_s): - 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, angles): + 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) @@ -102,7 +117,7 @@ def ready_cb(bs_id, angles): end_time = time.time() + recording_time_s while time.time() < end_time: - time_left = end_time - time.time() + time_left = int(end_time - time.time()) visible = ', '.join(sorted(bs_seen)) print(f'{time_left}s, bs visible: {visible}') bs_seen = set() @@ -113,16 +128,95 @@ def ready_cb(bs_id, angles): return result -def parse(recording_time, default): +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 estimate_geometry(origin, x_axis, xy_plane, samples): +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 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 = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + + print('Initial guess base stations at:') + print_base_stations_poses(initial_guess.bs_poses) + visualize(initial_guess.cf_poses, initial_guess.bs_poses.values()) + print(f'{len(matched_samples)} samples will be used') + solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) if not solution.success: print('Solution did not converge, it might not be good!') @@ -134,20 +228,38 @@ def estimate_geometry(origin, x_axis, xy_plane, samples): 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)) - bs_aligned_poses = LighthouseSystemAligner.align(origin_pos, x_axis_pos, xy_plane_pos, solution.bs_poses) + print('Raw solution:') print(' Base stations at:') - for bs_id, pose in sorted(bs_aligned_poses.items()): - pos = pose.translation - print(f' {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})') + 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}') - return bs_aligned_poses + # 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)) -def upload_geometry(scf, bs_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() @@ -158,7 +270,7 @@ def upload_geometry(scf, bs_poses): event = Event() - def data_written(success): + def data_written(_): event.set() helper = LighthouseConfigWriter(scf.cf) @@ -166,33 +278,41 @@ def data_written(success): event.wait() -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - -if __name__ == '__main__': - uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - # Initialize the low-level drivers - cflib.crtp.init_drivers() - +def connect_and_estimate(uri: str): + """Connect to a Crazyflie, collect data and estimate the geometry of the system""" print(f'Step 1. Connecting to the Crazyflie on uri {uri}...') with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: print(' Connected') print('') print('In the 3 following steps we will define the coordinate system.') + print('Step 2. Put the Crazyflie where you want the origin of your coordinate system.') - input('Press return when ready. ') - print(' Recording...') - origin = record_angles_average(scf) - print('Step 3. Put the Crazyflie somehere on the positive X-axis.') - print('Multiple samples can be recorded if you want to, type "r" before you hit enter to repeat the step.') + origin = None + do_repeat = True + while do_repeat: + input('Press return when ready. ') + print(' Recording...') + measurement = record_angles_average(scf) + do_repeat = False + if measurement is not None: + origin = measurement + else: + do_repeat = True + + print(f'Step 3. Put the Crazyflie on the positive X-axis, exactly {REFERENCE_DIST} meters from the origin. ' + + 'This position defines the direction of the X-axis, but it is also used for scaling of the system.') x_axis = [] do_repeat = True while do_repeat: - do_repeat = 'r' == input('Press return when ready. ').lower() + input('Press return when ready. ') print(' Recording...') - x_axis.append(record_angles_average(scf)) + measurement = record_angles_average(scf) + do_repeat = False + if measurement is not None: + x_axis = [measurement] + else: + do_repeat = True print('Step 4. Put the Crazyflie somehere in the XY-plane, but not on the X-axis.') print('Multiple samples can be recorded if you want to, type "r" before you hit enter to repeat the step.') @@ -201,18 +321,20 @@ def data_written(success): while do_repeat: do_repeat = 'r' == input('Press return when ready. ').lower() print(' Recording...') - xy_plane.append(record_angles_average(scf)) + measurement = record_angles_average(scf) + if measurement is not None: + xy_plane.append(measurement) + else: + do_repeat = True print() print('Step 5. We will now record data from the space you plan to fly in and optimize the base station ' + 'geometry based on this data. Move the Crazyflie around, try to cover all of the space, make sure ' + - 'all the base stations are received and do not move too fast. Make sure the Crazyflie is fairly ' + - 'level during the recording.') - print('This step does not add anything in a system with only one base station, enter 0 in this case.') - default_time = 10 + '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, default_time) + recording_time_s = parse_recording_time(recording_time, default_time) print(' Recording started...') samples = record_angles_sequence(scf, recording_time_s) print(' Recording ended') @@ -225,3 +347,14 @@ def data_written(success): 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') + connect_and_estimate(uri) diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py index f54c4caa2..8b3c01240 100644 --- a/test/localization/test_lighthouse_geometry_solver.py +++ b/test/localization/test_lighthouse_geometry_solver.py @@ -32,26 +32,6 @@ class TestLighthouseGeometrySolver(LighthouseTestBase): def setUp(self): self.fixtures = LighthouseFixtures() - def test_that_one_bs_poses_in_one_sample_is_estimated(self): - # Fixture - # CF_ORIGIN is used in the first sample and will define the global reference frame - bs_id0 = 3 - matched_samples = [ - LhCfPoseSample(angles_calibrated={ - bs_id0: self.fixtures.angles_cf_origin_bs0, - }), - ] - - initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) - - # Test - actual = LighthouseGeometrySolver.solve( - initial_guess, matched_samples, LhDeck4SensorPositions.positions) - - # Assert - bs_poses = actual.bs_poses - self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3) - def test_that_two_bs_poses_in_one_sample_are_estimated(self): # Fixture # CF_ORIGIN is used in the first sample and will define the global reference frame diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py index 3a91e29a7..1b620293a 100644 --- a/test/localization/test_lighthouse_initial_estimator.py +++ b/test/localization/test_lighthouse_initial_estimator.py @@ -34,7 +34,7 @@ class TestLighthouseInitialEstimator(LighthouseTestBase): def setUp(self): self.fixtures = LighthouseFixtures() - def test_that_one_bs_pose_is_found(self): + def test_that_one_bs_pose_raises_exception(self): # Fixture # CF_ORIGIN is used in the first sample and will define the global reference frame bs_id = 3 @@ -43,10 +43,9 @@ def test_that_one_bs_pose_is_found(self): ] # Test - actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) - # Assert - self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id], places=3) + with self.assertRaises(Exception): + LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) def test_that_two_bs_poses_in_same_sample_are_found(self): # Fixture @@ -161,6 +160,7 @@ def test_that_raises_for_isolated_bs(self): bs_id0 = 3 bs_id1 = 1 bs_id2 = 2 + bs_id3 = 4 samples = [ LhCfPoseSample(angles_calibrated={ bs_id0: self.fixtures.angles_cf_origin_bs0, @@ -168,6 +168,7 @@ def test_that_raises_for_isolated_bs(self): }), LhCfPoseSample(angles_calibrated={ bs_id2: self.fixtures.angles_cf1_bs2, + bs_id3: self.fixtures.angles_cf2_bs2, }), ] diff --git a/test/localization/test_lighthouse_system_aligner.py b/test/localization/test_lighthouse_system_aligner.py index db0660344..0e5cea781 100644 --- a/test/localization/test_lighthouse_system_aligner.py +++ b/test/localization/test_lighthouse_system_aligner.py @@ -73,7 +73,7 @@ def test_that_base_stations_are_rotated(self): bs_poses = {bs_id: Pose.from_rot_vec(t_vec=(1.0, 0.0, 1.0))} # Test - actual = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) + actual, transform = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) # Assert self.assertPosesAlmostEqual(Pose.from_rot_vec( @@ -90,7 +90,7 @@ def test_that_solution_is_de_flipped(self): expected = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(0.0, 0.0, 1.0)) # Test - actual = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) + actual, transform = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) # Assert self.assertPosesAlmostEqual(expected, actual[bs_id]) @@ -109,7 +109,7 @@ def test_that_is_aligned_for_multiple_points_where_system_is_rotated_and_poins_a } # Test - actual = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) + actual, transform = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) # Assert self.assertVectorsAlmostEqual((-1.0, 0.0, 1.0), actual[1].translation, places=1) From 6869b392af662bf74862b50b0af89a123036c733 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 15 Mar 2022 11:28:47 +0100 Subject: [PATCH 392/861] Added build-docs to CI build --- .github/workflows/CI.yml | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 0b158d4ff..f2a0e408b 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -12,6 +12,11 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - name: Check and build - run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build + - name: Checkout repo + uses: actions/checkout@v2 + + - name: Build + run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build + + - name: Build docs + run: docker run --rm -v ${PWD}:/module bitcraze/web-builder ./tools/build-docs/build-docs From 89d2c1d8adc60763d4f7ec25dbc8449b0f4be048 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 15 Mar 2022 13:57:51 +0100 Subject: [PATCH 393/861] Always pip install cflib in temp dir when building docs --- tools/build-docs/build-docs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs index 6ca7a555f..05c97dcb6 100755 --- a/tools/build-docs/build-docs +++ b/tools/build-docs/build-docs @@ -6,7 +6,7 @@ scriptDir=$(dirname $0) cflibDir=$scriptDir/../../cflib apiRefDir=$scriptDir/../../docs/api templateDir=$apiRefDir/template -tempDir=$scriptDir/.tmp +tempDir=$scriptDir/../../.tmp mkdir -p $apiRefDir mkdir -p $tempDir @@ -25,13 +25,13 @@ export PYTHONPATH=$PYTHONPATH:$scriptDir/.local/lib/python3.7/site-packages # The rewriting of HOME is a hack to make this work in a bare-bone Docker # environment. # -python3 -m pip show cflib || { # this will run if cflib is not found - HOME=$scriptDir pip3 install --user --upgrade pip - HOME=$scriptDir pip3 install --user -e $scriptDir/../../ -} + +# Make a temporary install in the temp dir +HOME=$tempDir pip3 install --user --upgrade pip +HOME=$tempDir pip3 install --user -e $scriptDir/../../ # Generate documentation -pdoc3 --force $cflibDir --output-dir $tempDir --template-dir $templateDir +HOME=$tempDir pdoc3 --force $cflibDir --output-dir $tempDir --template-dir $templateDir # Modify the generated content to fit our file tree ## Rename root file that will be included in another md file From a763f910f0e425aca3ce7c29eec29d21e332da34 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 15 Mar 2022 14:13:49 +0100 Subject: [PATCH 394/861] Removed info print --- cflib/localization/lighthouse_initial_estimator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 2f020de6e..0468df666 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -198,8 +198,8 @@ def _choose_solutions(cls, solutions_1: tuple[Pose, Pose], solutions_2: tuple[Po best1 = solution_1 best2 = solution_2 - if min_dist > 0.5: - print('large error:', min_dist) + # if min_dist > 0.5: + # print('large error:', min_dist) return best1, best2 From b4a07eb91b2ee44cede205d3e1104684942d764f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 15 Mar 2022 14:17:29 +0100 Subject: [PATCH 395/861] Enabled API doc for localization --- cflib/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/cflib/__init__.py b/cflib/__init__.py index ac9d3653d..7d4db36c1 100644 --- a/cflib/__init__.py +++ b/cflib/__init__.py @@ -55,5 +55,4 @@ ``` """ __pdoc__ = {} -__pdoc__['cflib.localization'] = False __pdoc__['cflib.crtp.cflinkcppdriver'] = False From 2796bdefdd5e2fc63d94d0400f61f0c752852004 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 17 Mar 2022 14:45:58 +0100 Subject: [PATCH 396/861] Bumped version of deck mem system to 2 --- cflib/crazyflie/mem/deck_memory.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index dd777c57a..50056d8ac 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -144,7 +144,7 @@ class DeckMemoryManager(MemoryElement): SIZE_OF_VERSION = 1 SIZE_OF_INFO_SECTION = SIZE_OF_VERSION + MAX_NR_OF_DECKS * SIZE_OF_DECK_MEM_INFO INFO_SECTION_ADDRESS = 0 - SUPPORTED_VERSION = 1 + SUPPORTED_VERSION = 2 def __init__(self, id, type, size, mem_handler): """Initialize deck memory manager""" @@ -220,7 +220,7 @@ def _parse_info_section(self, data): version = struct.unpack(' Date: Thu, 17 Mar 2022 16:42:59 +0100 Subject: [PATCH 397/861] Fixed logging wrong memory --- cflib/crazyflie/mem/deck_memory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 50056d8ac..412a43dae 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -55,7 +55,7 @@ def __init__(self, deck_memory_manager): def contains(self, address): max = self._base_address + self.MEMORY_MAX_SIZE - return address >= self._base_address and address <= max + return address >= self._base_address and address < max def write(self, address, data, write_complete_cb, write_failed_cb=None, progress_cb=None): """Write a block of binary data to the deck""" From 7172e65eddf3e587be1e575efd8884ccf4c68d8f Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 21 Mar 2022 13:25:34 +0100 Subject: [PATCH 398/861] Update version to 0.1.18.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index bdf09eea3..c3cda5da4 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.17.1', + version='0.1.18.0', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 94cf0f8f39e0556af1a2dfb11e9d2a7668f15b49 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 23 Mar 2022 11:27:47 +0100 Subject: [PATCH 399/861] update version to 0.1.18.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index c3cda5da4..315d0e218 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.18.0', + version='0.1.18.1', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 0e5c42321ba49a048a05e627aa48233bcd5d737b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 23 Mar 2022 17:18:08 +0100 Subject: [PATCH 400/861] Updated deck memory mapping --- cflib/crazyflie/mem/deck_memory.py | 76 +++++++++++++++++++++++------- 1 file changed, 59 insertions(+), 17 deletions(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 412a43dae..88bef4cef 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -42,16 +42,27 @@ class DeckMemory: MASK_UPGRADE_REQUIRED = 32 MASK_BOOTLOADER_ACTIVE = 64 + MASK_SUPPORTS_RESET_TO_FW = 1 + MASK_SUPPORTS_RESET_TO_BOOTLOADER = 2 + + FLAG_COMMAND_RESET_TO_FW = 1 + FLAG_COMMAND_RESET_TO_BOOTLOADER = 2 + MEMORY_MAX_SIZE = 0x10000000 - def __init__(self, deck_memory_manager): + ADR_FW_NEW_FLASH = 0 + ADR_COMMAND_BIT_FIELD = 4 + + def __init__(self, deck_memory_manager: "DeckMemoryManager", _command_base_address): self._deck_memory_manager = deck_memory_manager self.required_hash = None self.required_length = None self.name = None self._base_address = None - self._bit_field = 0 + self._command_base_address = _command_base_address + self._bit_field1 = 0 + self._bit_field2 = 0 def contains(self, address): max = self._base_address + self.MEMORY_MAX_SIZE @@ -95,41 +106,72 @@ def read_sync(self, address, length): @property def is_valid(self): - return (self._bit_field & self.MASK_IS_VALID) != 0 + return (self._bit_field1 & self.MASK_IS_VALID) != 0 @property def is_started(self): - return (self._bit_field & self.MASK_IS_STARTED) != 0 + return (self._bit_field1 & self.MASK_IS_STARTED) != 0 @property def supports_read(self): - return (self._bit_field & self.MASK_SUPPORTS_READ) != 0 + return (self._bit_field1 & self.MASK_SUPPORTS_READ) != 0 @property def supports_write(self): - return (self._bit_field & self.MASK_SUPPORTS_WRITE) != 0 + return (self._bit_field1 & self.MASK_SUPPORTS_WRITE) != 0 @property def supports_fw_upgrade(self): - return (self._bit_field & self.MASK_SUPPORTS_UPGRADE) != 0 + return (self._bit_field1 & self.MASK_SUPPORTS_UPGRADE) != 0 @property def is_fw_upgrade_required(self): - return (self._bit_field & self.MASK_UPGRADE_REQUIRED) != 0 + return (self._bit_field1 & self.MASK_UPGRADE_REQUIRED) != 0 @property def is_bootloader_active(self): - return (self._bit_field & self.MASK_BOOTLOADER_ACTIVE) != 0 + return (self._bit_field1 & self.MASK_BOOTLOADER_ACTIVE) != 0 + + @property + def supports_reset_to_fw(self): + return (self._bit_field2 & self.MASK_SUPPORTS_RESET_TO_FW) != 0 + + @property + def supports_reset_to_bootloader(self): + return (self._bit_field2 & self.MASK_SUPPORTS_RESET_TO_BOOTLOADER) != 0 + + def reset_to_fw(self): + data = struct.pack(' Date: Wed, 23 Mar 2022 17:23:04 +0100 Subject: [PATCH 401/861] Updated bootloader --- cflib/bootloader/__init__.py | 43 ++++++++++++++++++++++++++---------- 1 file changed, 31 insertions(+), 12 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index d66daccdc..8f849e20d 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -406,7 +406,7 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): # Check that we want to flash this deck deck_target = [t for t in targets if t == Target('deck', deck.name, 'fw')] if (not flash_all_targets) and len(deck_target) == 0: - print(f'Skipping {deck.name}') + print(f'Skipping {deck.name}, not in the target list') continue # Check that we have an artifact for this deck @@ -420,36 +420,55 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): self.progress_cb(f'Updating deck {deck.name}', 0) # Test and wait for the deck to be started + timeout_time = time.time() + 5 while not deck.is_started: + if time.time() > timeout_time: + raise RuntimeError(f"Deck {deck.name} did not start") print('Deck not yet started ...') - time.sleep(500) + time.sleep(0.5) deck = mgr.query_decks()[deck_index] - # Run a brunch of sanity checks ... + # Supports upgrades? if not deck.supports_fw_upgrade: print(f'Deck {deck.name} does not support firmware update, skipping!') continue - if not deck.is_fw_upgrade_required: - print(f'Deck {deck.name} firmware up to date, skipping') - continue + # Reset to bootloader mode, if supported + if deck.supports_reset_to_bootloader: + print(f'Deck {deck.name}, reset to bootloader') + deck.reset_to_bootloader() - if not deck.is_bootloader_active: - print(f'Error: Deck {deck.name} bootloader not active, skipping!') - continue + time.sleep(0.1) + deck = mgr.query_decks()[deck_index] + else: + # Is an upgrade required? + if not deck.is_fw_upgrade_required: + print(f'Deck {deck.name} firmware up to date, skipping') + continue + + # Wait for bootloader to be ready + timeout_time = time.time() + 5 + while not deck.is_bootloader_active: + if time.time() > timeout_time: + raise RuntimeError(f"Deck {deck.name} did not enter bootloader mode") + print(f'Error: Deck {deck.name} bootloader not active yet...') + time.sleep(0.5) + deck = mgr.query_decks()[deck_index] progress_cb = self.progress_cb if not progress_cb: def progress_cb(msg: str, percent: int): frames = ['|', '/', '-', '\\'] frame = frames[int(percent) % 4] - print('{} {}% {}'.format(frame, percent, msg)) + print(f'{frame} {percent}% {msg}') + # Flash the new firmware + deck.set_fw_new_flash_size(len(deck_artifact.content)) result = deck.write_sync(0, deck_artifact.content, progress_cb) if result: if self.progress_cb: - self.progress_cb(f'Deck {deck.name} updated succesfully!', 100) + self.progress_cb(f'Deck {deck.name} updated successful!', 100) else: if self.progress_cb: self.progress_cb(f'Failed to update deck {deck.name}', int(0)) - raise Exception(f'Failed to update deck {deck.name}') + raise RuntimeError(f'Failed to update deck {deck.name}') From c852045e09ace4565ddda124a3e29a8323f51cb6 Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 23 Mar 2022 21:09:52 +0100 Subject: [PATCH 402/861] Connection working, needs cleanup --- cflib/crtp/__init__.py | 3 +- cflib/crtp/cpxdriver.py | 257 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 259 insertions(+), 1 deletion(-) create mode 100644 cflib/crtp/cpxdriver.py diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index b3fe1b77a..6d0d2dd8b 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -32,6 +32,7 @@ from .serialdriver import SerialDriver from .udpdriver import UdpDriver from .usbdriver import UsbDriver +from .cpxdriver import CPXDriver __author__ = 'Bitcraze AB' __all__ = [] @@ -58,7 +59,7 @@ def init_drivers(enable_debug_driver=False, enable_serial_driver=False): if enable_serial_driver: CLASSES.append(SerialDriver) - CLASSES.extend([UdpDriver, PrrtDriver]) + CLASSES.extend([UdpDriver, PrrtDriver, CPXDriver]) def scan_interfaces(address=None): diff --git a/cflib/crtp/cpxdriver.py b/cflib/crtp/cpxdriver.py new file mode 100644 index 000000000..54353c2f5 --- /dev/null +++ b/cflib/crtp/cpxdriver.py @@ -0,0 +1,257 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2022 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 . +""" CRTP CPX Driver. Tunnel CRTP over CPX to the Crazyflie STM32 """ +import queue +import re +import socket +import struct +import threading +from urllib.parse import urlparse + +from .crtpdriver import CRTPDriver +from .crtpstack import CRTPPacket +from .exceptions import WrongUriType + +__author__ = 'Bitcraze AB' +__all__ = ['CPXDriver'] + +class CPXTarget: + """ + List of CPX targets + """ + STM32 = 1 + ESP32 = 2 + HOST = 3 + GAP8 = 4 + +class CPXFunction: + """ + List of CPX targets + """ + SYSTEM = 1 + CONSOLE = 2 + CRTP = 3 + WIFI_CTRL = 4 + APP = 5 + TEST = 0x0E + BOOTLOADER = 0x0F + +class CPXPacket(object): + """ + A packet with routing and data + """ + + def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytearray(), wireHeader=None): + """ + Create an empty packet with default values. + """ + self.data = data + self.source = source + self.destination = destination + self.function = function + self._wireHeaderFormat = "> 3) & 0x07 + self.source = targetsAndFlags & 0x07 + self.lastPacket = targetsAndFlags & 0x40 != 0 + + def _get_wire_data(self): + """Create raw data to send via the wire""" + raw = bytearray() + # This is the length excluding the 2 byte legnth + wireLength = len(self.data) + 2 # 2 bytes for CPX header + targetsAndFlags = ((self.source & 0x7) << 3) | (self.destination & 0x7) + if self.lastPacket: + targetsAndFlags |= 0x40 + #print(self.destination) + #print(self.source) + #print(targets) + function = self.function & 0xFF + raw.extend(struct.pack(self._wireHeaderFormat, wireLength, targetsAndFlags, function)) + raw.extend(self.data) + + # We need to handle this better... + if (wireLength > 1022): + raise "Cannot send this packet, the size is too large!" + + return raw + + def __str__(self): + """Get a string representation of the packet""" + return "{:02X}->{:02X}/{:02X}".format(self.source, self.destination, self.function) + + wireData = property(_get_wire_data, None) + +class CPX(object): + """ + A packet with routing and data + """ + + def __init__(self, socket): + self._socket = socket + + def _rx_bytes(self, size): + data = bytearray() + while len(data) < size: + #print(size - len(data)) + data.extend(self._socket.recv(size-len(data))) + return data + + def send(self, packet): + self._socket.send(packet.wireData) + + def receive(self): + header = self._rx_bytes(4) + packet = CPXPacket(wireHeader=header) + packet.data = self._rx_bytes(packet.length - 2) # remove routing info here + return packet + + def transaction(self, packet): + self.send(packet) + return self.receive() + +class CPXDriver(CRTPDriver): + + def __init__(self): + self.needs_resending = False + + def connect(self, uri, linkQualityCallback, linkErrorCallback): + if not re.search('^cpx://', uri): + raise WrongUriType('Not an UDP URI') + + parse = urlparse(uri.split(" ")[0]) + + print("Connecting to socket on {}:{}...".format(parse.hostname, parse.port)) + self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._socket.connect((parse.hostname, parse.port)) + print("Socket connected") + + self.in_queue = queue.Queue() + + self._cpx = CPX(self._socket) + + self._thread = _UsbReceiveThread(self._cpx, self.in_queue, + linkErrorCallback) + self._thread.start() + + + self._cpx.send(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.SYSTEM, + data=[0x10, 0x01])) + + def receive_packet(self, time=0): + if time == 0: + try: + return self.in_queue.get(False) + except queue.Empty: + return None + elif time < 0: + try: + return self.in_queue.get(True) + except queue.Empty: + return None + else: + try: + return self.in_queue.get(True, time) + except queue.Empty: + return None + + def send_packet(self, pk): + raw = (pk.header,) + struct.unpack('B' * len(pk.data), pk.data) + self._cpx.send(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.CRTP, + data=raw)) + + def close(self): + """ Close the link. """ + # Stop the comm thread + self._thread.stop() + + # Close the USB dongle + try: + # Should close socket here! + pass + + except Exception as e: + # If we pull out the dongle we will not make this call + logger.info('Could not close {}'.format(e)) + pass + self._cpx = None + + def get_name(self): + return 'cpx' + + def scan_interface(self, address): + return [ + ("cpx://192.168.6.57:5000", "Office"), + ("cpx://192.168.1.236:5000", "Home") + ] + +# Transmit/receive radio thread +class _UsbReceiveThread(threading.Thread): + """ + Radio link receiver thread used to read data from the + Crazyradio USB driver. """ + + def __init__(self, cpx, inQueue, link_error_callback): + """ Create the object """ + threading.Thread.__init__(self) + self._cpx = cpx + self.in_queue = inQueue + self.sp = False + self.link_error_callback = link_error_callback + + def stop(self): + """ Stop the thread """ + self.sp = True + try: + self.join() + except Exception: + pass + + def run(self): + """ Run the receiver thread """ + + while (True): + if (self.sp): + break + try: + # Block until a full packet is available though the socket + cpxPacket = self._cpx.receive() + data = struct.unpack('B' * len(cpxPacket.data), cpxPacket.data) + if len(data) > 0: + pk = CRTPPacket(data[0], + list(data[1:])) + self.in_queue.put(pk) + except Exception as e: + import traceback + + self.link_error_callback( + 'Error communicating with the Crazyflie\n' + 'Exception:%s\n\n%s' % (e, + traceback.format_exc())) \ No newline at end of file From 29975d617956777a8d492e636b51304e306369ef Mon Sep 17 00:00:00 2001 From: Marcus Date: Sat, 26 Mar 2022 08:46:57 +0100 Subject: [PATCH 403/861] Added zeroconf requirement and reconnecting --- cflib/crtp/cpxdriver.py | 58 +++++++++++++++++++++++++++++++---------- requirements.txt | 1 + 2 files changed, 45 insertions(+), 14 deletions(-) diff --git a/cflib/crtp/cpxdriver.py b/cflib/crtp/cpxdriver.py index 54353c2f5..f8f7d8d68 100644 --- a/cflib/crtp/cpxdriver.py +++ b/cflib/crtp/cpxdriver.py @@ -29,6 +29,7 @@ import struct import threading from urllib.parse import urlparse +from zeroconf import ServiceBrowser, Zeroconf from .crtpdriver import CRTPDriver from .crtpstack import CRTPPacket @@ -135,6 +136,39 @@ def transaction(self, packet): self.send(packet) return self.receive() +# For each scan the driver is re-initialized, if we do ZeroConf inside +# the driver init we will not have time to find any devices, so start +# ZeroCont at startup and keep it running all the time. The driver +# will just query this to return discovered devices. +persistentZeroContListener = None +class ZeroConfListener: + def __init__(self): + self._hosts = [] + + zeroconf = Zeroconf() + browser = ServiceBrowser(zeroconf, "_cpx._tcp.local.", self) + + def remove_service(self, zeroconf, type, name): + print("Service %s removed" % (name,)) + info = zeroconf.get_service_info(type, name) + self._hosts.remove(info) + + def add_service(self, zeroconf, type, name): + info = zeroconf.get_service_info(type, name) + print("Service %s added, service info: %s" % (name, info)) + self._hosts.append(info) + + def getAvailableHosts(self): + cpxHosts = [] + for hosts in self._hosts: + cpxHosts.append(("cpx://{}:{}".format(hosts.server, hosts.port), hosts.properties[b"name"])) + return cpxHosts + + def update_service(self, zeroconf, type, name): + print("Updated service") + +persistentZeroContListener = ZeroConfListener() + class CPXDriver(CRTPDriver): def __init__(self): @@ -149,13 +183,12 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): print("Connecting to socket on {}:{}...".format(parse.hostname, parse.port)) self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._socket.connect((parse.hostname, parse.port)) - print("Socket connected") self.in_queue = queue.Queue() self._cpx = CPX(self._socket) - self._thread = _UsbReceiveThread(self._cpx, self.in_queue, + self._thread = _CPXReceiveThread(self._cpx, self.in_queue, linkErrorCallback) self._thread.start() @@ -192,13 +225,12 @@ def close(self): # Stop the comm thread self._thread.stop() - # Close the USB dongle + # Close the socket try: - # Should close socket here! - pass + self._socket.close() + self._socket = None except Exception as e: - # If we pull out the dongle we will not make this call logger.info('Could not close {}'.format(e)) pass self._cpx = None @@ -207,16 +239,13 @@ def get_name(self): return 'cpx' def scan_interface(self, address): - return [ - ("cpx://192.168.6.57:5000", "Office"), - ("cpx://192.168.1.236:5000", "Home") - ] + return persistentZeroContListener.getAvailableHosts() -# Transmit/receive radio thread -class _UsbReceiveThread(threading.Thread): +# Transmit/receive thread +class _CPXReceiveThread(threading.Thread): """ Radio link receiver thread used to read data from the - Crazyradio USB driver. """ + Socket. """ def __init__(self, cpx, inQueue, link_error_callback): """ Create the object """ @@ -241,7 +270,8 @@ def run(self): if (self.sp): break try: - # Block until a full packet is available though the socket + # Block until a packet is available though the socket + # CPX receive will only return full packets cpxPacket = self._cpx.receive() data = struct.unpack('B' * len(cpxPacket.data), cpxPacket.data) if len(data) > 0: diff --git a/requirements.txt b/requirements.txt index 8ba626a4e..c31386d93 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1 +1,2 @@ pyusb>=1.0.0b2 +zeroconf>=0.26.1 From 60892bb5ff896fcc3eccf78c5d3bf7b366495f12 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 5 Apr 2022 15:43:24 +0200 Subject: [PATCH 404/861] Can run CRTP and CPX to GAP8 at the same time --- cflib/cpx/__init__.py | 196 +++++++++++++++++++++++++++++++ cflib/cpx/gap8/__init__.py | 0 cflib/cpx/gap8/bootloader.py | 57 +++++++++ cflib/cpx/transports.py | 63 ++++++++++ cflib/crtp/cpxdriver.py | 129 +++----------------- examples/aideck/crtp-cpx-wifi.py | 101 ++++++++++++++++ 6 files changed, 432 insertions(+), 114 deletions(-) create mode 100644 cflib/cpx/__init__.py create mode 100644 cflib/cpx/gap8/__init__.py create mode 100644 cflib/cpx/gap8/bootloader.py create mode 100644 cflib/cpx/transports.py create mode 100644 examples/aideck/crtp-cpx-wifi.py diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py new file mode 100644 index 000000000..9108915a7 --- /dev/null +++ b/cflib/cpx/__init__.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2022 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 . +""" CPX Router and discovery""" +import queue +import socket +import struct +import threading +import enum +from urllib.parse import urlparse +from zeroconf import ServiceBrowser, Zeroconf +from .gap8.bootloader import GAP8Bootloader + +__author__ = 'Bitcraze AB' +__all__ = ['CPXRouter'] + +print("CPX import") + +class CPXTarget(enum.Enum): + """ + List of CPX targets + """ + STM32 = 1 + ESP32 = 2 + HOST = 3 + GAP8 = 4 + +class CPXFunction(enum.Enum): + """ + List of CPX targets + """ + SYSTEM = 1 + CONSOLE = 2 + CRTP = 3 + WIFI_CTRL = 4 + APP = 5 + TEST = 0x0E + BOOTLOADER = 0x0F + +class CPXPacket(object): + """ + A packet with routing and data + """ + + def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytearray(), wireHeader=None): + """ + Create an empty packet with default values. + """ + self.data = data + self.source = source + self.destination = destination + self.function = function + self._wireHeaderFormat = "> 3) & 0x07) + self.destination = CPXTarget(targetsAndFlags & 0x07) + self.lastPacket = targetsAndFlags & 0x40 != 0 + self.function = CPXFunction(self.function) + + def _get_wire_data(self): + """Create raw data to send via the wire""" + raw = bytearray() + # This is the length excluding the 2 byte legnth + wireLength = len(self.data) + 2 # 2 bytes for CPX header + targetsAndFlags = ((self.source.value & 0x7) << 3) | (self.destination.value & 0x7) + if self.lastPacket: + targetsAndFlags |= 0x40 + #print(self.destination) + #print(self.source) + #print(targets) + function = self.function.value & 0xFF + raw.extend(struct.pack(self._wireHeaderFormat, wireLength, targetsAndFlags, function)) + raw.extend(self.data) + + # We need to handle this better... + if (wireLength > 1022): + raise "Cannot send this packet, the size is too large!" + + return raw + + def __str__(self): + """Get a string representation of the packet""" + return "{}->{}/{} (size={} bytes)".format(self.source, self.destination, self.function, self.length) + + wireData = property(_get_wire_data, None) + +# Internal here, route to modules and from public facing API +class CPXRouter(threading.Thread): + + def __init__(self, transport): + threading.Thread.__init__(self) + self._transport = transport + self._rxQueues = {} + self._packet_assembly = [] + + # Register and/or blocking calls for ports + def receivePacket(self, function): + + # Check if a queue exists, if not then create it + # the user might have implemented new functions + if not function.value in self._rxQueues: + print("Creating queue for {}".format(function)) + self._rxQueues[function.value] = queue.Queue() + + return self._rxQueues[function.value].get(block=True) + + def makeTransaction(self, packet): + self.sendPacket(packet) + return self.getPacket(packet.function) + + def sendPacket(self, packet): + # Do we queue here? + self._transport.write(packet.wireData) + + def run(self): + while(1): + # Read one packet from the transport + + # Packages might have been split up along the + # way, due to MTU limitations on links. But here we have + # lots of memory, so assemble full packets by looking at last + # packet byte. Note that chunks of one packet could be mixed + # with chunks from antother packet. + header = self._transport.read(4) + packet = CPXPacket(wireHeader=header) + packet.data = self._transport.read(packet.length - 2) # remove routing info here + #print(packet) + # if not packet.target in self._packet_assembly: + # self._packet_assembly[packet.target][packet.function] = [] + # else + # if not packet.function in self._packet_assembly[packet.target]: + # self._packet_assembly[packet.target][packet.function] = [] + + # self._packet_assembly[packet.target][packet.function].append(packet) + + # if (packet.lastPart): + # # Assemble packet and send up stack + # pass + if not packet.function.value in self._rxQueues: + pass + #print("Got packet for {}, but have no queue".format(packet.function)) + else: + self._rxQueues[packet.function.value].put(packet) + #self._rxQueues[packet.function.value] = queue.Queue() + #print(self._rxQueues[packet.function.value].qsize()) + + +# Public facing +class CPX: + def __init__(self, transport): + self._router = CPXRouter(transport) + self._router.start() + #self.gap8.bootloader = GAP8Bootloader(self) + + def receivePacket(self, function): + # Block on a function queue + + #if self._router.isUsedBySubmodule(target, function): + # raise ValueError("The CPX target {} and function {} is registered in a sub module".format(target, function)) + + # Will block on queue + return self._router.receivePacket(function) + + def makeTransaction(self, packet): + return self._router.makeTransaction(packet) + + def sendPacket(self, packet): + self._router.sendPacket(packet) + + def close(self): + print("Should close transport connection") \ No newline at end of file diff --git a/cflib/cpx/gap8/__init__.py b/cflib/cpx/gap8/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cflib/cpx/gap8/bootloader.py b/cflib/cpx/gap8/bootloader.py new file mode 100644 index 000000000..4e2546ad9 --- /dev/null +++ b/cflib/cpx/gap8/bootloader.py @@ -0,0 +1,57 @@ + +#from cflib.cpx import CPXTarget, CPXFunction + +class GAP8Bootloader: + + #target = CPXTarget.GAP8 + #function = CPXFunction.BOOTLOADER + + def __init__(self): + print("GAP8 bootloader module init") + + # def getVersion(self): + # version = self._cpx.transaction(CPXPacket(destination=CPXTarget.GAP8, + # function=CPXFunction.BOOTLOADER, + # data=bytearray([0x00]))) + # return version.data[1:] + + # def readFlash(self, start, count): + # cmd = struct.pack("> 3) & 0x07 - self.source = targetsAndFlags & 0x07 - self.lastPacket = targetsAndFlags & 0x40 != 0 - - def _get_wire_data(self): - """Create raw data to send via the wire""" - raw = bytearray() - # This is the length excluding the 2 byte legnth - wireLength = len(self.data) + 2 # 2 bytes for CPX header - targetsAndFlags = ((self.source & 0x7) << 3) | (self.destination & 0x7) - if self.lastPacket: - targetsAndFlags |= 0x40 - #print(self.destination) - #print(self.source) - #print(targets) - function = self.function & 0xFF - raw.extend(struct.pack(self._wireHeaderFormat, wireLength, targetsAndFlags, function)) - raw.extend(self.data) - - # We need to handle this better... - if (wireLength > 1022): - raise "Cannot send this packet, the size is too large!" - - return raw - - def __str__(self): - """Get a string representation of the packet""" - return "{:02X}->{:02X}/{:02X}".format(self.source, self.destination, self.function) - - wireData = property(_get_wire_data, None) - -class CPX(object): - """ - A packet with routing and data - """ - - def __init__(self, socket): - self._socket = socket - - def _rx_bytes(self, size): - data = bytearray() - while len(data) < size: - #print(size - len(data)) - data.extend(self._socket.recv(size-len(data))) - return data - - def send(self, packet): - self._socket.send(packet.wireData) - - def receive(self): - header = self._rx_bytes(4) - packet = CPXPacket(wireHeader=header) - packet.data = self._rx_bytes(packet.length - 2) # remove routing info here - return packet - - def transaction(self, packet): - self.send(packet) - return self.receive() - # For each scan the driver is re-initialized, if we do ZeroConf inside # the driver init we will not have time to find any devices, so start # ZeroCont at startup and keep it running all the time. The driver @@ -180,22 +85,18 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): parse = urlparse(uri.split(" ")[0]) - print("Connecting to socket on {}:{}...".format(parse.hostname, parse.port)) - self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self._socket.connect((parse.hostname, parse.port)) - self.in_queue = queue.Queue() - self._cpx = CPX(self._socket) + self.cpx = CPX(SocketTransport(parse.hostname, parse.port)) - self._thread = _CPXReceiveThread(self._cpx, self.in_queue, + self._thread = _CPXReceiveThread(self.cpx, self.in_queue, linkErrorCallback) self._thread.start() - self._cpx.send(CPXPacket(destination=CPXTarget.STM32, - function=CPXFunction.SYSTEM, - data=[0x10, 0x01])) + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.SYSTEM, + data=[0x10, 0x01])) def receive_packet(self, time=0): if time == 0: @@ -216,9 +117,9 @@ def receive_packet(self, time=0): def send_packet(self, pk): raw = (pk.header,) + struct.unpack('B' * len(pk.data), pk.data) - self._cpx.send(CPXPacket(destination=CPXTarget.STM32, - function=CPXFunction.CRTP, - data=raw)) + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.CRTP, + data=raw)) def close(self): """ Close the link. """ @@ -227,13 +128,13 @@ def close(self): # Close the socket try: - self._socket.close() - self._socket = None + self._cpx.close() + self._cpx = None except Exception as e: logger.info('Could not close {}'.format(e)) pass - self._cpx = None + self.cpx = None def get_name(self): return 'cpx' @@ -272,7 +173,7 @@ def run(self): try: # Block until a packet is available though the socket # CPX receive will only return full packets - cpxPacket = self._cpx.receive() + cpxPacket = self._cpx.receivePacket(CPXFunction.CRTP) data = struct.unpack('B' * len(cpxPacket.data), cpxPacket.data) if len(data) > 0: pk = CRTPPacket(data[0], diff --git a/examples/aideck/crtp-cpx-wifi.py b/examples/aideck/crtp-cpx-wifi.py new file mode 100644 index 000000000..9ba2199ad --- /dev/null +++ b/examples/aideck/crtp-cpx-wifi.py @@ -0,0 +1,101 @@ + +""" +This example illustrates how to use CRTP over CPX while also using CPX +to access other targets on the Crazyflie. This is done by connecting to +the Crazyflie and reading out images from the WiFi streamer and naming +them according to the post of the Crazyflie. + +For the example to work you will need the WiFi example flashed on the +GAP8 and an up to date code-base on the STM32/ESP32. + +""" + +import logging +import time +import queue +import threading +import struct + +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.utils import uri_helper + +from cflib.cpx import CPXPacket, CPXFunction, CPXTarget + +uri = uri_helper.uri_from_env(default='cpx://aideck-B09D74.local:5000') + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + +class Example(threading.Thread): + + def __init__(self, uri): + threading.Thread.__init__(self) + self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + self._lg_stab.add_variable('stateEstimate.x', 'float') + self._lg_stab.add_variable('stateEstimate.y', 'float') + self._lg_stab.add_variable('stateEstimate.z', 'float') + self._lg_stab.add_variable('stabilizer.roll', 'float') + self._lg_stab.add_variable('stabilizer.pitch', 'float') + self._lg_stab.add_variable('stabilizer.yaw', 'float') + + self._cf = Crazyflie(rw_cache='./cache') + self._cf.connected.add_callback(self.connected) + self._cf.disconnected.add_callback(self.disconnected) + self._cf.open_link(uri) + self._cpx = None + + def connected(self, uri): + print("Connected to {}".format(uri)) + self._cpx = self._cf.link.cpx + self.start() + try: + self._cf.log.add_config(self._lg_stab) + # This callback will receive the data + self._lg_stab.data_received_cb.add_callback(self._stab_log_data) + # This callback will be called on errors + self._lg_stab.error_cb.add_callback(self._stab_log_error) + # Start the logging + self._lg_stab.start() + except KeyError as e: + print('Could not start log configuration,' + '{} not found in TOC'.format(str(e))) + except AttributeError: + print('Could not add Stabilizer log config, bad configuration.') + + def disconnected(self, uri): + print("Disconnected from {}".format(uri)) + + 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): + print(f'[{timestamp}][{logconf.name}]: ', end='') + for name, value in data.items(): + print(f'{name}: {value:3.3f} ', end='') + print() + + def run(self): + while True: + p = self._cpx.receivePacket(CPXFunction.APP) + [magic, width, height, depth, format, size] = struct.unpack(' Date: Thu, 7 Apr 2022 13:24:05 +0200 Subject: [PATCH 405/861] Logging imrpovements --- cflib/bootloader/__init__.py | 11 +++++++++++ cflib/crazyflie/mem/deck_memory.py | 2 +- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 8f849e20d..56d77ef60 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -384,6 +384,14 @@ def _get_platform_id(self): return identifier + + def console_callback(self, 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='') + + def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): flash_all_targets = len(targets) == 0 @@ -391,6 +399,9 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): self.progress_cb('Detecting deck to be updated', 0) with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: + # Uncomment to enable console logs from the CF. Useful when debuging deck flashing. + # scf.cf.console.receivedChar.add_callback(self.console_callback) + deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) deck_mems_count = len(deck_mems) if deck_mems_count == 0: diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 88bef4cef..f7b4e015a 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -263,7 +263,7 @@ def _parse_info_section(self, data): version = struct.unpack(' Date: Thu, 7 Apr 2022 15:23:00 +0200 Subject: [PATCH 406/861] Handle invalid deck mem protocol version --- cflib/bootloader/__init__.py | 13 ++++++++++--- cflib/crazyflie/mem/deck_memory.py | 30 ++++++++++++++++++++++-------- cflib/utils/callbacks.py | 2 +- 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 56d77ef60..6240ec6d7 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -394,12 +394,11 @@ def console_callback(self, text: str): def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): flash_all_targets = len(targets) == 0 - if self.progress_cb: self.progress_cb('Detecting deck to be updated', 0) with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: - # Uncomment to enable console logs from the CF. Useful when debuging deck flashing. + # Uncomment to enable console logs from the CF. # scf.cf.console.receivedChar.add_callback(self.console_callback) deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) @@ -408,7 +407,15 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): return mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0]) - decks = mgr.query_decks() + try: + decks = mgr.query_decks() + except RuntimeError as e: + if self.progress_cb: + message = f'Failed to read decks: {str(e)}' + self.progress_cb(message, 0) + logger.error(message) + time.sleep(2) + raise RuntimeError(message) for (deck_index, deck) in decks.items(): if self.terminate_flashing_cb and self.terminate_flashing_cb(): diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index f7b4e015a..d77789dbf 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -195,6 +195,7 @@ def __init__(self, id, type, size, mem_handler): super(DeckMemoryManager, self).__init__(id=id, type=type, size=size, mem_handler=mem_handler) self._query_complete_cb = None + self._query_failed_cb = None self.deck_memories = {} self._read_complete_cb = None @@ -203,13 +204,16 @@ def __init__(self, id, type, size, mem_handler): self._write_complete_cb = None self._write_failed_cb = None + self._error = None - def query_decks(self, query_complete_cb): + def query_decks(self, query_complete_cb, query_failed_cb=None): if self._query_complete_cb is not None: raise Exception('Query ongoing') + self._error = None self.deck_memories = {} self._query_complete_cb = query_complete_cb + self._query_failed_cb = query_failed_cb self.mem_handler.read(self, self.INFO_SECTION_ADDRESS, self.SIZE_OF_INFO_SECTION) def _read(self, base_address, address, length, read_complete_cb, read_failed_cb): @@ -228,10 +232,16 @@ def _new_data(self, mem, addr, data): """Callback when new memory data has been fetched""" if mem.id == self.id: if addr == self.INFO_SECTION_ADDRESS: - self.deck_memories = self._parse_info_section(data) - tmp_cb = self._query_complete_cb - self._clear_query_cb() - tmp_cb(self.deck_memories) + try: + self.deck_memories = self._parse_info_section(data) + tmp_cb = self._query_complete_cb + self._clear_query_cb() + tmp_cb(self.deck_memories) + except RuntimeError as e: + tmp_cb = self._query_failed_cb + self._clear_query_cb() + if tmp_cb: + tmp_cb(str(e)) else: tmp_cb = self._read_complete_cb self._clear_read_cb() @@ -253,6 +263,7 @@ def _new_data_failed(self, mem, addr, data): def _clear_query_cb(self): self._query_complete_cb = None + self._query_failed_cb = None def _clear_read_cb(self): self._read_complete_cb = None @@ -263,7 +274,7 @@ def _parse_info_section(self, data): version = struct.unpack(' Date: Thu, 7 Apr 2022 17:17:52 +0200 Subject: [PATCH 407/861] Update opencv-python-headess --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 315d0e218..5522f86de 100644 --- a/setup.py +++ b/setup.py @@ -37,7 +37,7 @@ 'libusb-package~=1.0', 'scipy~=1.7', 'numpy>=1.20,<1.25', - 'opencv-python-headless~=4.5.1', + 'opencv-python-headless~=4.5.5', ], # $ pip install -e .[dev] From f451ec100b077994643370bb5e35b9b2a38003e4 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 7 Apr 2022 17:24:37 +0200 Subject: [PATCH 408/861] Reboot CF between flashing dekcs --- cflib/bootloader/__init__.py | 34 +++++++++++++++++++++++++--------- cflib/utils/power_switch.py | 18 +++++++++++++++--- 2 files changed, 40 insertions(+), 12 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 6240ec6d7..eda5f1b74 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -44,6 +44,7 @@ from cflib.crazyflie.mem import deck_memory from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils.power_switch import PowerSwitch logger = logging.getLogger(__name__) @@ -166,10 +167,14 @@ def flash(self, filename: str, targets: List[Target], cf=None): self.close() time.sleep(3) - self._flash_deck(deck_artifacts, deck_targets) - - if self.progress_cb: - self.progress_cb('Deck updated! Restarting firmware.', int(100)) + # Flash all decks and reboot after each deck + current_index = 0 + while current_index != -1: + current_index = self._flash_deck_incrementally(deck_artifacts, deck_targets, current_index) + self.progress_cb('Deck updated! Restarting...', int(100)) + if current_index != -1: + PowerSwitch(self.clink).reboot_to_fw() + time.sleep(3) # Put the crazyflie back in Bootloader mode to exit the function in the same state we entered it self.start_bootloader(warm_boot=True, cf=cf) @@ -384,18 +389,16 @@ def _get_platform_id(self): return identifier - def console_callback(self, 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='') - - def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): + def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: List[Target], start_index: int): flash_all_targets = len(targets) == 0 if self.progress_cb: - self.progress_cb('Detecting deck to be updated', 0) + self.progress_cb('Identifying deck to be updated', 0) with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: # Uncomment to enable console logs from the CF. @@ -404,7 +407,7 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) deck_mems_count = len(deck_mems) if deck_mems_count == 0: - return + return -1 mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0]) try: @@ -421,6 +424,10 @@ def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): if self.terminate_flashing_cb and self.terminate_flashing_cb(): raise Exception('Flashing terminated') + # Skip decks up to the start_index + if deck_index < start_index: + continue + # Check that we want to flash this deck deck_target = [t for t in targets if t == Target('deck', deck.name, 'fw')] if (not flash_all_targets) and len(deck_target) == 0: @@ -490,3 +497,12 @@ def progress_cb(msg: str, percent: int): if self.progress_cb: self.progress_cb(f'Failed to update deck {deck.name}', int(0)) raise RuntimeError(f'Failed to update deck {deck.name}') + + # We flashed a deck, return for re-boot + next_index = deck_index + 1 + if next_index >= len(decks): + next_index = -1 + return next_index + + # We have flashed the last deck + return -1 diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py index 671f3b3fa..b53054309 100644 --- a/cflib/utils/power_switch.py +++ b/cflib/utils/power_switch.py @@ -34,6 +34,8 @@ class PowerSwitch: BOOTLOADER_CMD_ALLOFF = 0x01 BOOTLOADER_CMD_SYSOFF = 0x02 BOOTLOADER_CMD_SYSON = 0x03 + BOOTLOADER_CMD_RESET_INIT = 0xFF + BOOTLOADER_CMD_RESET = 0xF0 def __init__(self, uri): self.uri = uri @@ -72,13 +74,23 @@ def stm_power_cycle(self): time.sleep(1) self.stm_power_up() + def reboot_to_fw(self): + """Reboot the platform and start in firmware mode""" + self._send(self.BOOTLOADER_CMD_RESET_INIT) + self._send(self.BOOTLOADER_CMD_RESET, [1]) + + def reboot_to_bootloader(self): + """Reboot the platform and start the bootloader""" + self._send(self.BOOTLOADER_CMD_RESET_INIT) + self._send(self.BOOTLOADER_CMD_RESET, [0]) + def close(self): if self.link: self.link.close() - def _send(self, cmd): + def _send(self, cmd, data=[]): if not self.link: - packet = [0xf3, 0xfe, cmd] + packet = [0xf3, 0xfe, cmd] + data cr = RadioManager.open(devid=self.devid) cr.set_channel(self.channel) @@ -103,7 +115,7 @@ def _send(self, cmd): else: # send command (will be repeated until acked) - pk = CRTPPacket(0xFF, [0xfe, cmd]) + pk = CRTPPacket(0xFF, [0xfe, cmd] + data) self.link.send_packet(pk) # wait up to 1s pk = self.link.receive_packet(0.1) From 82101732b59d938b16de17fc48d635db8e9da699 Mon Sep 17 00:00:00 2001 From: Marcus Date: Sun, 10 Apr 2022 07:44:28 +0200 Subject: [PATCH 409/861] Improve disconnect --- cflib/cpx/__init__.py | 66 +++++++++++++++++++++++------------------ cflib/cpx/transports.py | 4 ++- cflib/crtp/cpxdriver.py | 15 +++++++--- 3 files changed, 51 insertions(+), 34 deletions(-) diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index 9108915a7..0b92d64b2 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -117,9 +117,10 @@ def __init__(self, transport): self._transport = transport self._rxQueues = {} self._packet_assembly = [] + self._connected = True # Register and/or blocking calls for ports - def receivePacket(self, function): + def receivePacket(self, function, timeout=None): # Check if a queue exists, if not then create it # the user might have implemented new functions @@ -127,7 +128,7 @@ def receivePacket(self, function): print("Creating queue for {}".format(function)) self._rxQueues[function.value] = queue.Queue() - return self._rxQueues[function.value].get(block=True) + return self._rxQueues[function.value].get(block=True, timeout=timeout) def makeTransaction(self, packet): self.sendPacket(packet) @@ -136,9 +137,13 @@ def makeTransaction(self, packet): def sendPacket(self, packet): # Do we queue here? self._transport.write(packet.wireData) + + def transport(self): + self._connected = False + return self._transport def run(self): - while(1): + while(self._connected): # Read one packet from the transport # Packages might have been split up along the @@ -146,29 +151,32 @@ def run(self): # lots of memory, so assemble full packets by looking at last # packet byte. Note that chunks of one packet could be mixed # with chunks from antother packet. - header = self._transport.read(4) - packet = CPXPacket(wireHeader=header) - packet.data = self._transport.read(packet.length - 2) # remove routing info here - #print(packet) - # if not packet.target in self._packet_assembly: - # self._packet_assembly[packet.target][packet.function] = [] - # else - # if not packet.function in self._packet_assembly[packet.target]: - # self._packet_assembly[packet.target][packet.function] = [] - - # self._packet_assembly[packet.target][packet.function].append(packet) - - # if (packet.lastPart): - # # Assemble packet and send up stack - # pass - if not packet.function.value in self._rxQueues: - pass - #print("Got packet for {}, but have no queue".format(packet.function)) - else: - self._rxQueues[packet.function.value].put(packet) - #self._rxQueues[packet.function.value] = queue.Queue() - #print(self._rxQueues[packet.function.value].qsize()) - + try: + header = self._transport.read(4) + packet = CPXPacket(wireHeader=header) + packet.data = self._transport.read(packet.length - 2) # remove routing info here + #print(packet) + # if not packet.target in self._packet_assembly: + # self._packet_assembly[packet.target][packet.function] = [] + # else + # if not packet.function in self._packet_assembly[packet.target]: + # self._packet_assembly[packet.target][packet.function] = [] + + # self._packet_assembly[packet.target][packet.function].append(packet) + + # if (packet.lastPart): + # # Assemble packet and send up stack + # pass + if not packet.function.value in self._rxQueues: + pass + #print("Got packet for {}, but have no queue".format(packet.function)) + else: + self._rxQueues[packet.function.value].put(packet) + #self._rxQueues[packet.function.value] = queue.Queue() + #print(self._rxQueues[packet.function.value].qsize()) + except Exception as e: + print("Exception while reading transport, link probably closed?") + print(e) # Public facing class CPX: @@ -177,14 +185,14 @@ def __init__(self, transport): self._router.start() #self.gap8.bootloader = GAP8Bootloader(self) - def receivePacket(self, function): + def receivePacket(self, function, timeout=None): # Block on a function queue #if self._router.isUsedBySubmodule(target, function): # raise ValueError("The CPX target {} and function {} is registered in a sub module".format(target, function)) # Will block on queue - return self._router.receivePacket(function) + return self._router.receivePacket(function, timeout) def makeTransaction(self, packet): return self._router.makeTransaction(packet) @@ -193,4 +201,4 @@ def sendPacket(self, packet): self._router.sendPacket(packet) def close(self): - print("Should close transport connection") \ No newline at end of file + self._router.transport().disconnect() \ No newline at end of file diff --git a/cflib/cpx/transports.py b/cflib/cpx/transports.py index 01ea8f9a3..0f1b9f0d1 100644 --- a/cflib/cpx/transports.py +++ b/cflib/cpx/transports.py @@ -30,9 +30,11 @@ def connect(self): print("Connecting to socket on {}:{}...".format(self._host, self._port)) self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._socket.connect((self._host, self._port)) + #self._socket.settimeout(0.1) print("Connected") def disconnect(self): + print("Closing transport") self._socket.close() self._socket = None @@ -41,7 +43,7 @@ def write(self, data): def read(self, size): data = bytearray() - while len(data) < size: + while len(data) < size and self._socket is not None: data.extend(self._socket.recv(size-len(data))) return data diff --git a/cflib/crtp/cpxdriver.py b/cflib/crtp/cpxdriver.py index a6ab8e9d2..3c56b3ef7 100644 --- a/cflib/crtp/cpxdriver.py +++ b/cflib/crtp/cpxdriver.py @@ -38,6 +38,9 @@ from .crtpstack import CRTPPacket from .exceptions import WrongUriType +import logging +logger = logging.getLogger(__name__) + __author__ = 'Bitcraze AB' __all__ = ['CPXDriver'] @@ -128,12 +131,14 @@ def close(self): # Close the socket try: - self._cpx.close() - self._cpx = None + self.cpx.close() + self.cpx = None except Exception as e: - logger.info('Could not close {}'.format(e)) + print(e) + logger.error('Could not close {}'.format(e)) pass + print("Driver closed") self.cpx = None def get_name(self): @@ -173,12 +178,14 @@ def run(self): try: # Block until a packet is available though the socket # CPX receive will only return full packets - cpxPacket = self._cpx.receivePacket(CPXFunction.CRTP) + cpxPacket = self._cpx.receivePacket(CPXFunction.CRTP, timeout=0.1) data = struct.unpack('B' * len(cpxPacket.data), cpxPacket.data) if len(data) > 0: pk = CRTPPacket(data[0], list(data[1:])) self.in_queue.put(pk) + except queue.Empty as e: + pass # This is ok except Exception as e: import traceback From e5c0897d848e95b4b870c005a1646179c7060dfc Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 11 Apr 2022 11:05:55 +0200 Subject: [PATCH 410/861] Styling fixes --- cflib/bootloader/__init__.py | 4 ++-- cflib/crazyflie/mem/deck_memory.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index eda5f1b74..fc6df6332 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -448,7 +448,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis timeout_time = time.time() + 5 while not deck.is_started: if time.time() > timeout_time: - raise RuntimeError(f"Deck {deck.name} did not start") + raise RuntimeError(f'Deck {deck.name} did not start') print('Deck not yet started ...') time.sleep(0.5) deck = mgr.query_decks()[deck_index] @@ -475,7 +475,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis timeout_time = time.time() + 5 while not deck.is_bootloader_active: if time.time() > timeout_time: - raise RuntimeError(f"Deck {deck.name} did not enter bootloader mode") + raise RuntimeError(f'Deck {deck.name} did not enter bootloader mode') print(f'Error: Deck {deck.name} bootloader not active yet...') time.sleep(0.5) deck = mgr.query_decks()[deck_index] diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index d77789dbf..6eb65bbd4 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -53,7 +53,7 @@ class DeckMemory: ADR_FW_NEW_FLASH = 0 ADR_COMMAND_BIT_FIELD = 4 - def __init__(self, deck_memory_manager: "DeckMemoryManager", _command_base_address): + def __init__(self, deck_memory_manager: 'DeckMemoryManager', _command_base_address): self._deck_memory_manager = deck_memory_manager self.required_hash = None self.required_length = None From ec8369590aa642990822cbf886c8062881971b1d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 11 Apr 2022 14:47:15 +0200 Subject: [PATCH 411/861] Updated names --- cflib/crazyflie/mem/deck_memory.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 6eb65bbd4..0e99569a8 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -286,13 +286,13 @@ def _parse_info_section(self, data): return result - def _write(self, base_address, address, data, read_complete_cb, read_failed_cb, progress_cb): + def _write(self, base_address, address, data, complete_cb, failed_cb, progress_cb): """Called from deck memory to write data""" if self._write_complete_cb is not None: raise Exception('Write operation ongoing') - self._write_complete_cb = read_complete_cb - self._write_failed_cb = read_failed_cb + self._write_complete_cb = complete_cb + self._write_failed_cb = failed_cb mapped_address = address + base_address self.mem_handler.write(self, mapped_address, data, flush_queue=True, progress_cb=progress_cb) From 29ae80a1fbe8149552fc9aeb439b676867b5c535 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 11 Apr 2022 14:47:35 +0200 Subject: [PATCH 412/861] Do not call callback if null --- cflib/bootloader/__init__.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index fc6df6332..cf6823656 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -171,7 +171,8 @@ def flash(self, filename: str, targets: List[Target], cf=None): current_index = 0 while current_index != -1: current_index = self._flash_deck_incrementally(deck_artifacts, deck_targets, current_index) - self.progress_cb('Deck updated! Restarting...', int(100)) + if self.progress_cb: + self.progress_cb('Deck updated! Restarting...', int(100)) if current_index != -1: PowerSwitch(self.clink).reboot_to_fw() time.sleep(3) From 4a793985a57faeed3ffc922f6eb8f74a12f6c4bc Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 20 Apr 2022 09:55:24 +0200 Subject: [PATCH 413/861] Fixed spelling error --- cflib/cpx/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index 0b92d64b2..d9ae3acd9 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -132,7 +132,7 @@ def receivePacket(self, function, timeout=None): def makeTransaction(self, packet): self.sendPacket(packet) - return self.getPacket(packet.function) + return self.receivePacket(packet.function) def sendPacket(self, packet): # Do we queue here? From ccc672601948f7433948bf8363f22fabf832761b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 2 May 2022 15:30:44 +0200 Subject: [PATCH 414/861] Add missing future import --- examples/lighthouse/multi_bs_geometry_estimation.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py index 3bdc4c630..69e8c3159 100644 --- a/examples/lighthouse/multi_bs_geometry_estimation.py +++ b/examples/lighthouse/multi_bs_geometry_estimation.py @@ -41,6 +41,8 @@ 2. 2 or more base stations ''' +from __future__ import annotations + import logging import time from threading import Event From 933a1d074bfda6303d8fd5c1cde4c75d9f370520 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 10 May 2022 13:23:52 +0200 Subject: [PATCH 415/861] reduce square flown autonomousSequence --- examples/autonomy/autonomousSequence.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index a4e03e96d..e7353a8c0 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -45,13 +45,14 @@ # Change the sequence according to your setup # x y z YAW sequence = [ - (2.5, 2.5, 1.2, 0), - (1.5, 2.5, 1.2, 0), - (2.5, 2.0, 1.2, 0), - (3.5, 2.5, 1.2, 0), - (2.5, 3.0, 1.2, 0), - (2.5, 2.5, 1.2, 0), - (2.5, 2.5, 0.4, 0), + (0.0, 0.0, 0.4, 0), + (0.0, 0.0, 1.2, 0), + (0.5, -0.5, 1.2, 0), + (0.5, 0.5, 1.2, 0), + (-0.5, 0.5, 1.2, 0), + (-0.5, -0.5, 1.2, 0), + (0.0, 0.0, 1.2, 0), + (0.0, 0.0, 0.4, 0), ] From f4ff11d7800cf2d52eebc2b7863cd389cd92d3c0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 10 May 2022 13:43:56 +0200 Subject: [PATCH 416/861] restore cfbridge for px4 example --- examples/cfbridge.py | 135 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 135 insertions(+) create mode 100755 examples/cfbridge.py diff --git a/examples/cfbridge.py b/examples/cfbridge.py new file mode 100755 index 000000000..ca02b2e56 --- /dev/null +++ b/examples/cfbridge.py @@ -0,0 +1,135 @@ +#!/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) From c40316d2841e7775b81612cd9ad889dea6901199 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 10 May 2022 16:48:11 +0200 Subject: [PATCH 417/861] more scripts with several updates and improvements --- examples/basicLedparamSync.py | 6 +++--- examples/lighthouse/lighthouse_openvr_multigrab.py | 4 ++-- examples/lighthouse/read_lighthouse_mem.py | 3 ++- examples/lighthouse/write_lighthouse_mem.py | 3 ++- examples/positioning/initial_position.py | 3 ++- examples/radio/scan.py | 5 +++-- examples/step-by-step/sbs_motion_commander.py | 4 ++-- examples/tuning/PID_controller_tuner.py | 4 ++-- 8 files changed, 18 insertions(+), 14 deletions(-) diff --git a/examples/basicLedparamSync.py b/examples/basicLedparamSync.py index a205a2812..49c018fb6 100644 --- a/examples/basicLedparamSync.py +++ b/examples/basicLedparamSync.py @@ -66,9 +66,9 @@ # 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', '0x0000A0') + cf.param.set_value('ring.fadeColor', int('0000A0',16)) time.sleep(1) - cf.param.set_value('ring.fadeColor', '0x00A000') + cf.param.set_value('ring.fadeColor', int('00A000',16)) time.sleep(1) - cf.param.set_value('ring.fadeColor', '0xA00000') + cf.param.set_value('ring.fadeColor', int('A00000',16)) time.sleep(1) diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index a1fa69a05..fcd57b911 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -15,8 +15,8 @@ from cflib.crazyflie.syncLogger import SyncLogger # URI to the Crazyflie to connect to -uri0 = 'radio://0/80/2M' -uri1 = 'radio://0/80/2M/E7E7E7E701' +uri0 = 'radio://0/80/2M/E7E7E7E701' +uri1 = 'radio://0/80/2M/E7E7E7E702' print('Opening') vr = openvr.init(openvr.VRApplication_Other) diff --git a/examples/lighthouse/read_lighthouse_mem.py b/examples/lighthouse/read_lighthouse_mem.py index 7f46d160f..42b85dcea 100644 --- a/examples/lighthouse/read_lighthouse_mem.py +++ b/examples/lighthouse/read_lighthouse_mem.py @@ -30,6 +30,7 @@ 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) @@ -67,7 +68,7 @@ def _calib_read_ready(self, calib_data): if __name__ == '__main__': # URI to the Crazyflie to connect to - uri = 'radio://0/80' + uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Initialize the low-level drivers cflib.crtp.init_drivers() diff --git a/examples/lighthouse/write_lighthouse_mem.py b/examples/lighthouse/write_lighthouse_mem.py index 5bc331e5c..c35e99651 100644 --- a/examples/lighthouse/write_lighthouse_mem.py +++ b/examples/lighthouse/write_lighthouse_mem.py @@ -32,6 +32,7 @@ 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) @@ -63,7 +64,7 @@ def _data_written(self, success): if __name__ == '__main__': # URI to the Crazyflie to connect to - uri = 'radio://0/80' + uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Initialize the low-level drivers cflib.crtp.init_drivers() diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index cbe324abe..6ab9a4f1a 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -41,9 +41,10 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Change the sequence according to your setup # x y z diff --git a/examples/radio/scan.py b/examples/radio/scan.py index 6dd200d13..e67d79bfe 100644 --- a/examples/radio/scan.py +++ b/examples/radio/scan.py @@ -22,7 +22,7 @@ # 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 and lists them. +Simple example that scans for available Crazyflies with a certain address and lists them. """ import cflib.crtp @@ -30,7 +30,8 @@ cflib.crtp.init_drivers() print('Scanning interfaces for Crazyflies...') -available = cflib.crtp.scan_interfaces() +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 9e3b420cb..376d0b0d1 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -127,7 +127,7 @@ def param_deck_flow(_, value_str): logconf.start() - # take_off_simple(scf) + take_off_simple(scf) # move_linear_simple(scf) # move_box_limit(scf) - # logconf.stop() + logconf.stop() diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 449d68639..ccbae9ea4 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -61,7 +61,7 @@ def __init__(self, master): self.master = master self.master.title('PID tuner Crazyflie') - self.figplot = plt.Figure(figsize=(5, 4), dpi=100) + self.figplot = plt.Figure(figsize=(2, 1), dpi=100) self.ax2 = self.figplot.add_subplot(111) self.line2 = FigureCanvasTkAgg(self.figplot, self.master) self.line2.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=1) @@ -191,7 +191,7 @@ def send_pid_gains(self): 'position PID controller: Kp: ' + str(self.pid_gui.scale_Kp.get()) + ', Ki: ' + str(self.pid_gui.scale_Ki.get()) + - ', Kd: '+str(self.pid_gui.scale_Ki.get())) + ', Kd: '+str(self.pid_gui.scale_Kd.get())) cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + 'Kp', self.pid_gui.scale_Kp.get()) cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + From 0ca693a54b899801ff9ec40246ada42716d74dd2 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 10 May 2022 17:10:21 +0200 Subject: [PATCH 418/861] autopep8 fixes --- examples/basicLedmemSync.py | 6 ++-- .../lighthouse/lighthouse_openvr_multigrab.py | 8 ++--- examples/radio/scan.py | 2 +- examples/tuning/PID_controller_tuner.py | 30 +++++++++---------- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/examples/basicLedmemSync.py b/examples/basicLedmemSync.py index dec847798..57defadd8 100644 --- a/examples/basicLedmemSync.py +++ b/examples/basicLedmemSync.py @@ -56,9 +56,9 @@ # 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[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) diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index fcd57b911..d4d2377f2 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -127,8 +127,8 @@ def run_sequence(scf0, scf1): 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]] + 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 @@ -149,7 +149,7 @@ def run_sequence(scf0, scf1): if not grabbed and trigger: print('Grab started') - grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] + grab_controller_start = [-1 * pose[2][3], -1 * pose[0][3], pose[1][3]] dist0 = vector_norm(vector_subtract(grab_controller_start, setpoints[0])) @@ -169,7 +169,7 @@ def run_sequence(scf0, scf1): grabbed = trigger if trigger: - curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] + 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)) diff --git a/examples/radio/scan.py b/examples/radio/scan.py index e67d79bfe..055feab3e 100644 --- a/examples/radio/scan.py +++ b/examples/radio/scan.py @@ -31,7 +31,7 @@ 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/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index ccbae9ea4..917db4eb4 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -191,12 +191,12 @@ def send_pid_gains(self): 'position PID controller: Kp: ' + str(self.pid_gui.scale_Kp.get()) + ', Ki: ' + str(self.pid_gui.scale_Ki.get()) + - ', Kd: '+str(self.pid_gui.scale_Kd.get())) - cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + + ', Kd: ' + str(self.pid_gui.scale_Kd.get())) + cf.param.set_value(self.unit_choice + 'CtlPid.' + self.axis_choice + 'Kp', self.pid_gui.scale_Kp.get()) - cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + + cf.param.set_value(self.unit_choice + 'CtlPid.' + self.axis_choice + 'Ki', self.pid_gui.scale_Ki.get()) - cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + + cf.param.set_value(self.unit_choice + 'CtlPid.' + self.axis_choice + 'Kd', self.pid_gui.scale_Kd.get()) cf.param.set_value('posCtlPid.xVelMax', self.pid_gui.scale_vMax.get()) cf.param.set_value('posCtlPid.yVelMax', self.pid_gui.scale_vMax.get()) @@ -240,11 +240,11 @@ def do_step(self): # print(sp_history) self.pid_gui.draw_plot(time_history, pos_history, sp_history) if self.axis_choice == 'z': - self.commander.go_to(0, 0, -1*STEP_SIZE, 0, 1.0, relative=True) + self.commander.go_to(0, 0, -1 * STEP_SIZE, 0, 1.0, relative=True) elif self.axis_choice == 'x': - self.commander.go_to(-1*STEP_SIZE, 0, 0, 0, 1.0, relative=True) + self.commander.go_to(-1 * STEP_SIZE, 0, 0, 0, 1.0, relative=True) elif self.axis_choice == 'y': - self.commander.go_to(0, -1*STEP_SIZE, 0, 0, 1.0, relative=True) + self.commander.go_to(0, -1 * STEP_SIZE, 0, 0, 1.0, relative=True) else: print('WRONG CHOICE?!?!') self.stop_gui() @@ -256,7 +256,7 @@ def stop_gui(self): # parameter update def change_param_axis_callback(self, value_axis): # - print(self.unit_choice + 'CtlPid.'+value_axis) + print(self.unit_choice + 'CtlPid.' + value_axis) groupname = self.unit_choice + 'CtlPid' self.cf.param.remove_update_callback( @@ -277,9 +277,9 @@ def change_param_axis_callback(self, value_axis): group=groupname, name=value_axis + 'Kd', cb=self.param_updated_callback_Kd) - self.cf.param.request_param_update(groupname+'.'+value_axis+'Kp') - self.cf.param.request_param_update(groupname+'.'+value_axis+'Ki') - self.cf.param.request_param_update(groupname+'.'+value_axis+'Kd') + self.cf.param.request_param_update(groupname + '.' + value_axis + 'Kp') + self.cf.param.request_param_update(groupname + '.' + value_axis + 'Ki') + self.cf.param.request_param_update(groupname + '.' + value_axis + 'Kd') time.sleep(0.1) self.update_scale_info() @@ -311,13 +311,13 @@ def change_param_unit_callback(self, value_unit): group=groupname_new, name=self.axis_choice + 'Kd', cb=self.param_updated_callback_Kd) - print(groupname_new+'.'+self.axis_choice+'Kp') + print(groupname_new + '.' + self.axis_choice + 'Kp') self.cf.param.request_param_update( - groupname_new+'.'+self.axis_choice+'Kp') + groupname_new + '.' + self.axis_choice + 'Kp') self.cf.param.request_param_update( - groupname_new+'.'+self.axis_choice+'Ki') + groupname_new + '.' + self.axis_choice + 'Ki') self.cf.param.request_param_update( - groupname_new+'.'+self.axis_choice+'Kd') + groupname_new + '.' + self.axis_choice + 'Kd') time.sleep(0.1) self.update_scale_info() From 997ef866ee1625433e35a34d1d4aef8b25c4b89b Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 10 May 2022 17:28:35 +0200 Subject: [PATCH 419/861] added callback check to basic param script --- examples/basicLedparamSync.py | 6 +++--- examples/parameters/basicparam.py | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/basicLedparamSync.py b/examples/basicLedparamSync.py index 49c018fb6..bb4326372 100644 --- a/examples/basicLedparamSync.py +++ b/examples/basicLedparamSync.py @@ -66,9 +66,9 @@ # 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)) + cf.param.set_value('ring.fadeColor', int('0000A0', 16)) time.sleep(1) - cf.param.set_value('ring.fadeColor', int('00A000',16)) + cf.param.set_value('ring.fadeColor', int('00A000', 16)) time.sleep(1) - cf.param.set_value('ring.fadeColor', int('A00000',16)) + cf.param.set_value('ring.fadeColor', int('A00000', 16)) time.sleep(1) diff --git a/examples/parameters/basicparam.py b/examples/parameters/basicparam.py index 6dc18b713..7f874ddfc 100644 --- a/examples/parameters/basicparam.py +++ b/examples/parameters/basicparam.py @@ -125,6 +125,8 @@ def _param_callback(self, name, value): self._cf.param.set_value('pid_attitude.pitch_kd', '{:.2f}'.format(pkd)) + self._cf.param.request_param_update('pid_attitude.pitch_kd') + def _a_pitch_kd_callback(self, name, value): """Callback for pid_attitude.pitch_kd""" print('Readback: {0}={1}'.format(name, value)) From 63d9226b085bb6ea38e6b045375406e16a403a3d Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 11 May 2022 10:05:05 +0200 Subject: [PATCH 420/861] Create test-python-publish.yml --- .github/workflows/test-python-publish.yml | 39 +++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 .github/workflows/test-python-publish.yml diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml new file mode 100644 index 000000000..707fef98c --- /dev/null +++ b/.github/workflows/test-python-publish.yml @@ -0,0 +1,39 @@ +# This workflow will upload a Python Package using Twine when a release is created +# For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries + +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. + +name: Test Release + +on: + workflow_dispatch: + +permissions: + contents: read + +jobs: + deploy: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: '3.x' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install build + - name: Build package + run: python -m build + - name: Publish package to TestPyPI + uses: pypa/gh-action-pypi-publish@release/v1 + with: + user: ${{ secrets.PYPI_USERNAME }} + password: ${{ secrets.PYPI_PASSWORD }} + repository_url: https://test.pypi.org/legacy/ From 75c18bb435227df2e51a9693c23d24c69d3cb208 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 11 May 2022 10:14:31 +0200 Subject: [PATCH 421/861] update version to 0.1.19 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 5522f86de..49d52c000 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.18.1', + version='0.1.19', packages=find_packages(exclude=['examples', 'tests']), description='Crazyflie python driver', From 6ca9631978d2971c2a89cfd7e972f4b6b34f18a9 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 11 May 2022 10:23:55 +0200 Subject: [PATCH 422/861] Create python-publish.yml --- .github/workflows/python-publish.yml | 38 ++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 .github/workflows/python-publish.yml diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml new file mode 100644 index 000000000..bf2b59ede --- /dev/null +++ b/.github/workflows/python-publish.yml @@ -0,0 +1,38 @@ +# This workflow will upload a Python Package using Twine when a release is created +# For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries + +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. + +name: Release + +on: + workflow_dispatch: + +permissions: + contents: read + +jobs: + deploy: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: '3.x' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install build + - name: Build package + run: python -m build + - name: Publish package + uses: pypa/gh-action-pypi-publish@release/v1.5.0 + with: + user: ${{ secrets.PYPI_USERNAME }} + password: ${{ secrets.PYPI_PASSWORD }} From 6323bee02efc90ba05438e9816de670542d41ec0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 11 May 2022 10:29:38 +0200 Subject: [PATCH 423/861] Update test-python-publish.yml --- .github/workflows/test-python-publish.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index 707fef98c..5f3a6d555 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -32,7 +32,7 @@ jobs: - name: Build package run: python -m build - name: Publish package to TestPyPI - uses: pypa/gh-action-pypi-publish@release/v1 + uses: pypa/gh-action-pypi-publish@release/v1.5.0 with: user: ${{ secrets.PYPI_USERNAME }} password: ${{ secrets.PYPI_PASSWORD }} From dbde44053345d9a31e6874910ddbd71944479140 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 11 May 2022 10:32:14 +0200 Subject: [PATCH 424/861] fix publish actions --- .github/workflows/python-publish.yml | 2 +- .github/workflows/test-python-publish.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml index bf2b59ede..c88f03fd9 100644 --- a/.github/workflows/python-publish.yml +++ b/.github/workflows/python-publish.yml @@ -32,7 +32,7 @@ jobs: - name: Build package run: python -m build - name: Publish package - uses: pypa/gh-action-pypi-publish@release/v1.5.0 + uses: pypa/gh-action-pypi-publish@release/v1 with: user: ${{ secrets.PYPI_USERNAME }} password: ${{ secrets.PYPI_PASSWORD }} diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index 5f3a6d555..707fef98c 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -32,7 +32,7 @@ jobs: - name: Build package run: python -m build - name: Publish package to TestPyPI - uses: pypa/gh-action-pypi-publish@release/v1.5.0 + uses: pypa/gh-action-pypi-publish@release/v1 with: user: ${{ secrets.PYPI_USERNAME }} password: ${{ secrets.PYPI_PASSWORD }} From b9a26120286ac1e250d2d755aaf169745ca399ed Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 11 May 2022 10:34:55 +0200 Subject: [PATCH 425/861] remove update param example --- examples/parameters/basicparam.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/parameters/basicparam.py b/examples/parameters/basicparam.py index 7f874ddfc..6dc18b713 100644 --- a/examples/parameters/basicparam.py +++ b/examples/parameters/basicparam.py @@ -125,8 +125,6 @@ def _param_callback(self, name, value): self._cf.param.set_value('pid_attitude.pitch_kd', '{:.2f}'.format(pkd)) - self._cf.param.request_param_update('pid_attitude.pitch_kd') - def _a_pitch_kd_callback(self, name, value): """Callback for pid_attitude.pitch_kd""" print('Readback: {0}={1}'.format(name, value)) From d40a37db541e4e91d5237d7030d2b99a470fc4be Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 11 May 2022 16:00:39 +0200 Subject: [PATCH 426/861] fix to cflib doc --- docs/development/matlab.md | 9 ++--- docs/installation/install.md | 68 ++++++++++-------------------------- 2 files changed, 24 insertions(+), 53 deletions(-) diff --git a/docs/development/matlab.md b/docs/development/matlab.md index bc8216c78..df5ea1732 100644 --- a/docs/development/matlab.md +++ b/docs/development/matlab.md @@ -4,14 +4,15 @@ page_id: matlab --- -Using Matlab with the Crazyflie API is easy -- you just need to install -the python 'matlab engine' and then can access all matlab commands +To use the Python cflib with matlab, you will need to install the python 'matlab engine' and then can access all matlab commands directly from python. -Prerequisites +*Note that these are old instructions and they might not work anymore** + +Tried with ------------- -1. MATLAB 2014b or later +1. MATLAB 2014b 2. 64 bit python 2.7, 3.3 or 3.4 3. The Crazyflie API diff --git a/docs/installation/install.md b/docs/installation/install.md index 22c7fbfd4..a1a1f64be 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -7,14 +7,22 @@ page_id: install This project requires Python 3.7+. See below sections for more platform-specific requirements. -## Development -### Developing for the cflib -* [Fork the cflib](https://help.github.com/articles/fork-a-repo/) -* [Clone the cflib](https://help.github.com/articles/cloning-a-repository/), `git clone git@github.com:YOUR-USERNAME/crazyflie-lib-python.git` -* [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` - - -* [Uninstall the cflib if you don't want it any more](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib` +## Install from Source +### Clone the repository + ``` + git clone https://github.com/bitcraze/crazyflie-lib-python.git + ``` +### Install cflib from source + ``` + cd crazyflie-lib-pyhon + pip install -e . + ``` + +### Uninstall cflib + + ``` +pip uninstall cflib + ``` Note: If you are developing for the cflib you must use python3. On Ubuntu (20.04+) use `pip3` instead of `pip`. @@ -34,9 +42,6 @@ you can skip this section. * To deactivate the virtualenv when you are done using it `deactivate` -Note: For systems that support [make](https://www.gnu.org/software/make/manual/html_node/Simple-Makefile.html), you can use `make venv` to -create an environment, activate it and install dependencies. - #### Install cflib dependencies Install dependencies required by the lib: `pip install -r requirements.txt`. If you are planning on developing on the lib you should also run: `pip install -r requirements-dev.txt`. @@ -61,47 +66,12 @@ For information and installation of the Note: Docker and the toolbelt is an optional way of running tests and reduces the work needed to maintain your python environment. -### Native python on Linux, OSX, Windows - [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox` -* If test fails after installing tox with `pip install tox`, installing with `sudo apt-get install tox`result a successful test run - -* Test package in python3.4 `TOXENV=py34 tox` -* Test package in python3.6 `TOXENV=py36 tox` - -Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python 3.3, 3.4 and would require all python versions to be installed on the machine.) - - ## Platform notes ### Linux -#### Setting udev permissions - -The following steps make it possible to use the USB Radio without being root. +With linux, the crazyradio is easily recognized, but you have to setup UDEVpermissions. Look at the [usb permission instructions](/docs/installation/usb_permissions.md) to setup udev on linux. -``` -sudo groupadd plugdev -sudo usermod -a -G plugdev $USER -``` +### Windows -You will need to log out and log in again in order to be a member of the plugdev group. - -Create a file named ```/etc/udev/rules.d/99-crazyradio.rules``` and add the -following: -``` -# Crazyradio (normal operation) -SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="7777", MODE="0664", GROUP="plugdev" -# Bootloader -SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="0101", MODE="0664", GROUP="plugdev" -``` - -To connect Crazyflie 2.0 via usb, create a file name ```/etc/udev/rules.d/99-crazyflie.rules``` and add the following: -``` -SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE="0664", GROUP="plugdev" -``` - -You can reload the udev-rules using the following: -``` -sudo udevadm control --reload-rules -sudo udevadm trigger -``` +Look at the [Zadig crazyradio instructions](https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/usbwindows/) to install crazyradio on Windows From e35b73c506243d6fe0bc3a92778f7e39a67030dd Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 16 May 2022 21:43:02 +0200 Subject: [PATCH 427/861] typo, fixes #337 --- docs/installation/install.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index a1a1f64be..2303ec6c2 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -14,7 +14,7 @@ See below sections for more platform-specific requirements. ``` ### Install cflib from source ``` - cd crazyflie-lib-pyhon + cd crazyflie-lib-python pip install -e . ``` From 8465d7ba1ed1b6da56a735e63de824fd4ba307b1 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Tue, 24 May 2022 13:41:38 +0200 Subject: [PATCH 428/861] Altered cfusb driver so it works with a composite usb device --- cflib/drivers/cfusb.py | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index 5667974bb..a5a02d758 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -89,16 +89,22 @@ def __init__(self, device=None, devid=0): self.dev = devices[devid] except Exception: self.dev = None - - if self.dev: - if platform.system() == 'Linux': - self.dev.reset() - - self.dev.set_configuration(1) - self.handle = self.dev - self.version = float( - '{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, - self.dev.bcdDevice & 0x0FF)) + + try: # configuration might already be confgiured by composite VCP, try claim interface + usb.util.claim_interface(self.dev, 0) + except Exception: + try: + self.dev.set_configuration() #it was not, then set configuration + except Exception: + if self.dev: + if platform.system() == 'Linux': + self.dev.reset() + self.dev.set_configuration() + + self.handle = self.dev + self.version = float( + '{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, + self.dev.bcdDevice & 0x0FF)) def get_serial(self): # The signature for get_string has changed between versions to 1.0.0b1, From 1a3ad383d42140605bfd8bb3069b4fef9a4aba27 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Wed, 25 May 2022 10:12:03 +0200 Subject: [PATCH 429/861] Fixed cfusb pep8 errors --- cflib/drivers/cfusb.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index a5a02d758..124e27061 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -89,18 +89,18 @@ def __init__(self, device=None, devid=0): self.dev = devices[devid] except Exception: self.dev = None - - try: # configuration might already be confgiured by composite VCP, try claim interface + + try: # configuration might already be confgiured by composite VCP, try claim interface usb.util.claim_interface(self.dev, 0) except Exception: - try: - self.dev.set_configuration() #it was not, then set configuration + try: + self.dev.set_configuration() # it was not, then set configuration except Exception: if self.dev: if platform.system() == 'Linux': self.dev.reset() self.dev.set_configuration() - + self.handle = self.dev self.version = float( '{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, From 92763d1a2d123266b4d67741a6be86508b8ee02f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 31 May 2022 10:23:46 +0200 Subject: [PATCH 430/861] Cast param to float when needed --- cflib/crazyflie/param.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 7a5bad532..f2de4cd6a 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -338,10 +338,10 @@ def set_value(self, complete_name, value): else: pk.data = struct.pack(' Date: Thu, 9 Jun 2022 16:03:39 +0200 Subject: [PATCH 431/861] Updated uart_communication with kbuild change. --- docs/development/uart_communication.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/uart_communication.md b/docs/development/uart_communication.md index f02bc945b..51a545928 100644 --- a/docs/development/uart_communication.md +++ b/docs/development/uart_communication.md @@ -16,7 +16,7 @@ If you are connecting to a Raspberry Pi look for the UART pins there connect the ## Crazyflie Firmware -Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the firmware has to be compiled with the `UART2_LINK=1` flag (e.g. `make UART2_LINK=1`) and flashed to the Crazyflie. +Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the firmware has to be compiled with the `communication->CRTP_OVER_UART2` kbuild config enabled. ## Controlling Device From 6eb9559f407a034fb521cb685eb144c2a9433a66 Mon Sep 17 00:00:00 2001 From: Marcus Date: Thu, 23 Jun 2022 11:19:43 +0200 Subject: [PATCH 432/861] Updatred serial driver to use CPX --- cflib/cpx/__init__.py | 44 +++--- cflib/cpx/transports.py | 84 ++++++++++ cflib/crtp/serialdriver.py | 211 ++++++++----------------- docs/development/uart_communication.md | 2 +- 4 files changed, 179 insertions(+), 162 deletions(-) diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index d9ae3acd9..2bed6d452 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -63,7 +63,7 @@ class CPXPacket(object): A packet with routing and data """ - def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytearray(), wireHeader=None): + def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytearray()): """ Create an empty packet with default values. """ @@ -71,22 +71,14 @@ def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytear self.source = source self.destination = destination self.function = function - self._wireHeaderFormat = "> 3) & 0x07) - self.destination = CPXTarget(targetsAndFlags & 0x07) - self.lastPacket = targetsAndFlags & 0x40 != 0 - self.function = CPXFunction(self.function) def _get_wire_data(self): """Create raw data to send via the wire""" raw = bytearray() # This is the length excluding the 2 byte legnth - wireLength = len(self.data) + 2 # 2 bytes for CPX header + #wireLength = len(self.data) + 2 # 2 bytes for CPX header targetsAndFlags = ((self.source.value & 0x7) << 3) | (self.destination.value & 0x7) if self.lastPacket: targetsAndFlags |= 0x40 @@ -94,20 +86,29 @@ def _get_wire_data(self): #print(self.source) #print(targets) function = self.function.value & 0xFF - raw.extend(struct.pack(self._wireHeaderFormat, wireLength, targetsAndFlags, function)) + raw.extend(struct.pack(" 1022): - raise "Cannot send this packet, the size is too large!" + #if (wireLength > 1022): + # raise "Cannot send this packet, the size is too large!" return raw + def _set_wire_data(self, data): + [targetsAndFlags, self.function] = struct.unpack("> 3) & 0x07) + self.destination = CPXTarget(targetsAndFlags & 0x07) + self.lastPacket = targetsAndFlags & 0x40 != 0 + self.function = CPXFunction(self.function) + self.data = data[2:] + self.length = len(self.data) + def __str__(self): """Get a string representation of the packet""" return "{}->{}/{} (size={} bytes)".format(self.source, self.destination, self.function, self.length) - wireData = property(_get_wire_data, None) + wireData = property(_get_wire_data, _set_wire_data) # Internal here, route to modules and from public facing API class CPXRouter(threading.Thread): @@ -127,7 +128,7 @@ def receivePacket(self, function, timeout=None): if not function.value in self._rxQueues: print("Creating queue for {}".format(function)) self._rxQueues[function.value] = queue.Queue() - + return self._rxQueues[function.value].get(block=True, timeout=timeout) def makeTransaction(self, packet): @@ -136,7 +137,7 @@ def makeTransaction(self, packet): def sendPacket(self, packet): # Do we queue here? - self._transport.write(packet.wireData) + self._transport.writePacket(packet) def transport(self): self._connected = False @@ -152,9 +153,10 @@ def run(self): # packet byte. Note that chunks of one packet could be mixed # with chunks from antother packet. try: - header = self._transport.read(4) - packet = CPXPacket(wireHeader=header) - packet.data = self._transport.read(packet.length - 2) # remove routing info here + packet = self._transport.readPacket() + #header = self._transport.read(4) + #packet = CPXPacket(wireHeader=header) + #packet.data = self._transport.read(packet.length - 2) # remove routing info here #print(packet) # if not packet.target in self._packet_assembly: # self._packet_assembly[packet.target][packet.function] = [] @@ -177,6 +179,8 @@ def run(self): except Exception as e: print("Exception while reading transport, link probably closed?") print(e) + import traceback + print(traceback.format_exc()) # Public facing class CPX: diff --git a/cflib/cpx/transports.py b/cflib/cpx/transports.py index 0f1b9f0d1..405bcd86a 100644 --- a/cflib/cpx/transports.py +++ b/cflib/cpx/transports.py @@ -1,5 +1,15 @@ import socket +from threading import Lock + +from . import CPXPacket + +found_serial = True +try: + import serial + import serial.tools.list_ports as list_ports +except ImportError: + found_serial = False class CPXTransport: def __init__(self): @@ -47,6 +57,80 @@ def read(self, size): data.extend(self._socket.recv(size-len(data))) return data +class UARTTransport(CPXTransport): + def __init__(self, device, baudrate): + print("CPX UART transport") + self._device = device + self._baudrate = baudrate + self._serial = None + self._cts = False + self._lock = Lock() + + self.connect() + + def connect(self): + print("Connecting to UART on {} @ {}...".format(self._device, self._baudrate)) + self._serial = serial.Serial(self._device, self._baudrate, timeout=None) + + isInSync = False + + while not isInSync: + start = self._serial.read(1)[0] + print(start) + if start == 0xFF: + print("Got start") + size = self._serial.read(1)[0] + print(size) + if size == 0x00: + isInSync = True + self.cts = True + + # Send back sync + self._serial.write([0xFF, 0x00]) + + print("Connected") + + def _calcXORchecksum(self, data): + checksum = 0 + for i in data: + checksum ^= i + return checksum + + def disconnect(self): + print("Closing transport") + self._serial.close() + self._serial = None + + def writePacket(self, packet): + self._lock.acquire() + data = packet.wireData + if len(data) > 100: + raise "Packet too large!" + + buff = bytearray([0xFF, len(data)]) + buff.extend(data) + buff.extend([self._calcXORchecksum(buff)]) + self._serial.write(buff) + + def readPacket(self): + size = 0 + while size == 0: + start = self._serial.read(1)[0] + if start == 0xFF: + size = self._serial.read(1)[0] + if size == 0: + self._lock.release() + else: + data = self._serial.read(size) # Size is excluding start (0xFF) and checksum at end + crc = self._serial.read(1) + # Send CTS + self._serial.write([0xFF, 0x00]) + + packet = CPXPacket() + packet.wireData = data + + return packet + class CRTPTransport(CPXTransport): def __init__(self): print("CPX CRTP transport") diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index 20b4b86fc..bb0e9dffd 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -30,11 +30,15 @@ import queue import re import threading +import struct from .crtpstack import CRTPPacket from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver +from cflib.cpx import CPX, CPXPacket, CPXTarget, CPXFunction +from cflib.cpx.transports import UARTTransport + found_serial = True try: import serial @@ -47,20 +51,6 @@ logger = logging.getLogger(__name__) -MTU = 32 -START_BYTE1 = 0xbc -START_BYTE2 = 0xcf -SYSLINK_RADIO_RAW = 0x00 - - -def compute_cksum(list): - cksum0, cksum1 = 0, 0 - for i in range(len(list)): - cksum0 = (cksum0 + list[i]) & 0xff - cksum1 = (cksum1 + cksum0) & 0xff - return bytearray([cksum0, cksum1]) - - class SerialDriver(CRTPDriver): def __init__(self): @@ -72,6 +62,7 @@ def __init__(self): self.out_queue = None self._receive_thread = None self._send_thread = None + self.needs_resending = False logger.info('Initialized serial driver.') def connect(self, uri, linkQualityCallback, linkErrorCallback): @@ -84,9 +75,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): if not uri_data: raise Exception('Invalid serial URI') - if not found_serial: - raise Exception('PySerial package is missing') - + # Move to Serial transport? device_name = uri_data.group(1) devices = self.get_devices() if device_name not in devices: @@ -99,25 +88,29 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): # Prepare the inter-thread communication queue self.in_queue = queue.Queue() - self.out_queue = queue.Queue(1) - self.ser = serial.Serial(device, 512000, timeout=1) + self.cpx = CPX(UARTTransport(device, 576000)) + + self._thread = _CPXReceiveThread(self.cpx, self.in_queue, + linkErrorCallback) + self._thread.start() - # Launch the comm thread - self._receive_thread = _SerialReceiveThread( - self.ser, self.in_queue, linkQualityCallback, linkErrorCallback) - self._receive_thread.start() - self._send_thread = _SerialSendThread( - self.ser, self.out_queue, linkQualityCallback, linkErrorCallback) - self._send_thread.start() + # Switch the link bridge to CPX in the Crazyflie + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.SYSTEM, + data=[0x10, 0x01])) + # Force client connect to true + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.SYSTEM, + data=[0x20, 0x01])) + + # TODO! These should be reset again! def send_packet(self, pk): - try: - self.out_queue.put(pk, True, 2) - except queue.Full: - if self.link_error_callback: - self.link_error_callback( - 'RadioDriver: Could not send packet to copter') + raw = (pk.header,) + struct.unpack('B' * len(pk.data), pk.data) + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.CRTP, + data=raw)) def receive_packet(self, wait=0): try: @@ -138,21 +131,30 @@ def get_name(self): return 'serial' def scan_interface(self, address): + print("Scanning serial") if found_serial: + print("Found serial") devices_names = self.get_devices().keys() + print(devices_names) return [('serial://' + x, '') for x in devices_names] else: return [] def close(self): - self._receive_thread.stop() - self._send_thread.stop() + """ Close the link. """ + # Stop the comm thread + self._thread.stop() + + # Close the socket try: - self._receive_thread.join() - self._send_thread.join() - except Exception: + self.cpx.close() + self.cpx = None + + except Exception as e: + print(e) + logger.error('Could not close {}'.format(e)) pass - self.ser.close() + print("Driver closed") def get_devices(self): result = {} @@ -166,121 +168,48 @@ def get_devices(self): return result +class _CPXReceiveThread(threading.Thread): + """ + Radio link receiver thread used to read data from the + Socket. """ -class _SerialReceiveThread(threading.Thread): - - def __init__(self, ser, inQueue, link_quality_callback, - link_error_callback): + def __init__(self, cpx, inQueue, link_error_callback): """ Create the object """ threading.Thread.__init__(self) - self.ser = ser + self._cpx = cpx self.in_queue = inQueue - self._stop = False + self.sp = False self.link_error_callback = link_error_callback def stop(self): """ Stop the thread """ - self._stop = True + self.sp = True + try: + self.join() + except Exception: + pass def run(self): """ Run the receiver thread """ - READ_END = bytes([START_BYTE1, START_BYTE2]) - received = bytearray(MTU + 4) - received_header = memoryview(received)[0:2] - while not self._stop: - try: - r = self.ser.read_until(READ_END)[-2:] - if len(r) != 2: - continue - - if r[0] != START_BYTE1 or r[1] != START_BYTE2: - continue - - r = self.ser.readinto(received_header) - if r != 2: - continue - - if not (0 < received_header[1] <= MTU): - continue - - expected = received_header[1] + 2 - - received_data_chk = memoryview(received)[2:2+expected] - r = self.ser.readinto(received_data_chk) - if r != expected: - continue - - # NOTE: end is (expected - 2) as the length of the data +2 for - # the header bytes - cksum = compute_cksum(memoryview(received)[:expected]) - if cksum[0] != received_data_chk[-2] or \ - cksum[1] != received_data_chk[-1]: - continue - - pk = CRTPPacket(received[2], received[3:expected]) - self.in_queue.put(pk) + while (True): + if (self.sp): + break + try: + # Block until a packet is available though the socket + # CPX receive will only return full packets + cpxPacket = self._cpx.receivePacket(CPXFunction.CRTP, timeout=1) + data = struct.unpack('B' * cpxPacket.length, cpxPacket.data) + if len(data) > 0: + pk = CRTPPacket(data[0], + list(data[1:])) + self.in_queue.put(pk) + except queue.Empty as e: + pass # This is ok except Exception as e: import traceback - if self.link_error_callback: - self.link_error_callback( - 'Error communicating with the Crazyflie!\n' - 'Exception:%s\n\n%s' % (e, traceback.format_exc())) - - -class _SerialSendThread(threading.Thread): - - def __init__(self, ser, outQueue, link_quality_callback, - link_error_callback): - """ Create the object """ - threading.Thread.__init__(self) - self.ser = ser - self.out_queue = outQueue - self._stop = False - self.link_error_callback = link_error_callback - def stop(self): - """ Stop the thread """ - self._stop = True - - def run(self): - """ Run the sender thread """ - out_data = bytearray(MTU + 6) - out_data[0:3] = bytearray( - [START_BYTE1, START_BYTE2, SYSLINK_RADIO_RAW]) - - empty_packet = CRTPPacket(header=0xFF) - empty_packet_data_length = 0 - empty_packet_data = bytearray(7) - empty_packet_data[0:5] = bytearray( - [START_BYTE1, START_BYTE2, SYSLINK_RADIO_RAW, 0x01, - empty_packet.header]) - empty_packet_data[5:7] = compute_cksum(empty_packet_data[2:5]) - - while not self._stop: - try: - pk = self.out_queue.get(True, timeout=0.0003) - data = pk.data - len_data = len(data) - end_of_payload = 5 + len_data - - out_data[3] = len_data + 1 - out_data[4] = pk.header - out_data[5:end_of_payload] = data - out_data[end_of_payload:end_of_payload + - 2] = compute_cksum(out_data[2:end_of_payload]) - - written = self.ser.write(out_data[0:end_of_payload + 2]) - - except queue.Empty: - pk = empty_packet - len_data = empty_packet_data_length - written = self.ser.write(empty_packet_data) - - if written != len_data + 7: - if self.link_error_callback: - self.link_error_callback( - 'SerialDriver: Could only send {:d}B bytes of {:d}B ' - 'packet to Crazyflie'.format( - written, len_data + 7) - ) + self.link_error_callback( + 'Error communicating with the Crazyflie\n' + 'Exception:%s\n\n%s' % (e, + traceback.format_exc())) diff --git a/docs/development/uart_communication.md b/docs/development/uart_communication.md index 51a545928..d80772aa2 100644 --- a/docs/development/uart_communication.md +++ b/docs/development/uart_communication.md @@ -16,7 +16,7 @@ If you are connecting to a Raspberry Pi look for the UART pins there connect the ## Crazyflie Firmware -Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the firmware has to be compiled with the `communication->CRTP_OVER_UART2` kbuild config enabled. +Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the firmware has to be compiled with the `Expansion deck configuration->Support CRTP over UART2` and `Expansion deck configuration->crtpOverUART2` kbuild config enabled. ## Controlling Device From 684bd1fa539d48ab256cec66e7cd1e358e6d31ab Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 27 Jun 2022 14:35:02 +0200 Subject: [PATCH 433/861] Added start address to trajectory mem write --- cflib/crazyflie/mem/trajectory_memory.py | 23 ++++++++++++++----- .../autonomous_sequence_high_level.py | 12 +++++----- ...tonomous_sequence_high_level_compressed.py | 11 ++++----- examples/qualisys/qualisys_hl_commander.py | 12 +++++++++- 4 files changed, 39 insertions(+), 19 deletions(-) diff --git a/cflib/crazyflie/mem/trajectory_memory.py b/cflib/crazyflie/mem/trajectory_memory.py index d6f6b1219..d9460b28f 100644 --- a/cflib/crazyflie/mem/trajectory_memory.py +++ b/cflib/crazyflie/mem/trajectory_memory.py @@ -158,13 +158,12 @@ class TrajectoryMemory(MemoryElement): def __init__(self, id, type, size, mem_handler): """Initialize trajectory memory""" - super(TrajectoryMemory, self).__init__(id=id, type=type, size=size, - mem_handler=mem_handler) + super(TrajectoryMemory, self).__init__(id=id, type=type, size=size, mem_handler=mem_handler) self._write_finished_cb = None self._write_failed_cb = None # A list of trajectory elements to write to the Crazyflie. The elements can either be - # Poly4D instances for uncomressed trajectorys or one CompressedStart instance followed + # Poly4D instances for uncompressed trajectories or one CompressedStart instance followed # by CompressedSegment instances. It is not possible to mix uncompressed and compressed # elements in the same trajectory. self.trajectory = [] @@ -179,8 +178,19 @@ def poly4Ds(self): def poly4Ds(self, trajectory): self.trajectory = trajectory - def write_data(self, write_finished_cb, write_failed_cb=None): - """Write trajectory data to the Crazyflie""" + def write_data(self, write_finished_cb, write_failed_cb=None, start_addr=0x00): + """ + Write trajectory data to the Crazyflie. + The trajectory in self.trajectory is written to the Crazyflie. + By default the trajectory is written to address 0 of the Crazyflie trajectory memory, but it is possible to + use a different address. This can be interesting if you want to define more than one trajectory but requires + careful handling of the addresses to avoid overwriting trajectories and staying within the trajectory memory. + + @param write_finished_cb A callback that is called when the write trajectory is uploaded. + @param write_failed_cb Callback that is called if the upload failed + @param start_addr The address in the trajectory memory to upload the trajectory to (0 by default) + @return The number of bytes used for the trajectory + """ self._write_finished_cb = write_finished_cb self._write_failed_cb = write_failed_cb data = bytearray() @@ -188,7 +198,8 @@ def write_data(self, write_finished_cb, write_failed_cb=None): for element in self.trajectory: data += element.pack() - self.mem_handler.write(self, 0x00, data, flush_queue=True) + self.mem_handler.write(self, start_addr, data, flush_queue=True) + return len(data) def write_done(self, mem, addr): if self._write_finished_cb and mem.id == self.id: diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 5c05f6a49..90e650b71 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -65,27 +65,26 @@ class Uploader: def __init__(self): self._is_done = False - self._sucess = True + self._success = True def upload(self, trajectory_mem): print('Uploading data') - trajectory_mem.write_data(self._upload_done, - write_failed_cb=self._upload_failed) + trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed) while not self._is_done: time.sleep(0.2) - return self._sucess + return self._success def _upload_done(self, mem, addr): print('Data uploaded') self._is_done = True - self._sucess = True + self._success = True def _upload_failed(self, mem, addr): print('Data upload failed') self._is_done = True - self._sucess = False + self._success = False def wait_for_position_estimator(scf): @@ -147,6 +146,7 @@ def activate_mellinger_controller(cf): 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: diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index f45b95b4d..d4e9ece0c 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -61,27 +61,26 @@ class Uploader: def __init__(self): self._is_done = False - self._sucess = True + self._success = True def upload(self, trajectory_mem): print('Uploading data') - trajectory_mem.write_data(self._upload_done, - write_failed_cb=self._upload_failed) + trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed) while not self._is_done: time.sleep(0.2) - return self._sucess + return self._success def _upload_done(self, mem, addr): print('Data uploaded') self._is_done = True - self._sucess = True + self._success = True def _upload_failed(self, mem, addr): print('Data upload failed') self._is_done = True - self._sucess = False + self._success = False def wait_for_position_estimator(scf): diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index bafef9ad2..3ec7d9ff1 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -152,17 +152,26 @@ async def _close(self): class Uploader: def __init__(self): self._is_done = False + self._success = True def upload(self, trajectory_mem): print('Uploading data') - trajectory_mem.write_data(self._upload_done) + trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed) while not self._is_done: time.sleep(0.2) + return self._success + def _upload_done(self, mem, addr): print('Data uploaded') self._is_done = True + self._success = True + + def _upload_failed(self, mem, addr): + print('Data upload failed') + self._is_done = True + self._success = False def wait_for_position_estimator(scf): @@ -263,6 +272,7 @@ def activate_mellinger_controller(cf): 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: From 17057944d4080f75b026fa96f0b6cff158cd3cb8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 27 Jun 2022 16:00:49 +0200 Subject: [PATCH 434/861] Add callback when fully connected. This is really only exposing param.all_updated.add_callback through the Crazyflie class --- cflib/crazyflie/__init__.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 2e09afdf2..918ae0b0d 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -88,6 +88,9 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): # Called when the link is established and the TOCs (that are not # cached) have been downloaded self.connected = Caller() + # Called when the the link is established and all data, including parameters have been downloaded + self.fully_connected = Caller() + # Called if establishing of the link fails (i.e times out) self.connection_failed = Caller() # Called for every packet received @@ -131,6 +134,8 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.connected_ts = None + self.param.all_updated.add_callback(self._all_parameters_updated) + # Connect callbacks to logger self.disconnected.add_callback( lambda uri: logger.info('Callback->Disconnected from [%s]', uri)) @@ -147,8 +152,9 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): lambda uri: logger.info( 'Callback->Connection initialized[%s]', uri)) self.connected.add_callback( - lambda uri: logger.info( - 'Callback->Connection setup finished [%s]', uri)) + lambda uri: logger.info('Callback->Connection setup finished [%s]', uri)) + self.fully_connected.add_callback( + lambda uri: logger.info('Callback->Connection completed [%s]', uri)) def _disconnected(self, link_uri): """ Callback when disconnected.""" @@ -181,6 +187,11 @@ def _log_toc_updated_cb(self): logger.info('Log TOC finished updating') self.mem.refresh(self._mems_updated_cb) + def _all_parameters_updated(self): + """Called when all parameters have been updated""" + logger.info('All parameters updated') + self.fully_connected.call(self.link_uri) + def _link_error_cb(self, errmsg): """Called from the link driver when there's an error""" logger.warning('Got link error callback [%s] in state [%s]', From 88c06af5777265fa523b89bbf87596d3a387604c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 27 Jun 2022 16:07:07 +0200 Subject: [PATCH 435/861] refactoring --- cflib/crazyflie/__init__.py | 12 ++++-------- cflib/crazyflie/param.py | 3 +-- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 918ae0b0d..7bf6790b1 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -85,8 +85,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.link_established = Caller() # Called when the user requests a connection self.connection_requested = Caller() - # Called when the link is established and the TOCs (that are not - # cached) have been downloaded + # Called when the link is established and the TOCs (that are not cached) have been downloaded self.connected = Caller() # Called when the the link is established and all data, including parameters have been downloaded self.fully_connected = Caller() @@ -143,14 +142,11 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.link_established.add_callback( lambda uri: logger.info('Callback->Connected to [%s]', uri)) self.connection_lost.add_callback( - lambda uri, errmsg: logger.info( - 'Callback->Connection lost to [%s]: %s', uri, errmsg)) + lambda uri, errmsg: logger.info('Callback->Connection lost to [%s]: %s', uri, errmsg)) self.connection_failed.add_callback( - lambda uri, errmsg: logger.info( - 'Callback->Connected failed to [%s]: %s', uri, errmsg)) + lambda uri, errmsg: logger.info('Callback->Connected failed to [%s]: %s', uri, errmsg)) self.connection_requested.add_callback( - lambda uri: logger.info( - 'Callback->Connection initialized[%s]', uri)) + lambda uri: logger.info('Callback->Connection initialized[%s]', uri)) self.connected.add_callback( lambda uri: logger.info('Callback->Connection setup finished [%s]', uri)) self.fully_connected.add_callback( diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index f2de4cd6a..30c394694 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -537,8 +537,7 @@ def request_extended_types(self, elements): pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) - pk.data = struct.pack(' Date: Mon, 27 Jun 2022 16:09:04 +0200 Subject: [PATCH 436/861] Moved call to all-params-updated-callback, to after all individual param update callbacks have been called --- cflib/crazyflie/param.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 30c394694..272f4bc63 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -203,13 +203,6 @@ def _param_updated(self, pk): self.values[element.group] = {} self.values[element.group][element.name] = s - # Once all the parameters are updated call the - # callback for "everything updated" - if self._check_if_all_updated() and not self.is_updated: - self.is_updated = True - self._initialized.set() - self.all_updated.call() - logger.debug('Updated parameter [%s]' % complete_name) if complete_name in self.param_update_callbacks: self.param_update_callbacks[complete_name].call( @@ -218,6 +211,13 @@ def _param_updated(self, pk): self.group_update_callbacks[element.group].call( complete_name, s) self.all_update_callback.call(complete_name, s) + + # Once all the parameters are updated call the + # callback for "everything updated" + if self._check_if_all_updated() and not self.is_updated: + self.is_updated = True + self._initialized.set() + self.all_updated.call() else: logger.debug('Variable id [%d] not found in TOC', var_id) From 6d7fbd196af4278b681e36691c8e3b2146917db8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 28 Jun 2022 10:06:29 +0200 Subject: [PATCH 437/861] Raise exception if set/get parameter is called callback before params are downloaded --- cflib/crazyflie/__init__.py | 4 ++++ cflib/crazyflie/param.py | 14 ++++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 7bf6790b1..de22fc1ca 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -34,6 +34,7 @@ import logging import time from collections import namedtuple +from threading import current_thread from threading import Lock from threading import Thread from threading import Timer @@ -354,6 +355,9 @@ def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): self.packet_sent.call(pk) self._send_lock.release() + def is_called_by_incoming_handler_thread(self): + return current_thread() == self.incoming + _CallbackContainer = namedtuple('CallbackConstainer', 'port port_mask channel channel_mask callback') diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 272f4bc63..c3af16ffc 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -316,8 +316,11 @@ def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ - if not self._initialized.wait(timeout=60): - raise Exception('Connection timed out') + if not self._initialized.isSet(): + if self.cf.is_called_by_incoming_handler_thread(): + raise Exception('Can not set parameter from callback until fully connected.') + if not self._initialized.wait(timeout=60): + raise Exception('Connection timed out') element = self.toc.get_element_by_complete_name(complete_name) @@ -351,8 +354,11 @@ def get_value(self, complete_name, timeout=60): Read a value for the supplied parameter. This can block for a period of time if the parameter values have not been fetched yet. """ - if not self._initialized.wait(timeout=60): - raise Exception('Connection timed out') + if not self._initialized.isSet(): + if self.cf.is_called_by_incoming_handler_thread(): + raise Exception('Can not get parameter from callback until fully connected.') + if not self._initialized.wait(timeout=60): + raise Exception('Connection timed out') [group, name] = complete_name.split('.') return self.values[group][name] From ffdcfae7ecabcd70e4d0150eb30c5a211ad39a29 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 28 Jun 2022 10:16:23 +0200 Subject: [PATCH 438/861] Updated SyncCrazyflie to use new callback --- cflib/crazyflie/syncCrazyflie.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index 779e934c7..6a8e82647 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2016 Bitcraze AB +# Copyright (C) 2016-2022 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -159,14 +159,14 @@ def _disconnected(self, link_uri): if self._disconnect_event: self._disconnect_event.set() - def _all_params_updated(self): + def _all_params_updated(self, link_uri): self._params_updated_event.set() def _add_callbacks(self): self.cf.connected.add_callback(self._connected) self.cf.connection_failed.add_callback(self._connection_failed) self.cf.disconnected.add_callback(self._disconnected) - self.cf.param.all_updated.add_callback(self._all_params_updated) + self.cf.fully_connected.add_callback(self._all_params_updated) def _remove_callbacks(self): def remove_callback(container, callback): @@ -178,4 +178,4 @@ def remove_callback(container, callback): remove_callback(self.cf.connected, self._connected) remove_callback(self.cf.connection_failed, self._connection_failed) remove_callback(self.cf.disconnected, self._disconnected) - remove_callback(self.cf.param.all_updated, self._all_params_updated) + remove_callback(self.cf.fully_connected, self._all_params_updated) From 64dc3cf86486f71183594a4ff6e89cd1d8d1b93e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 28 Jun 2022 11:37:56 +0200 Subject: [PATCH 439/861] Reset param state on connect --- cflib/crazyflie/param.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index c3af16ffc..c529fea21 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -152,11 +152,11 @@ def __init__(self, crazyflie): self.all_update_callback = Caller() self.param_updater = None - self.param_updater = _ParamUpdater( - self.cf, self._useV2, self._param_updated) + self.param_updater = _ParamUpdater(self.cf, self._useV2, self._param_updated) self.param_updater.start() self.cf.disconnected.add_callback(self._disconnected) + self.cf.connection_requested.add_callback(self._connection_requested) self.all_updated = Caller() self.is_updated = False @@ -277,11 +277,19 @@ def refresh_done(): refresh_done, toc_cache) toc_fetcher.start() + def _connection_requested(self, uri): + # Reset the internal state on connect to make sure we have a clean state + self.is_updated = False + self.toc = Toc() + self.values = {} + self._initialized.clear() + def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() - self.is_updated = False - self._initialized.clear() + + # Do not clear self.is_updated here as we might get spurious parameter updates later + # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} From 3e376f2cb8fd7131253d91dd5cc31d02b4e31a9e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 28 Jun 2022 11:39:12 +0200 Subject: [PATCH 440/861] Update basicparam example --- examples/parameters/basicparam.py | 73 ++++++++++++++----------------- 1 file changed, 33 insertions(+), 40 deletions(-) diff --git a/examples/parameters/basicparam.py b/examples/parameters/basicparam.py index 6dc18b713..51c57a894 100644 --- a/examples/parameters/basicparam.py +++ b/examples/parameters/basicparam.py @@ -34,7 +34,7 @@ from cflib.crazyflie import Crazyflie from cflib.utils import uri_helper -address = uri_helper.address_from_env(default=0xE7E7E7E7E7) +address = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -53,6 +53,7 @@ def __init__(self, link_uri): # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) + self._cf.fully_connected.add_callback(self._fully_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) @@ -72,7 +73,7 @@ def __init__(self, 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.""" + has been connected and the TOCs have been downloaded. Parameter values are not downloaded yet.""" print('Connected to %s' % link_uri) # Print the param TOC @@ -84,8 +85,7 @@ def _connected(self, link_uri): self._param_check_list.append('{0}.{1}'.format(group, param)) self._param_groups.append('{}'.format(group)) # For every group, register the callback - self._cf.param.add_update_callback(group=group, name=None, - cb=self._param_callback) + self._cf.param.add_update_callback(group=group, name=None, cb=self._param_callback) # You can also register a callback for a specific group.name combo self._cf.param.add_update_callback(group='cpu', name='flash', @@ -93,6 +93,21 @@ def _connected(self, link_uri): print('') + def _fully_connected(self, link_uri): + """This callback is called when the Crazyflie has been connected and all parameters have been + downloaded. It is now OK to set and get parameters.""" + print(f'Parameters downloaded to {link_uri}') + + # We can get a parameter value directly without using a callback + value = self._cf.param.get_value('pid_attitude.pitch_kd') + print(f'Value read with get() is {value}') + + # When a parameter is set, the callback is called with the new value + self._cf.param.add_update_callback(group='pid_attitude', name='pitch_kd', cb=self._a_pitch_kd_callback) + # When setting a value the parameter is automatically read back + # and the registered callbacks will get the updated value + self._cf.param.set_value('pid_attitude.pitch_kd', 0.1234) + def _cpu_flash_callback(self, name, value): """Specific callback for the cpu.flash parameter""" print('The connected Crazyflie has {}kb of flash'.format(value)) @@ -101,35 +116,20 @@ def _param_callback(self, name, value): """Generic callback registered for all the groups""" print('{0}: {1}'.format(name, value)) - # Remove each parameter from the list and close the link when - # all are fetched + # Remove each parameter from the list when fetched self._param_check_list.remove(name) if len(self._param_check_list) == 0: print('Have fetched all parameter values.') - # First remove all the group callbacks + # Remove all the group callbacks for g in self._param_groups: - self._cf.param.remove_update_callback(group=g, - cb=self._param_callback) - - # Create a new random value [0.00,1.00] for pid_attitude.pitch_kd - # and set it - pkd = random.random() - print('') - print('Write: pid_attitude.pitch_kd={:.2f}'.format(pkd)) - self._cf.param.add_update_callback(group='pid_attitude', - name='pitch_kd', - cb=self._a_pitch_kd_callback) - # When setting a value the parameter is automatically read back - # and the registered callbacks will get the updated value - self._cf.param.set_value('pid_attitude.pitch_kd', - '{:.2f}'.format(pkd)) + self._cf.param.remove_update_callback(group=g, cb=self._param_callback) def _a_pitch_kd_callback(self, name, value): """Callback for pid_attitude.pitch_kd""" - print('Readback: {0}={1}'.format(name, value)) + print('Read back: {0}={1}'.format(name, value)) - # End the example by closing the link (will cause the app to quit) + # This is the end of the example, close link self._cf.close_link() def _connection_failed(self, link_uri, msg): @@ -150,21 +150,14 @@ def _disconnected(self, link_uri): if __name__ == '__main__': + import logging + logging.getLogger().setLevel(logging.INFO) # Initialize the low-level drivers cflib.crtp.init_drivers() - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces(address) - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - pe = ParamExample(available[0][0]) - # The Crazyflie lib doesn't contain anything to keep the application - # alive, so this is where your application should do something. In our - # case we are just waiting until we are disconnected. - while pe.is_connected: - time.sleep(1) - else: - print('No Crazyflies found, cannot run example') + pe = ParamExample(address) + + # The Crazyflie lib doesn't contain anything to keep the application + # alive, so this is where your application should do something. In our + # case we are just waiting until we are disconnected. + while pe.is_connected: + time.sleep(1) From 3859359441551ceee00b51cc25f43524f6bc2723 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 28 Jun 2022 11:55:38 +0200 Subject: [PATCH 441/861] Updated documentation --- docs/user-guides/python_api.md | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 1ec10fc1b..9517e4916 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -36,7 +36,7 @@ ideas for more like *udp*, *serial*, *usb*, etc\...Here are some examples: - _radio://0/10/250K_ : Radio interface, USB dongle number 0, radio channel 10 and radio - speed 250 Kbit/s: radio://0/10/250K + speed 250 Kbit/s: radio://0/10/250K - _debug://0/1_ : Debug interface, id 0, channel 1 ### Variables and logging @@ -159,6 +159,8 @@ the following callbacks when events occur: # Called when the link is established and the TOCs (that are not cached) # have been downloaded connected = Caller() + # Called when the the link is established and all data, including parameters have been downloaded + fully_connected = Caller() # Called if establishing of the link fails (i.e times out) connection_failed = Caller() # Called for every packet received @@ -258,7 +260,7 @@ functionality should be used when: If this is not the case then the logging framework should be used instead. -To set a parameter you have to the connected to the Crazyflie. A +To set a parameter you must have the connected to the Crazyflie. A parameter is set using: ``` python @@ -297,6 +299,21 @@ Here\'s an example of how to use the calls. print "%s has value %d" % (name, value) ``` +It is also possible to get the current value of a parameter (when connected) without using a callback +``` python + value = get_value(complete_name) +``` + +Note 1: If you call `set_value()` and then directly call `get_value()` for a parameter, you might not read back the new +value, but get the old one instead. The process is asynchronous and `get_value()` will not return the new value until +the parameter value has propagated to the Crazyflie and back. Use the callback method if you need to be certain +that you get the correct value after an update. + +Note 2: `get_value()` and `set_value()` can not be called from callbacks until the Crazyflie is fully connected. +Most notably they can not be called from the `connected` callback as the parameter values have not been +downloaded yet. Use the `fully_connected` callback to make sure the system is ready for parameter use. It is OK to +call `get_value()` and `set_value()` from the `fully_connected` callback. + ## Logging The logging framework is used to enable the \"automatic\" sending of From 15ba8d987ee59a6b63ccaab730a6abb79b2a2233 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 28 Jun 2022 12:35:37 +0200 Subject: [PATCH 442/861] Updated unit test --- test/crazyflie/test_syncCrazyflie.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/test/crazyflie/test_syncCrazyflie.py b/test/crazyflie/test_syncCrazyflie.py index 0a88b0f6d..579c769d2 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -26,7 +26,6 @@ from unittest.mock import MagicMock from cflib.crazyflie import Crazyflie -from cflib.crazyflie import Param from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper from cflib.utils.callbacks import Caller @@ -41,6 +40,7 @@ def setUp(self): self.cf_mock.connected = Caller() self.cf_mock.connection_failed = Caller() self.cf_mock.disconnected = Caller() + self.cf_mock.fully_connected = Caller() self.cf_mock.open_link = AsyncCallbackCaller( cb=self.cf_mock.connected, @@ -55,11 +55,6 @@ def setUp(self): ) self.cf_mock.close_link = self.close_link_mock.trigger - # Mock the behaviour that param values are updated(downloaded) after connection - self.param_mock = MagicMock(spec=Param) - self.param_mock.all_updated = Caller() - self.cf_mock.param = self.param_mock - # Register a callback to be called when connected. Use it to trigger a callback # to trigger the call to the param.all_updated() callback self.cf_mock.connected.add_callback(self._connected_callback) @@ -233,10 +228,11 @@ def _assertAllCallbacksAreRemoved(self): self.assertEqual(0, len(self.cf_mock.connected.callbacks)) self.assertEqual(0, len(self.cf_mock.connection_failed.callbacks)) self.assertEqual(0, len(self.cf_mock.disconnected.callbacks)) - self.assertEqual(0, len(self.param_mock.all_updated.callbacks)) + self.assertEqual(0, len(self.cf_mock.fully_connected.callbacks)) def _connected_callback(self, uri): AsyncCallbackCaller( - cb=self.param_mock.all_updated, + cb=self.cf_mock.fully_connected, + args=[self.uri], delay=0.2 ).trigger() From 69398010152a2e4d2e52837015bea103ab40bb7d Mon Sep 17 00:00:00 2001 From: Akash Patel <17132214+acxz@users.noreply.github.com> Date: Fri, 1 Jul 2022 09:06:38 -0400 Subject: [PATCH 443/861] correct typo to properly exclude the test folder --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 49d52c000..d25aca5fe 100644 --- a/setup.py +++ b/setup.py @@ -10,7 +10,7 @@ setup( name='cflib', version='0.1.19', - packages=find_packages(exclude=['examples', 'tests']), + packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', url='https://github.com/bitcraze/crazyflie-lib-python', From fba80c94c05948d91091f56b58726c3d0a09002c Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 6 Jul 2022 21:44:26 +0200 Subject: [PATCH 444/861] Set Crazyflie disconnected state properly --- cflib/crazyflie/__init__.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 2e09afdf2..eabed3fd9 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -261,6 +261,7 @@ def close_link(self): self.link = None self._answer_patterns = {} self.disconnected.call(self.link_uri) + self.state = State.DISCONNECTED """Check if the communication link is open or not.""" From 1028500752f8620ddc6276d248c9b042e1fce14d Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 6 Jul 2022 21:45:16 +0200 Subject: [PATCH 445/861] Fixed socket CPX transport after serial re-implementation --- cflib/cpx/transports.py | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/cflib/cpx/transports.py b/cflib/cpx/transports.py index 405bcd86a..ea263c868 100644 --- a/cflib/cpx/transports.py +++ b/cflib/cpx/transports.py @@ -1,6 +1,7 @@ import socket from threading import Lock +import struct from . import CPXPacket @@ -45,18 +46,37 @@ def connect(self): def disconnect(self): print("Closing transport") + self._socket.shutdown(socket.SHUT_WR) self._socket.close() self._socket = None - def write(self, data): + def writePacket(self, packet): + #print("Write: {}".format(packet)) + data = bytearray(struct.pack("H", packet.length+2)) + data += packet.wireData self._socket.send(data) - def read(self, size): + def _readData(self, size): data = bytearray() while len(data) < size and self._socket is not None: data.extend(self._socket.recv(size-len(data))) return data + def readPacket(self): + size = struct.unpack("H", self._readData(2))[0] + + data = self._readData(size) + + packet = CPXPacket() + packet.wireData = data + #print("Read: {}".format(packet)) + + return packet + + def __del__(self): + print("Socket transport is being destroyed!") + + class UARTTransport(CPXTransport): def __init__(self, device, baudrate): print("CPX UART transport") From 57a34f3c9ed6dfb15ed2848d2442f37e76593a52 Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 6 Jul 2022 21:45:49 +0200 Subject: [PATCH 446/861] Renamed CPX Driver to TCP Driver --- cflib/crtp/__init__.py | 4 ++-- cflib/crtp/{cpxdriver.py => tcpdriver.py} | 10 ++++++---- 2 files changed, 8 insertions(+), 6 deletions(-) rename cflib/crtp/{cpxdriver.py => tcpdriver.py} (96%) diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 6d0d2dd8b..0b7b4ff24 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -32,7 +32,7 @@ from .serialdriver import SerialDriver from .udpdriver import UdpDriver from .usbdriver import UsbDriver -from .cpxdriver import CPXDriver +from .tcpdriver import TcpDriver __author__ = 'Bitcraze AB' __all__ = [] @@ -59,7 +59,7 @@ def init_drivers(enable_debug_driver=False, enable_serial_driver=False): if enable_serial_driver: CLASSES.append(SerialDriver) - CLASSES.extend([UdpDriver, PrrtDriver, CPXDriver]) + CLASSES.extend([UdpDriver, PrrtDriver, TcpDriver]) def scan_interfaces(address=None): diff --git a/cflib/crtp/cpxdriver.py b/cflib/crtp/tcpdriver.py similarity index 96% rename from cflib/crtp/cpxdriver.py rename to cflib/crtp/tcpdriver.py index 3c56b3ef7..64b471d04 100644 --- a/cflib/crtp/cpxdriver.py +++ b/cflib/crtp/tcpdriver.py @@ -42,7 +42,7 @@ logger = logging.getLogger(__name__) __author__ = 'Bitcraze AB' -__all__ = ['CPXDriver'] +__all__ = ['TcpDriver'] # For each scan the driver is re-initialized, if we do ZeroConf inside # the driver init we will not have time to find any devices, so start @@ -69,7 +69,7 @@ def add_service(self, zeroconf, type, name): def getAvailableHosts(self): cpxHosts = [] for hosts in self._hosts: - cpxHosts.append(("cpx://{}:{}".format(hosts.server, hosts.port), hosts.properties[b"name"])) + cpxHosts.append(("tcp://{}:{}".format(hosts.server, hosts.port), hosts.properties[b"name"])) return cpxHosts def update_service(self, zeroconf, type, name): @@ -77,13 +77,13 @@ def update_service(self, zeroconf, type, name): persistentZeroContListener = ZeroConfListener() -class CPXDriver(CRTPDriver): +class TcpDriver(CRTPDriver): def __init__(self): self.needs_resending = False def connect(self, uri, linkQualityCallback, linkErrorCallback): - if not re.search('^cpx://', uri): + if not re.search('^tcp://', uri): raise WrongUriType('Not an UDP URI') parse = urlparse(uri.split(" ")[0]) @@ -120,6 +120,7 @@ def receive_packet(self, time=0): def send_packet(self, pk): raw = (pk.header,) + struct.unpack('B' * len(pk.data), pk.data) + #print("OUT: {}".format(pk)) self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, function=CPXFunction.CRTP, data=raw)) @@ -183,6 +184,7 @@ def run(self): if len(data) > 0: pk = CRTPPacket(data[0], list(data[1:])) + #print("IN: {}".format(pk)) self.in_queue.put(pk) except queue.Empty as e: pass # This is ok From b96763468a74f6ebc91b194ae8f77e68a77e34b9 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 12 Jul 2022 14:58:29 +0200 Subject: [PATCH 447/861] Updated CPX bridge enable command --- cflib/crtp/serialdriver.py | 2 +- cflib/crtp/tcpdriver.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index bb0e9dffd..af577dbfb 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -98,7 +98,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): # Switch the link bridge to CPX in the Crazyflie self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, function=CPXFunction.SYSTEM, - data=[0x10, 0x01])) + data=[0x21, 0x01])) # Force client connect to true self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, function=CPXFunction.SYSTEM, diff --git a/cflib/crtp/tcpdriver.py b/cflib/crtp/tcpdriver.py index 64b471d04..1cb3e1bb0 100644 --- a/cflib/crtp/tcpdriver.py +++ b/cflib/crtp/tcpdriver.py @@ -99,7 +99,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, function=CPXFunction.SYSTEM, - data=[0x10, 0x01])) + data=[0x21, 0x01])) def receive_packet(self, time=0): if time == 0: From 871fc93d666eab1e161e691114fa330cd76638cf Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 12 Jul 2022 15:24:48 +0200 Subject: [PATCH 448/861] Fixes for Flake8 --- cflib/cpx/__init__.py | 180 ++++++++++++--------------- cflib/cpx/gap8/bootloader.py | 11 +- cflib/cpx/transports.py | 202 +++++++++++++++---------------- cflib/crtp/__init__.py | 2 +- cflib/crtp/serialdriver.py | 25 ++-- cflib/crtp/tcpdriver.py | 51 ++++---- examples/aideck/crtp-cpx-wifi.py | 129 +++++++++----------- setup.py | 1 + 8 files changed, 283 insertions(+), 318 deletions(-) diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index 2bed6d452..99d35b582 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -23,40 +23,37 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . """ CPX Router and discovery""" +import enum import queue -import socket import struct import threading -import enum -from urllib.parse import urlparse -from zeroconf import ServiceBrowser, Zeroconf -from .gap8.bootloader import GAP8Bootloader __author__ = 'Bitcraze AB' __all__ = ['CPXRouter'] -print("CPX import") class CPXTarget(enum.Enum): - """ - List of CPX targets - """ - STM32 = 1 - ESP32 = 2 - HOST = 3 - GAP8 = 4 + """ + List of CPX targets + """ + STM32 = 1 + ESP32 = 2 + HOST = 3 + GAP8 = 4 + class CPXFunction(enum.Enum): - """ - List of CPX targets - """ - SYSTEM = 1 - CONSOLE = 2 - CRTP = 3 - WIFI_CTRL = 4 - APP = 5 - TEST = 0x0E - BOOTLOADER = 0x0F + """ + List of CPX targets + """ + SYSTEM = 1 + CONSOLE = 2 + CRTP = 3 + WIFI_CTRL = 4 + APP = 5 + TEST = 0x0E + BOOTLOADER = 0x0F + class CPXPacket(object): """ @@ -77,26 +74,19 @@ def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytear def _get_wire_data(self): """Create raw data to send via the wire""" raw = bytearray() - # This is the length excluding the 2 byte legnth - #wireLength = len(self.data) + 2 # 2 bytes for CPX header + targetsAndFlags = ((self.source.value & 0x7) << 3) | (self.destination.value & 0x7) if self.lastPacket: - targetsAndFlags |= 0x40 - #print(self.destination) - #print(self.source) - #print(targets) + targetsAndFlags |= 0x40 + function = self.function.value & 0xFF - raw.extend(struct.pack(" 1022): - # raise "Cannot send this packet, the size is too large!" return raw def _set_wire_data(self, data): - [targetsAndFlags, self.function] = struct.unpack("> 3) & 0x07) self.destination = CPXTarget(targetsAndFlags & 0x07) self.lastPacket = targetsAndFlags & 0x40 != 0 @@ -106,103 +96,83 @@ def _set_wire_data(self, data): def __str__(self): """Get a string representation of the packet""" - return "{}->{}/{} (size={} bytes)".format(self.source, self.destination, self.function, self.length) + return '{}->{}/{} (size={} bytes)'.format(self.source, self.destination, self.function, self.length) wireData = property(_get_wire_data, _set_wire_data) -# Internal here, route to modules and from public facing API + class CPXRouter(threading.Thread): - + def __init__(self, transport): - threading.Thread.__init__(self) - self._transport = transport - self._rxQueues = {} - self._packet_assembly = [] - self._connected = True + threading.Thread.__init__(self) + self._transport = transport + self._rxQueues = {} + self._packet_assembly = [] + self._connected = True # Register and/or blocking calls for ports def receivePacket(self, function, timeout=None): - # Check if a queue exists, if not then create it - # the user might have implemented new functions - if not function.value in self._rxQueues: - print("Creating queue for {}".format(function)) - self._rxQueues[function.value] = queue.Queue() + # Check if a queue exists, if not then create it + # the user might have implemented new functions + if function.value not in self._rxQueues: + print('Creating queue for {}'.format(function)) + self._rxQueues[function.value] = queue.Queue() - return self._rxQueues[function.value].get(block=True, timeout=timeout) + return self._rxQueues[function.value].get(block=True, timeout=timeout) def makeTransaction(self, packet): - self.sendPacket(packet) - return self.receivePacket(packet.function) + self.sendPacket(packet) + return self.receivePacket(packet.function) def sendPacket(self, packet): - # Do we queue here? - self._transport.writePacket(packet) + self._transport.writePacket(packet) def transport(self): - self._connected = False - return self._transport - + self._connected = False + return self._transport + def run(self): while(self._connected): - # Read one packet from the transport - - # Packages might have been split up along the - # way, due to MTU limitations on links. But here we have - # lots of memory, so assemble full packets by looking at last - # packet byte. Note that chunks of one packet could be mixed - # with chunks from antother packet. - try: - packet = self._transport.readPacket() - #header = self._transport.read(4) - #packet = CPXPacket(wireHeader=header) - #packet.data = self._transport.read(packet.length - 2) # remove routing info here - #print(packet) - # if not packet.target in self._packet_assembly: - # self._packet_assembly[packet.target][packet.function] = [] - # else - # if not packet.function in self._packet_assembly[packet.target]: - # self._packet_assembly[packet.target][packet.function] = [] - - # self._packet_assembly[packet.target][packet.function].append(packet) - - # if (packet.lastPart): - # # Assemble packet and send up stack - # pass - if not packet.function.value in self._rxQueues: - pass - #print("Got packet for {}, but have no queue".format(packet.function)) - else: - self._rxQueues[packet.function.value].put(packet) - #self._rxQueues[packet.function.value] = queue.Queue() - #print(self._rxQueues[packet.function.value].qsize()) - except Exception as e: - print("Exception while reading transport, link probably closed?") - print(e) - import traceback - print(traceback.format_exc()) - -# Public facing + # Read one packet from the transport + + # Packages might have been split up along the + # way, we should re-assemble here + try: + packet = self._transport.readPacket() + + if packet.function.value not in self._rxQueues: + pass + else: + self._rxQueues[packet.function.value].put(packet) + except Exception as e: + print('Exception while reading transport, link probably closed?') + print(e) + import traceback + print(traceback.format_exc()) + +# Public facing + + class CPX: def __init__(self, transport): - self._router = CPXRouter(transport) - self._router.start() - #self.gap8.bootloader = GAP8Bootloader(self) + self._router = CPXRouter(transport) + self._router.start() def receivePacket(self, function, timeout=None): - # Block on a function queue + # Block on a function queue - #if self._router.isUsedBySubmodule(target, function): - # raise ValueError("The CPX target {} and function {} is registered in a sub module".format(target, function)) + # if self._router.isUsedBySubmodule(target, function): + # raise ValueError("The CPX target {} and function {} is registered in a sub module".format(target, function)) - # Will block on queue - return self._router.receivePacket(function, timeout) + # Will block on queue + return self._router.receivePacket(function, timeout) def makeTransaction(self, packet): - return self._router.makeTransaction(packet) + return self._router.makeTransaction(packet) def sendPacket(self, packet): - self._router.sendPacket(packet) + self._router.sendPacket(packet) def close(self): - self._router.transport().disconnect() \ No newline at end of file + self._router.transport().disconnect() diff --git a/cflib/cpx/gap8/bootloader.py b/cflib/cpx/gap8/bootloader.py index 4e2546ad9..3870ec4dc 100644 --- a/cflib/cpx/gap8/bootloader.py +++ b/cflib/cpx/gap8/bootloader.py @@ -1,13 +1,12 @@ - -#from cflib.cpx import CPXTarget, CPXFunction +# from cflib.cpx import CPXTarget, CPXFunction class GAP8Bootloader: - #target = CPXTarget.GAP8 - #function = CPXFunction.BOOTLOADER + # target = CPXTarget.GAP8 + # function = CPXFunction.BOOTLOADER def __init__(self): - print("GAP8 bootloader module init") + print('GAP8 bootloader module init') # def getVersion(self): # version = self._cpx.transaction(CPXPacket(destination=CPXTarget.GAP8, @@ -54,4 +53,4 @@ def __init__(self): # function=CPXFunction.BOOTLOADER, # data=data[totalBytesWritten:totalBytesWritten+nextChunk]) # self._cpx.send(fwWritePacket) - # totalBytesWritten += nextChunk \ No newline at end of file + # totalBytesWritten += nextChunk diff --git a/cflib/cpx/transports.py b/cflib/cpx/transports.py index ea263c868..94d6a4efc 100644 --- a/cflib/cpx/transports.py +++ b/cflib/cpx/transports.py @@ -1,169 +1,169 @@ - import socket -from threading import Lock import struct +from threading import Lock from . import CPXPacket found_serial = True try: import serial - import serial.tools.list_ports as list_ports except ImportError: found_serial = False + class CPXTransport: def __init__(self): - raise NotImplementedError("Cannot be used") + raise NotImplementedError('Cannot be used') # Change this to URI? def connect(host, port): - raise NotImplementedError("Cannot be used") + raise NotImplementedError('Cannot be used') def disconnect(): - raise NotImplementedError("Cannot be used") + raise NotImplementedError('Cannot be used') def send(self, data): - raise NotImplementedError("Cannot be used") + raise NotImplementedError('Cannot be used') def receive(self, size): - raise NotImplementedError("Cannot be used") + raise NotImplementedError('Cannot be used') + class SocketTransport(CPXTransport): def __init__(self, host, port): - print("CPX socket transport") - self._host = host - self._port = port + print('CPX socket transport') + self._host = host + self._port = port - self.connect() + self.connect() def connect(self): - print("Connecting to socket on {}:{}...".format(self._host, self._port)) - self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self._socket.connect((self._host, self._port)) - #self._socket.settimeout(0.1) - print("Connected") + print('Connecting to socket on {}:{}...'.format(self._host, self._port)) + self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._socket.connect((self._host, self._port)) + print('Connected') def disconnect(self): - print("Closing transport") - self._socket.shutdown(socket.SHUT_WR) - self._socket.close() - self._socket = None + print('Closing transport') + self._socket.shutdown(socket.SHUT_WR) + self._socket.close() + self._socket = None def writePacket(self, packet): - #print("Write: {}".format(packet)) - data = bytearray(struct.pack("H", packet.length+2)) - data += packet.wireData - self._socket.send(data) + data = bytearray(struct.pack('H', packet.length+2)) + data += packet.wireData + self._socket.send(data) def _readData(self, size): - data = bytearray() - while len(data) < size and self._socket is not None: - data.extend(self._socket.recv(size-len(data))) - return data + data = bytearray() + while len(data) < size and self._socket is not None: + data.extend(self._socket.recv(size-len(data))) + return data def readPacket(self): - size = struct.unpack("H", self._readData(2))[0] + size = struct.unpack('H', self._readData(2))[0] - data = self._readData(size) + data = self._readData(size) - packet = CPXPacket() - packet.wireData = data - #print("Read: {}".format(packet)) + packet = CPXPacket() + packet.wireData = data - return packet + return packet - def __del__(self): - print("Socket transport is being destroyed!") + def __del__(self): + print('Socket transport is being destroyed!') class UARTTransport(CPXTransport): def __init__(self, device, baudrate): - print("CPX UART transport") - self._device = device - self._baudrate = baudrate - self._serial = None - self._cts = False - self._lock = Lock() + print('CPX UART transport') + self._device = device + self._baudrate = baudrate + self._serial = None + self._cts = False + self._lock = Lock() - self.connect() + self.connect() def connect(self): - print("Connecting to UART on {} @ {}...".format(self._device, self._baudrate)) - self._serial = serial.Serial(self._device, self._baudrate, timeout=None) - - isInSync = False - - while not isInSync: - start = self._serial.read(1)[0] - print(start) - if start == 0xFF: - print("Got start") - size = self._serial.read(1)[0] - print(size) - if size == 0x00: - isInSync = True - self.cts = True - - # Send back sync - self._serial.write([0xFF, 0x00]) - - print("Connected") + print('Connecting to UART on {} @ {}...'.format(self._device, self._baudrate)) + self._serial = serial.Serial(self._device, self._baudrate, timeout=None) + + isInSync = False + + while not isInSync: + start = self._serial.read(1)[0] + print(start) + if start == 0xFF: + print('Got start') + size = self._serial.read(1)[0] + print(size) + if size == 0x00: + isInSync = True + self.cts = True + + # Send back sync + self._serial.write([0xFF, 0x00]) + + print('Connected') def _calcXORchecksum(self, data): - checksum = 0 - for i in data: - checksum ^= i - return checksum + checksum = 0 + for i in data: + checksum ^= i + return checksum def disconnect(self): - print("Closing transport") - self._serial.close() - self._serial = None + print('Closing transport') + self._serial.close() + self._serial = None def writePacket(self, packet): - self._lock.acquire() - data = packet.wireData - if len(data) > 100: - raise "Packet too large!" + self._lock.acquire() + data = packet.wireData + if len(data) > 100: + raise 'Packet too large!' - buff = bytearray([0xFF, len(data)]) - buff.extend(data) - buff.extend([self._calcXORchecksum(buff)]) - self._serial.write(buff) + buff = bytearray([0xFF, len(data)]) + buff.extend(data) + buff.extend([self._calcXORchecksum(buff)]) + self._serial.write(buff) def readPacket(self): - size = 0 - while size == 0: - start = self._serial.read(1)[0] - if start == 0xFF: - size = self._serial.read(1)[0] - if size == 0: - self._lock.release() - else: - data = self._serial.read(size) # Size is excluding start (0xFF) and checksum at end - crc = self._serial.read(1) - # Send CTS - self._serial.write([0xFF, 0x00]) - - packet = CPXPacket() - packet.wireData = data - - return packet + size = 0 + while size == 0: + start = self._serial.read(1)[0] + if start == 0xFF: + size = self._serial.read(1)[0] + if size == 0: + self._lock.release() + else: + data = self._serial.read(size) # Size is excluding start (0xFF) and checksum at end + crc = self._serial.read(1) + if self._calcXORchecksum(data) != crc: + print('CRC error!') + # Send CTS + self._serial.write([0xFF, 0x00]) + + packet = CPXPacket() + packet.wireData = data + + return packet + class CRTPTransport(CPXTransport): def __init__(self): - print("CPX CRTP transport") + print('CPX CRTP transport') # This connection will not really work... def connect(host, port): - pass + pass def disconnect(): - pass + pass def send(self, data): - pass + pass def receive(self, size): - pass + pass diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 0b7b4ff24..756e9a66c 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -30,9 +30,9 @@ from .prrtdriver import PrrtDriver from .radiodriver import RadioDriver from .serialdriver import SerialDriver +from .tcpdriver import TcpDriver from .udpdriver import UdpDriver from .usbdriver import UsbDriver -from .tcpdriver import TcpDriver __author__ = 'Bitcraze AB' __all__ = [] diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index af577dbfb..4cf228e13 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -29,19 +29,20 @@ import logging import queue import re -import threading import struct +import threading from .crtpstack import CRTPPacket from .exceptions import WrongUriType -from cflib.crtp.crtpdriver import CRTPDriver - -from cflib.cpx import CPX, CPXPacket, CPXTarget, CPXFunction +from cflib.cpx import CPX +from cflib.cpx import CPXFunction +from cflib.cpx import CPXPacket +from cflib.cpx import CPXTarget from cflib.cpx.transports import UARTTransport +from cflib.crtp.crtpdriver import CRTPDriver found_serial = True try: - import serial import serial.tools.list_ports as list_ports except ImportError: found_serial = False @@ -51,6 +52,7 @@ logger = logging.getLogger(__name__) + class SerialDriver(CRTPDriver): def __init__(self): @@ -103,7 +105,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, function=CPXFunction.SYSTEM, data=[0x20, 0x01])) - + # TODO! These should be reset again! def send_packet(self, pk): @@ -131,9 +133,9 @@ def get_name(self): return 'serial' def scan_interface(self, address): - print("Scanning serial") + print('Scanning serial') if found_serial: - print("Found serial") + print('Found serial') devices_names = self.get_devices().keys() print(devices_names) return [('serial://' + x, '') for x in devices_names] @@ -154,7 +156,7 @@ def close(self): print(e) logger.error('Could not close {}'.format(e)) pass - print("Driver closed") + print('Driver closed') def get_devices(self): result = {} @@ -168,6 +170,7 @@ def get_devices(self): return result + class _CPXReceiveThread(threading.Thread): """ Radio link receiver thread used to read data from the @@ -204,8 +207,8 @@ def run(self): pk = CRTPPacket(data[0], list(data[1:])) self.in_queue.put(pk) - except queue.Empty as e: - pass # This is ok + except queue.Empty: + pass # This is ok except Exception as e: import traceback diff --git a/cflib/crtp/tcpdriver.py b/cflib/crtp/tcpdriver.py index 1cb3e1bb0..f8c7ebd97 100644 --- a/cflib/crtp/tcpdriver.py +++ b/cflib/crtp/tcpdriver.py @@ -23,22 +23,24 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . """ CRTP CPX Driver. Tunnel CRTP over CPX to the Crazyflie STM32 """ +import logging import queue import re -import socket import struct import threading from urllib.parse import urlparse -from zeroconf import ServiceBrowser, Zeroconf -from cflib.cpx import CPX, CPXPacket, CPXTarget, CPXFunction -from cflib.cpx.transports import SocketTransport +from zeroconf import ServiceBrowser +from zeroconf import Zeroconf from .crtpdriver import CRTPDriver from .crtpstack import CRTPPacket from .exceptions import WrongUriType - -import logging +from cflib.cpx import CPX +from cflib.cpx import CPXFunction +from cflib.cpx import CPXPacket +from cflib.cpx import CPXTarget +from cflib.cpx.transports import SocketTransport logger = logging.getLogger(__name__) __author__ = 'Bitcraze AB' @@ -49,34 +51,38 @@ # ZeroCont at startup and keep it running all the time. The driver # will just query this to return discovered devices. persistentZeroContListener = None + + class ZeroConfListener: def __init__(self): self._hosts = [] zeroconf = Zeroconf() - browser = ServiceBrowser(zeroconf, "_cpx._tcp.local.", self) + ServiceBrowser(zeroconf, '_cpx._tcp.local.', self) def remove_service(self, zeroconf, type, name): - print("Service %s removed" % (name,)) + print('Service %s removed' % (name,)) info = zeroconf.get_service_info(type, name) self._hosts.remove(info) def add_service(self, zeroconf, type, name): info = zeroconf.get_service_info(type, name) - print("Service %s added, service info: %s" % (name, info)) + print('Service %s added, service info: %s' % (name, info)) self._hosts.append(info) - + def getAvailableHosts(self): - cpxHosts = [] - for hosts in self._hosts: - cpxHosts.append(("tcp://{}:{}".format(hosts.server, hosts.port), hosts.properties[b"name"])) - return cpxHosts + cpxHosts = [] + for hosts in self._hosts: + cpxHosts.append(('tcp://{}:{}'.format(hosts.server, hosts.port), hosts.properties[b'name'])) + return cpxHosts def update_service(self, zeroconf, type, name): - print("Updated service") + print('Updated service') + persistentZeroContListener = ZeroConfListener() + class TcpDriver(CRTPDriver): def __init__(self): @@ -86,7 +92,7 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): if not re.search('^tcp://', uri): raise WrongUriType('Not an UDP URI') - parse = urlparse(uri.split(" ")[0]) + parse = urlparse(uri.split(' ')[0]) self.in_queue = queue.Queue() @@ -96,7 +102,6 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): linkErrorCallback) self._thread.start() - self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, function=CPXFunction.SYSTEM, data=[0x21, 0x01])) @@ -120,7 +125,6 @@ def receive_packet(self, time=0): def send_packet(self, pk): raw = (pk.header,) + struct.unpack('B' * len(pk.data), pk.data) - #print("OUT: {}".format(pk)) self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, function=CPXFunction.CRTP, data=raw)) @@ -139,7 +143,7 @@ def close(self): print(e) logger.error('Could not close {}'.format(e)) pass - print("Driver closed") + print('Driver closed') self.cpx = None def get_name(self): @@ -149,6 +153,8 @@ def scan_interface(self, address): return persistentZeroContListener.getAvailableHosts() # Transmit/receive thread + + class _CPXReceiveThread(threading.Thread): """ Radio link receiver thread used to read data from the @@ -184,14 +190,13 @@ def run(self): if len(data) > 0: pk = CRTPPacket(data[0], list(data[1:])) - #print("IN: {}".format(pk)) self.in_queue.put(pk) - except queue.Empty as e: - pass # This is ok + except queue.Empty: + pass # This is ok except Exception as e: import traceback self.link_error_callback( 'Error communicating with the Crazyflie\n' 'Exception:%s\n\n%s' % (e, - traceback.format_exc())) \ No newline at end of file + traceback.format_exc())) diff --git a/examples/aideck/crtp-cpx-wifi.py b/examples/aideck/crtp-cpx-wifi.py index 9ba2199ad..3580b3526 100644 --- a/examples/aideck/crtp-cpx-wifi.py +++ b/examples/aideck/crtp-cpx-wifi.py @@ -1,4 +1,3 @@ - """ This example illustrates how to use CRTP over CPX while also using CPX to access other targets on the Crazyflie. This is done by connecting to @@ -9,93 +8,81 @@ GAP8 and an up to date code-base on the STM32/ESP32. """ - import logging -import time -import queue -import threading import struct +import threading import cflib.crtp +from cflib.cpx import CPXFunction 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.utils import uri_helper -from cflib.cpx import CPXPacket, CPXFunction, CPXTarget - -uri = uri_helper.uri_from_env(default='cpx://aideck-B09D74.local:5000') +uri = uri_helper.uri_from_env() # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) + class Example(threading.Thread): - def __init__(self, uri): - threading.Thread.__init__(self) - self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) - self._lg_stab.add_variable('stateEstimate.x', 'float') - self._lg_stab.add_variable('stateEstimate.y', 'float') - self._lg_stab.add_variable('stateEstimate.z', 'float') - self._lg_stab.add_variable('stabilizer.roll', 'float') - self._lg_stab.add_variable('stabilizer.pitch', 'float') - self._lg_stab.add_variable('stabilizer.yaw', 'float') - - self._cf = Crazyflie(rw_cache='./cache') - self._cf.connected.add_callback(self.connected) - self._cf.disconnected.add_callback(self.disconnected) - self._cf.open_link(uri) - self._cpx = None - - def connected(self, uri): - print("Connected to {}".format(uri)) - self._cpx = self._cf.link.cpx - self.start() - try: - self._cf.log.add_config(self._lg_stab) - # This callback will receive the data - self._lg_stab.data_received_cb.add_callback(self._stab_log_data) - # This callback will be called on errors - self._lg_stab.error_cb.add_callback(self._stab_log_error) - # Start the logging - self._lg_stab.start() - except KeyError as e: - print('Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - print('Could not add Stabilizer log config, bad configuration.') - - def disconnected(self, uri): - print("Disconnected from {}".format(uri)) - - 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): - print(f'[{timestamp}][{logconf.name}]: ', end='') - for name, value in data.items(): - print(f'{name}: {value:3.3f} ', end='') - print() - - def run(self): - while True: - p = self._cpx.receivePacket(CPXFunction.APP) - [magic, width, height, depth, format, size] = struct.unpack('=1.20,<1.25', 'opencv-python-headless~=4.5.5', + 'zeroconf>=0.26.1' ], # $ pip install -e .[dev] From d8feb414a41665a992541d661e617546a11074dc Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 12 Jul 2022 16:04:16 +0200 Subject: [PATCH 449/861] Removed Zeroconf since buider support is missing --- cflib/crtp/tcpdriver.py | 41 +---------------------------------------- requirements.txt | 1 - setup.py | 3 +-- 3 files changed, 2 insertions(+), 43 deletions(-) diff --git a/cflib/crtp/tcpdriver.py b/cflib/crtp/tcpdriver.py index f8c7ebd97..8325f5c87 100644 --- a/cflib/crtp/tcpdriver.py +++ b/cflib/crtp/tcpdriver.py @@ -30,9 +30,6 @@ import threading from urllib.parse import urlparse -from zeroconf import ServiceBrowser -from zeroconf import Zeroconf - from .crtpdriver import CRTPDriver from .crtpstack import CRTPPacket from .exceptions import WrongUriType @@ -46,42 +43,6 @@ __author__ = 'Bitcraze AB' __all__ = ['TcpDriver'] -# For each scan the driver is re-initialized, if we do ZeroConf inside -# the driver init we will not have time to find any devices, so start -# ZeroCont at startup and keep it running all the time. The driver -# will just query this to return discovered devices. -persistentZeroContListener = None - - -class ZeroConfListener: - def __init__(self): - self._hosts = [] - - zeroconf = Zeroconf() - ServiceBrowser(zeroconf, '_cpx._tcp.local.', self) - - def remove_service(self, zeroconf, type, name): - print('Service %s removed' % (name,)) - info = zeroconf.get_service_info(type, name) - self._hosts.remove(info) - - def add_service(self, zeroconf, type, name): - info = zeroconf.get_service_info(type, name) - print('Service %s added, service info: %s' % (name, info)) - self._hosts.append(info) - - def getAvailableHosts(self): - cpxHosts = [] - for hosts in self._hosts: - cpxHosts.append(('tcp://{}:{}'.format(hosts.server, hosts.port), hosts.properties[b'name'])) - return cpxHosts - - def update_service(self, zeroconf, type, name): - print('Updated service') - - -persistentZeroContListener = ZeroConfListener() - class TcpDriver(CRTPDriver): @@ -150,7 +111,7 @@ def get_name(self): return 'cpx' def scan_interface(self, address): - return persistentZeroContListener.getAvailableHosts() + return [] # Transmit/receive thread diff --git a/requirements.txt b/requirements.txt index c31386d93..8ba626a4e 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,2 +1 @@ pyusb>=1.0.0b2 -zeroconf>=0.26.1 diff --git a/setup.py b/setup.py index 2d840528e..9f6536394 100644 --- a/setup.py +++ b/setup.py @@ -37,8 +37,7 @@ 'libusb-package~=1.0', 'scipy~=1.7', 'numpy>=1.20,<1.25', - 'opencv-python-headless~=4.5.5', - 'zeroconf>=0.26.1' + 'opencv-python-headless~=4.5.5' ], # $ pip install -e .[dev] From 50281f1795a5d5af77f6cd00b0f25d96e0616e29 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 12 Jul 2022 16:26:22 +0200 Subject: [PATCH 450/861] Removed GAP8 bootloader draft file --- cflib/cpx/gap8/__init__.py | 0 cflib/cpx/gap8/bootloader.py | 56 ------------------------------------ 2 files changed, 56 deletions(-) delete mode 100644 cflib/cpx/gap8/__init__.py delete mode 100644 cflib/cpx/gap8/bootloader.py diff --git a/cflib/cpx/gap8/__init__.py b/cflib/cpx/gap8/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/cflib/cpx/gap8/bootloader.py b/cflib/cpx/gap8/bootloader.py deleted file mode 100644 index 3870ec4dc..000000000 --- a/cflib/cpx/gap8/bootloader.py +++ /dev/null @@ -1,56 +0,0 @@ -# from cflib.cpx import CPXTarget, CPXFunction - -class GAP8Bootloader: - - # target = CPXTarget.GAP8 - # function = CPXFunction.BOOTLOADER - - def __init__(self): - print('GAP8 bootloader module init') - - # def getVersion(self): - # version = self._cpx.transaction(CPXPacket(destination=CPXTarget.GAP8, - # function=CPXFunction.BOOTLOADER, - # data=bytearray([0x00]))) - # return version.data[1:] - - # def readFlash(self, start, count): - # cmd = struct.pack(" Date: Wed, 13 Jul 2022 13:36:55 +0200 Subject: [PATCH 451/861] Set CPX receive thread as daemon --- cflib/cpx/__init__.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index 99d35b582..a78158796 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -105,6 +105,7 @@ class CPXRouter(threading.Thread): def __init__(self, transport): threading.Thread.__init__(self) + self.daemon = True self._transport = transport self._rxQueues = {} self._packet_assembly = [] From e883739ef7a5a880e87c7868efc31ffa72262df1 Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 13 Jul 2022 13:46:55 +0200 Subject: [PATCH 452/861] Removed old example and added new one --- examples/aideck/crtp-cpx-wifi.py | 88 ----------- examples/aideck/fpv.py | 263 +++++++++++++++++++++++++++++++ 2 files changed, 263 insertions(+), 88 deletions(-) delete mode 100644 examples/aideck/crtp-cpx-wifi.py create mode 100644 examples/aideck/fpv.py diff --git a/examples/aideck/crtp-cpx-wifi.py b/examples/aideck/crtp-cpx-wifi.py deleted file mode 100644 index 3580b3526..000000000 --- a/examples/aideck/crtp-cpx-wifi.py +++ /dev/null @@ -1,88 +0,0 @@ -""" -This example illustrates how to use CRTP over CPX while also using CPX -to access other targets on the Crazyflie. This is done by connecting to -the Crazyflie and reading out images from the WiFi streamer and naming -them according to the post of the Crazyflie. - -For the example to work you will need the WiFi example flashed on the -GAP8 and an up to date code-base on the STM32/ESP32. - -""" -import logging -import struct -import threading - -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 - -uri = uri_helper.uri_from_env() - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class Example(threading.Thread): - - def __init__(self, uri): - threading.Thread.__init__(self) - self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) - self._lg_stab.add_variable('stateEstimate.x', 'float') - self._lg_stab.add_variable('stateEstimate.y', 'float') - self._lg_stab.add_variable('stateEstimate.z', 'float') - self._lg_stab.add_variable('stabilizer.roll', 'float') - self._lg_stab.add_variable('stabilizer.pitch', 'float') - self._lg_stab.add_variable('stabilizer.yaw', 'float') - - self._cf = Crazyflie(rw_cache='./cache') - self._cf.connected.add_callback(self.connected) - self._cf.disconnected.add_callback(self.disconnected) - self._cf.open_link(uri) - self._cpx = None - - def connected(self, uri): - print('Connected to {}'.format(uri)) - self._cpx = self._cf.link.cpx - self.start() - try: - self._cf.log.add_config(self._lg_stab) - # This callback will receive the data - self._lg_stab.data_received_cb.add_callback(self._stab_log_data) - # This callback will be called on errors - self._lg_stab.error_cb.add_callback(self._stab_log_error) - # Start the logging - self._lg_stab.start() - except KeyError as e: - print('Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - print('Could not add Stabilizer log config, bad configuration.') - - def disconnected(self, uri): - print('Disconnected from {}'.format(uri)) - - 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): - print(f'[{timestamp}][{logconf.name}]: ', end='') - for name, value in data.items(): - print(f'{name}: {value:3.3f} ', end='') - print() - - def run(self): - while True: - p = self._cpx.receivePacket(CPXFunction.APP) - [magic, width, height, depth, format, size] = struct.unpack('. +""" +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 math +import sys +import threading +import traceback +import struct + +import numpy as np + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.utils import uri_helper + +from cflib.cpx import CPXPacket, CPXFunction, CPXTarget + +try: + from sip import setapi + setapi('QVariant', 2) + setapi('QString', 2) +except ImportError: + pass + +from PyQt5 import QtCore, QtWidgets, QtGui, Qt + +logging.basicConfig(level=logging.INFO) + +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + +CAM_HEIGHT = 244 +CAM_WIDTH = 324 + +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(' Date: Wed, 13 Jul 2022 13:53:47 +0200 Subject: [PATCH 453/861] Fixed Flake8 --- examples/aideck/fpv.py | 125 ++++++++++++++++++++++++----------------- 1 file changed, 75 insertions(+), 50 deletions(-) diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 97ccf4d08..8fa971bb6 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -45,21 +45,18 @@ * AI deck 1.1 """ import logging -import math +import struct import sys import threading -import traceback -import struct 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 -from cflib.cpx import CPXPacket, CPXFunction, CPXTarget - try: from sip import setapi setapi('QVariant', 2) @@ -67,7 +64,7 @@ except ImportError: pass -from PyQt5 import QtCore, QtWidgets, QtGui, Qt +from PyQt5 import QtCore, QtWidgets, QtGui logging.basicConfig(level=logging.INFO) @@ -75,30 +72,33 @@ 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(' Date: Wed, 13 Jul 2022 14:01:16 +0200 Subject: [PATCH 454/861] Removed comment, this isn't needed with the indended usage --- cflib/crtp/serialdriver.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index 4cf228e13..80ab394d0 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -106,8 +106,6 @@ def connect(self, uri, linkQualityCallback, linkErrorCallback): function=CPXFunction.SYSTEM, data=[0x20, 0x01])) - # TODO! These should be reset again! - def send_packet(self, pk): raw = (pk.header,) + struct.unpack('B' * len(pk.data), pk.data) self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, From 2537d5bffb39892df32b2046a6888c98f79a6499 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 18 Jul 2022 10:50:12 +0200 Subject: [PATCH 455/861] update api doc cflib commander --- cflib/crazyflie/commander.py | 35 +++++++++++++++++++++------------- docs/user-guides/python_api.md | 14 +++++--------- 2 files changed, 27 insertions(+), 22 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index d7d86f703..f165078cd 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -60,12 +60,16 @@ def set_client_xmode(self, enabled): """ self._x_mode = enabled - def send_setpoint(self, roll, pitch, yaw, thrust): - """ - Send a new control setpoint for roll/pitch/yaw/thrust to the copter - - The arguments roll/pitch/yaw/trust is the new setpoints that should - be sent to the copter + def send_setpoint(self, roll, pitch, yawrate, thrust): + """ + Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the copter. + + The meaning of these values is depended on the mode of the RPYT commander in the firmware + Default settings are Roll, pitch, yawrate and thrust + + roll, pitch are in degrees + yawrate is in degrees/s + thrust is an integer value ranging from 10001 (next to no power) to 60000 (full power) """ if thrust > 0xFFFF or thrust < 0: raise ValueError('Thrust must be between 0 and 0xFFFF') @@ -89,7 +93,7 @@ def send_stop_setpoint(self): def send_velocity_world_setpoint(self, vx, vy, vz, yawrate): """ - Send Velocity in the world frame of reference setpoint. + Send Velocity in the world frame of reference setpoint with yawrate commands vx, vy, vz are in m/s yawrate is in degrees/s @@ -103,9 +107,12 @@ def send_velocity_world_setpoint(self, vx, vy, vz, yawrate): def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance): """ Control mode where the height is send as an absolute setpoint (intended - to be the distance to the surface under the Crazflie). + to be the distance to the surface under the Crazflie), while giving roll, + pitch and yaw rate commands - Roll, pitch, yawrate are defined as degrees, degrees, degrees/s + roll, pitch are in degrees + yawrate is in degrees/s + zdistance is in meters """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC @@ -116,10 +123,12 @@ def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance): def send_hover_setpoint(self, vx, vy, yawrate, zdistance): """ Control mode where the height is send as an absolute setpoint (intended - to be the distance to the surface under the Crazflie). + to be the distance to the surface under the Crazflie), while giving x, y velocity + commands in body-fixed coordinates. - vx and vy are in m/s + vx, vy are in m/s yawrate is in degrees/s + zdistance is in meters """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC @@ -129,10 +138,10 @@ def send_hover_setpoint(self, vx, vy, yawrate, zdistance): def send_position_setpoint(self, x, y, z, yaw): """ - Control mode where the position is sent as absolute x,y,z coordinate in + Control mode where the position is sent as absolute (world) x,y,z coordinate in meter and the yaw is the absolute orientation. - x and y are in m + x, y, z are in m yaw is in degrees """ pk = CRTPPacket() diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 9517e4916..1175f488b 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -1,5 +1,5 @@ --- -title: The Crazyflie Python API +title: The Crazyflie Python API explanation page_id: python_api --- @@ -14,6 +14,8 @@ If you are interested in more details look in the PyDoc in the code or: [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_log/) or [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_parameters/) +- [Automated documentation for Python API](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/) + ## Structure of the library The library is asynchronous and based on callbacks for events. Functions @@ -35,8 +37,8 @@ Currently only *radio* and *debug* interfaces are used but there\'s ideas for more like *udp*, *serial*, *usb*, etc\...Here are some examples: -- _radio://0/10/250K_ : Radio interface, USB dongle number 0, radio channel 10 and radio - speed 250 Kbit/s: radio://0/10/250K +- _radio://0/10/2M : Radio interface, USB dongle number 0, radio channel 10 and radio + speed 2 Mbit/s: radio://0/10/2M - _debug://0/1_ : Debug interface, id 0, channel 1 ### Variables and logging @@ -213,12 +215,6 @@ have a special API. ``` python send_setpoint(roll, pitch, yaw, thrust): - """ - Send a new control set-point for roll/pitch/yaw/thust to the copter - - The arguments roll/pitch/yaw/trust is the new set-points that should - be sent to the copter - """ ``` To send a new control set-point use the following: From ca7a24dede9ee9ef1f554fef0329491aea9103bb Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 18 Jul 2022 11:01:44 +0200 Subject: [PATCH 456/861] fix typo --- cflib/crazyflie/commander.py | 2 +- docs/user-guides/python_api.md | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index f165078cd..0e6d42743 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -79,7 +79,7 @@ def send_setpoint(self, roll, pitch, yawrate, thrust): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER - pk.data = struct.pack(' Date: Mon, 18 Jul 2022 12:05:56 +0200 Subject: [PATCH 457/861] fix flake8 --- cflib/crazyflie/commander.py | 4 ++-- docs/user-guides/python_api.md | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 0e6d42743..c75128ee5 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -63,10 +63,10 @@ def set_client_xmode(self, enabled): def send_setpoint(self, roll, pitch, yawrate, thrust): """ Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the copter. - + The meaning of these values is depended on the mode of the RPYT commander in the firmware Default settings are Roll, pitch, yawrate and thrust - + roll, pitch are in degrees yawrate is in degrees/s thrust is an integer value ranging from 10001 (next to no power) to 60000 (full power) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index ae2aaa1f3..3c0e20374 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -528,4 +528,3 @@ to be used with a positioning system such as LPS, the lighthouse or a mocap syst # The Crazyflie lands when leaving this "with" section # When leaving this "with" section, the connection is automatically closed ``` - From b0366efe9e9a948df42edd4dddad97883194b028 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 19 Jul 2022 15:18:18 +0200 Subject: [PATCH 458/861] remove cpx from pdoc --- cflib/__init__.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/__init__.py b/cflib/__init__.py index 7d4db36c1..8c44f9d14 100644 --- a/cflib/__init__.py +++ b/cflib/__init__.py @@ -56,3 +56,4 @@ """ __pdoc__ = {} __pdoc__['cflib.crtp.cflinkcppdriver'] = False +__pdoc__['cflib.cpx.transports'] = False From 3c2269107d5ad5f8cd20359845edfc2c9f8565b6 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Mon, 25 Jul 2022 15:49:57 +0200 Subject: [PATCH 459/861] update serial uart documentation fix for PR #342 --- docs/development/uart_communication.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/development/uart_communication.md b/docs/development/uart_communication.md index d80772aa2..07193302c 100644 --- a/docs/development/uart_communication.md +++ b/docs/development/uart_communication.md @@ -16,7 +16,9 @@ If you are connecting to a Raspberry Pi look for the UART pins there connect the ## Crazyflie Firmware -Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the firmware has to be compiled with the `Expansion deck configuration->Support CRTP over UART2` and `Expansion deck configuration->crtpOverUART2` kbuild config enabled. +Typically the Crazyflie expects control commands from Radio or Bluetooth and also sends its feedback there. To change this to UART, the crazyflie-firmware has to be compiled with the following [kbuild configurations](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/development/kbuild/) with `make menuconfig` +* `Expansion deck configuration`-> `Force load specified custom deck driver`->fill in `cpxOverUART2` +* `Expansion deck configuration`->`Support wired external host using CPX on UART`-> include in build with `y` ## Controlling Device From 6481262a7864e56d9654b502d47021116d5fdf67 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 26 Jul 2022 11:53:06 +0200 Subject: [PATCH 460/861] Added logging for the test lab --- cflib/crazyflie/platformservice.py | 31 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index b4f2c43ae..0bd17a57d 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -58,15 +58,9 @@ def __init__(self, crazyflie=None): """ self._cf = crazyflie - self._cf.add_port_callback(CRTPPort.PLATFORM, - self._platform_callback) - self._cf.add_port_callback(CRTPPort.LINKCTRL, - self._crt_service_callback) - - # Request protocol version. - # The semaphore makes sure that other module will wait for the version - # to be received before using it. - self._has_protocol_version = False + self._cf.add_port_callback(CRTPPort.PLATFORM, self._platform_callback) + self._cf.add_port_callback(CRTPPort.LINKCTRL, self._crt_service_callback) + self._protocolVersion = -1 self._callback = None @@ -104,26 +98,29 @@ def _request_protocol_version(self): pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE) pk.data = (0,) self._cf.send_packet(pk) + logger.info("Request _request_protocol_version()") def _crt_service_callback(self, pk): if pk.channel == LINKSERVICE_SOURCE: + logger.info("_crt_service_callback") # If the sink contains a magic string, get the protocol version, # otherwise -1 if pk.data[:18].decode('utf8') == 'Bitcraze Crazyflie': pk = CRTPPacket() pk.set_header(CRTPPort.PLATFORM, VERSION_COMMAND) pk.data = (VERSION_GET_PROTOCOL, ) + logger.info("Request protocol version") self._cf.send_packet(pk) else: self._protocolVersion = -1 - logger.info('Protocol version: {}'.format( - self.get_protocol_version())) + logger.info('Protocol version (crt): {}'.format(self.get_protocol_version())) self._callback() def _platform_callback(self, pk): - if pk.channel == VERSION_COMMAND and \ - pk.data[0] == VERSION_GET_PROTOCOL: - self._protocolVersion = pk.data[1] - logger.info('Protocol version: {}'.format( - self.get_protocol_version())) - self._callback() + if pk.channel == VERSION_COMMAND : + logger.info("_platform_callback") + + if pk.data[0] == VERSION_GET_PROTOCOL: + self._protocolVersion = pk.data[1] + logger.info('Protocol version (platform): {}'.format(self.get_protocol_version())) + self._callback() From 4bf72d0421315f8d6e02a7164908ddca91df00fd Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 26 Jul 2022 12:30:45 +0200 Subject: [PATCH 461/861] Updated log levels --- cflib/crazyflie/platformservice.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index 0bd17a57d..f35bada88 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -98,18 +98,18 @@ def _request_protocol_version(self): pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE) pk.data = (0,) self._cf.send_packet(pk) - logger.info("Request _request_protocol_version()") + logger.debug("Request _request_protocol_version()") def _crt_service_callback(self, pk): if pk.channel == LINKSERVICE_SOURCE: - logger.info("_crt_service_callback") + logger.debug("_crt_service_callback") # If the sink contains a magic string, get the protocol version, # otherwise -1 if pk.data[:18].decode('utf8') == 'Bitcraze Crazyflie': pk = CRTPPacket() pk.set_header(CRTPPort.PLATFORM, VERSION_COMMAND) pk.data = (VERSION_GET_PROTOCOL, ) - logger.info("Request protocol version") + logger.debug("Request protocol version") self._cf.send_packet(pk) else: self._protocolVersion = -1 @@ -118,7 +118,7 @@ def _crt_service_callback(self, pk): def _platform_callback(self, pk): if pk.channel == VERSION_COMMAND : - logger.info("_platform_callback") + logger.debug("_platform_callback") if pk.data[0] == VERSION_GET_PROTOCOL: self._protocolVersion = pk.data[1] From c5bca7351ee37f30b1c0bc0cc3c5b5b2470db1ac Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 26 Jul 2022 12:51:22 +0200 Subject: [PATCH 462/861] Fixed flake8 and changed back to info level --- cflib/crazyflie/platformservice.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index f35bada88..e1c8d398c 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -98,18 +98,18 @@ def _request_protocol_version(self): pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE) pk.data = (0,) self._cf.send_packet(pk) - logger.debug("Request _request_protocol_version()") + logger.info('Request _request_protocol_version()') def _crt_service_callback(self, pk): if pk.channel == LINKSERVICE_SOURCE: - logger.debug("_crt_service_callback") + logger.info('_crt_service_callback') # If the sink contains a magic string, get the protocol version, # otherwise -1 if pk.data[:18].decode('utf8') == 'Bitcraze Crazyflie': pk = CRTPPacket() pk.set_header(CRTPPort.PLATFORM, VERSION_COMMAND) pk.data = (VERSION_GET_PROTOCOL, ) - logger.debug("Request protocol version") + logger.info('Request protocol version') self._cf.send_packet(pk) else: self._protocolVersion = -1 @@ -117,8 +117,8 @@ def _crt_service_callback(self, pk): self._callback() def _platform_callback(self, pk): - if pk.channel == VERSION_COMMAND : - logger.debug("_platform_callback") + if pk.channel == VERSION_COMMAND: + logger.info('_platform_callback') if pk.data[0] == VERSION_GET_PROTOCOL: self._protocolVersion = pk.data[1] From 1fe6bedf23703869c62d77a64df107c7bc44d613 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Mon, 8 Aug 2022 13:53:13 +0200 Subject: [PATCH 463/861] Step by step swarm --- docs/user-guides/sbs_swarm_interface.md | 475 ++++++++++++++++++++++++ examples/step-by-step/sbs_swarm.py | 172 +++++++++ 2 files changed, 647 insertions(+) create mode 100644 docs/user-guides/sbs_swarm_interface.md create mode 100644 examples/step-by-step/sbs_swarm.py diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md new file mode 100644 index 000000000..039640da1 --- /dev/null +++ b/docs/user-guides/sbs_swarm_interface.md @@ -0,0 +1,475 @@ +--- +title: "Step-by-Step: Swarm Interface" +page_id: sbs_swarm_interface +--- + +Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib.For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a positioning system (Flowdeck,Lighthouse,LPS etc.) that is able to provide data for the position estimation of the crazyflies. + +# Prerequisites + +We will assume that you already know this before you start with the tutorial: + +* Some basic experience with python +* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) +* Read the [high level commander](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/high_level_commander/), [swarm](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/swarm/) and [SyncCrazyflie](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/syncCrazyflie/) documentation . + + +# Get the script started + +Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up an new python script called `swarm_rectangle.py`. First you will start by adding the following import to the script: + +```python +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm + +uris = { + 'radio://0/20/2M/E7E7E7E701', + 'radio://0/20/2M/E7E7E7E702', + 'radio://0/20/2M/E7E7E7E703', + 'radio://0/20/2M/E7E7E7E704', + # Add more URIs if you want more copters in the swarm +} + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: +``` + +This will import all the necessary modules and open the necessary links for communication with all the Crazyflies of the swarms.`Swarm` is a wrapper class which facilitates the execution of functions given by the user for each copter and can execute them in parallel or sequentially.Each Crazyflie is treated as a `SyncCrazyflie` instance and as the first argument in swarm wide actions.There is no need to worry about threads since they are handled internally.To reduce connection time,the factory is chosen to be instance of the `CachedCfFactory` class that will cache the Crazyflie objects in the `./cache` directory . + +The radio addresses of the copters are defined in the `uris` list and you can add more if you want to. + +# Step 1: Light Check + +In order to verify everything is setup and working properly a light check will be performed.During this check,all the copters will light up red for a short period of time and then return to normal. +This is achieved by setting the parameter `led.bitmask` to 255 which results to all the LED's of each copter light up simultaneously. + +Add the helper functions `activateBitMask`,`deactivateBitMask` and the function`lightCheck` above `__main__`: +```python +def activateBitMask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivateBitMask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def lightCheck(scf): + activateBitMask(scf) + time.sleep(2) + deactivateBitMask(scf) +``` +`lightCheck` will light up a copter red for 2 seconds and then return them to normal. + +Below `... Swarm(...)` in `__main__`, execute the light check for each copter: + +```python + swarm.parallel_safe(light_check) +``` +The `lightCheck()` is going to be called through the `parallel_safe()` method which will execute it for for all Crazyflies in the swarm, in parallel. One thread per Crazyflie is started to execute the function. The threads are joined at the end and if one or more of the threads raised an exception this function will also raise an exception. + +```python +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm + + +def activate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + activate_bit_mask(scf) + time.sleep(2) + deactivate_bit_mask(scf) + time.sleep(2) + + +uris = { + 'radio://0/20/2M/E7E7E7E701', + 'radio://0/20/2M/E7E7E7E702', + # Add more URIs if you want more copters in the swarm +} + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + swarm.parallel_safe(light_check) + +``` +If everything is working properly, you can move to the next step . + +# Step 2: Security Before Flying +Before executing any take off and flight maneuvers, the copters need to make sure that they have a precise enough position estimation.Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. +```python +with Swarm(uris, factory=factory) as swarm: + swarm.parallel_safe(lightCheck) + swarm.reset_estimators() +``` + +# Step 3: Taking off and Landing Sequentially +Now we are going to execute the fist take off and landing using the high level commander.The high level commander is a class that handles all the high level commands like takeoff,landing,hover,go to position, and others.You first need to enable it through the `commander.enHighLevel` parameter and then execute the take off and land commands through the below functions: +```python +def activate_high_level_commander(scf): + scf.cf.param.set_value('commander.enHighLevel', '1') + +def take_off(scf): + commander= scf.cf.high_level_commander + + commander.takeoff(1.0, 2.0) + time.sleep(3) + +def land(scf): + commander= scf.cf.high_level_commander + + commander.land(0.0, 2.0) + time.sleep(2) + + commander.stop() + +def hover_sequence(scf): + take_off(scf) + land(scf) +``` + +Notice that after landing, the high level commander is disabled again. + +Initially , we want only one copter at a time executing the hover_sequence so it is going to be called through the `sequential()` method of the `Swarm` in the following way: + +```python +swarm.parallel_safe(activate_high_level_commander) +swarm.sequential(hover_sequence) +``` +Leading to the following code: + +```python +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm + + +def activate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + activateBitMask(scf) + time.sleep(2) + deactivateBitMask(scf) + time.sleep(2) + +def activate_high_level_commander(scf): + scf.cf.param.set_value('commander.enHighLevel', '1') + +def take_off(scf): + commander= scf.cf.high_level_commander + + commander.takeoff(1.0, 2.0) + time.sleep(3) + +def land(scf): + commander= scf.cf.high_level_commander + + commander.land(0.0, 2.0) + time.sleep(2) + + commander.stop() + +def hover_sequence(scf): + take_off(scf) + land(scf) + +uris = { + 'radio://0/20/2M/E7E7E7E701', + 'radio://0/20/2M/E7E7E7E702', + 'radio://0/20/2M/E7E7E7E703', + 'radio://0/20/2M/E7E7E7E704', + # Add more URIs if you want more copters in the swarm +} + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + print('Connected to Crazyflies') + swarm.parallel_safe(lightCheck) + swarm.reset_estimators() + + swarm.parallel_safe(activate_high_level_commander) + swarm.sequential(hover_sequence) +``` +After executing it you will see all copters performing the light check and then each copter take off ,hover and land.This process is repeated for all copters in the swarm. + +# Step 4: Taking off and Landing in Sync +If you want to take off and land in sync, you can use the `parallel_safe()` method of the `Swarm` class. + +```python + swarm.parallel_safe(hover_sequence) +``` + +Now the same action is happening but for all the copters in parallel. + +# Step 5: Performing a square shape flight +To make the swarm fly in a square shape, we will use the go_to method of the high level commander. Each copter executes 4 relative movements to its current position covering a square shape. + +```python +def run_square_sequence(scf): + box_size = 1 + flight_time = 2 + + commander= scf.cf.high_level_commander + + 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) +``` +Keep in mind that the `go_to()` command does not block the code so you have to wait as long as the flight time of each movement to continue to the next one. + +Since we want these maneuvers to be synchronized, the `parallel_safe()` method is chosen to execute the sequence, in similar fashion with the above steps. You can include the sequence execution in the main code of the swarm in the following way: + +```python + swarm.parallel_safe(take_off) + swarm.parallel_safe(run_square_sequence) + swarm.parallel_safe(land) +``` +Make sure that your script looks similar to the following and execute it: + +```python +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm + + +def activate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + ... + +def activate_high_level_commander(scf): + scf.cf.param.set_value('commander.enHighLevel', '1') + +def take_off(scf): + ... + +def land(scf): + ... + +def run_square_sequence(scf: SyncCrazyflie): + ... + +uris = { + 'radio://0/20/2M/E7E7E7E701', + 'radio://0/20/2M/E7E7E7E702', + 'radio://0/20/2M/E7E7E7E703', + 'radio://0/20/2M/E7E7E7E704', + # Add more URIs if you want more copters in the swarm +} + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + print('Connected to Crazyflies') + swarm.parallel_safe(light_check) + swarm.reset_estimators() + + swarm.parallel_safe(activate_high_level_commander) + + swarm.parallel_safe(take_off) + swarm.parallel_safe(run_square_sequence) + swarm.parallel_safe(land) +``` + +# Step 6: Performing a flight with different arguments +You can also feed different arguments to each Crazyflie in the swarm. This can be done by providing a dictionary `args_dict` to the `parallel_safe()`,`parallel()` and `sequential()` methods following the below format. + +```python +args_dict = { + URI0: [optional_param0_cf0, optional_param1_cf0], + URI1: [optional_param0_cf1, optional_param1_cf1], + ... +} +``` + +where the key is the radio address of the copter and the value is a list of optional arguments.In this way you can differentiate the behavior of each copter and execute different actions based on the copter and its particular parameters. + + +In this example, the copters will be placed in a square shape as shown below (pay attention to the order of the Crazyflies) and each one of them will execute different relative movements. + + +```python +# The layout of the positions (1m apart from each other): +# <------ 1 m -----> +# 0 1 +# ^ ^ +# |Y | +# | | +# +------> X 1 m +# | +# | +# 3 2 . + + +h = 0.0 # remain constant height similar to take off height +x0, y0 = +1.0, +1.0 +x1, y1 = -1.0, -1.0 + +# x y z time +sequence0 = [ + (x1, y0, h, 3.0), + (x0, y1, h, 3.0), + (x0, 0, h, 3.0), +] + +sequence1 = [ + (x0, y0, h, 3.0), + (x1, y1, h, 3.0), + (.0, y1, h, 3.0), +] + +sequence2 = [ + (x0, y1, h, 3.0), + (x1, y0, h, 3.0), + (x1, 0, h, 3.0), +] + +sequence3 = [ + (x1, y1, h, 3.0), + (x0, y0, h, 3.0), + (.0, y0, h, 3.0), +] + +seq_args = { + uris[0]: [sequence0], + uris[1]: [sequence1], + uris[2]: [sequence2], + uris[3]: [sequence3], +} + +def run_sequence(scf: SyncCrazyflie, sequence): + cf = scf.cf + + for arguments in sequence: + commander = scf.cf.high_level_commander + + x, y, z = arguments[0], arguments[1], arguments[2] + duration = arguments[3] + + print('Setting position {} to cf {}'.format((x, y, z), cf.link_uri)) + commander.go_to(x, y, z, 0, duration, relative=True) + time.sleep(duration) +``` + +And in the main code of the swarm, you can execute the sequence as follows: + +```python +swarm.parallel_safe(run_sequence, args_dict=seq_args) +``` + +The final script is going to look like this : + +```python +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie import syncCrazyflie + + +def activate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + ... + +def activate_high_level_commander(scf): + scf.cf.param.set_value('commander.enHighLevel', '1') + +def take_off(scf): + ... + +def land(scf): + ... + + +def run_square_sequence(scf): + ... + +uris = ... + +# The layout of the positions: +# ^ Y +# 0 | 1 +# | +# +------> X +# +# 3 2 + +h = 0.0 +x0, y0 = +0.5, +0.5 +x1, y1 = -0.5, -0.5 + +# x y z time +sequence0 = ... + +sequence1 = ... + +sequence2 = ... + +sequence3 = ... + +seq_args = ... + +def run_sequence(scf: syncCrazyflie.SyncCrazyflie, sequence): + ... + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + print('Connected to Crazyflies') + swarm.parallel_safe(light_check) + + swarm.reset_estimators() + swarm.parallel_safe(activate_high_level_commander) + + swarm.parallel_safe(take_off) + swarm.parallel_safe(run_square_sequence) + swarm.parallel_safe(run_sequence, args_dict=seq_args) + swarm.parallel_safe(land) +``` + +You’re done! The full code of this tutorial can be found in the example/step-by-step/ folder. + +# What is next ? +Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality,simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications!.For more examples and inspiration on the Swarm functionality, you can check out the `examples/swarm/` folder of the cflib. diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py new file mode 100644 index 000000000..d7f45dd5e --- /dev/null +++ b/examples/step-by-step/sbs_swarm.py @@ -0,0 +1,172 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2022 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 . +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + + +def activate_bit_mask(scf: SyncCrazyflie): + scf.cf.param.set_value('led.bitmask', 255) + + +def deactivate_bit_mask(scf: SyncCrazyflie): + scf.cf.param.set_value('led.bitmask', 0) + + +def light_check(scf: SyncCrazyflie): + activate_bit_mask(scf) + time.sleep(2) + deactivate_bit_mask(scf) + time.sleep(2) + + +def activate_high_level_commander(scf: SyncCrazyflie): + scf.cf.param.set_value('commander.enHighLevel', '1') + + +def take_off(scf: SyncCrazyflie): + commander = scf.cf.high_level_commander + + commander.takeoff(1.0, 2.0) + time.sleep(3) + + +def land(scf: SyncCrazyflie): + commander = scf.cf.high_level_commander + + commander.land(0.0, 2.0) + time.sleep(2) + + commander.stop() + + +def run_square_sequence(scf: SyncCrazyflie): + box_size = 1.0 + flight_time = 3.0 + + commander = scf.cf.high_level_commander + + 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) + + +uris = [ + 'radio://0/20/2M/E7E7E7E701', + 'radio://0/20/2M/E7E7E7E702', + 'radio://0/20/2M/E7E7E7E703', + 'radio://0/20/2M/E7E7E7E704', + # Add more URIs if you want more copters in the swarm +] + +# The layout of the positions (1m apart from each other): +# <------ 1 m -----> +# 0 1 +# ^ ^ +# |Y | +# | | +# +------> X 1 m +# | +# | +# 3 2 . + + +h = 0.0 # remain constant height similar to take off height +x0, y0 = +0.5, +0.5 +x1, y1 = -0.5, -0.5 + +# x y z time +sequence0 = [ + (x1, y0, h, 3.0), + (x0, y1, h, 3.0), + (x0, 0, h, 3.0), +] + +sequence1 = [ + (x0, y0, h, 3.0), + (x1, y1, h, 3.0), + (.0, y1, h, 3.0), +] + +sequence2 = [ + (x0, y1, h, 3.0), + (x1, y0, h, 3.0), + (x1, 0, h, 3.0), +] + +sequence3 = [ + (x1, y1, h, 3.0), + (x0, y0, h, 3.0), + (.0, y0, h, 3.0), +] + +seq_args = { + uris[0]: [sequence0], + uris[1]: [sequence1], + # uris[2]: [sequence2], + # uris[3]: [sequence3], +} + + +def run_sequence(scf: SyncCrazyflie, sequence): + cf = scf.cf + + for arguments in sequence: + commander = scf.cf.high_level_commander + + x, y, z = arguments[0], arguments[1], arguments[2] + duration = arguments[3] + + print('Setting position {} to cf {}'.format((x, y, z), cf.link_uri)) + commander.go_to(x, y, z, 0, duration, relative=True) + time.sleep(duration) + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + print('Connected to Crazyflies') + swarm.parallel_safe(light_check) + print('Light check done') + + swarm.reset_estimators() + print('Estimators have been reset') + swarm.parallel_safe(activate_high_level_commander) + + swarm.parallel_safe(take_off) + # swarm.parallel_safe(run_square_sequence) + swarm.parallel_safe(run_sequence, args_dict=seq_args) + swarm.parallel_safe(land) From fd5d5422324c93918a091178af946316fce62eb4 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Mon, 8 Aug 2022 13:58:41 +0200 Subject: [PATCH 464/861] Fixing wrong uris number --- docs/user-guides/sbs_swarm_interface.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index 039640da1..83636deac 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -94,6 +94,8 @@ def light_check(scf): uris = { 'radio://0/20/2M/E7E7E7E701', 'radio://0/20/2M/E7E7E7E702', + 'radio://0/20/2M/E7E7E7E703', + 'radio://0/20/2M/E7E7E7E704', # Add more URIs if you want more copters in the swarm } From a8cb732b0fe266f66ad9db087d1544c65053e417 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Mon, 8 Aug 2022 14:02:26 +0200 Subject: [PATCH 465/861] fix typo --- docs/user-guides/sbs_swarm_interface.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index 83636deac..ff8f2135b 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -474,4 +474,4 @@ if __name__ == '__main__': You’re done! The full code of this tutorial can be found in the example/step-by-step/ folder. # What is next ? -Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality,simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications!.For more examples and inspiration on the Swarm functionality, you can check out the `examples/swarm/` folder of the cflib. +Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality,simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications! For more examples and inspiration on the Swarm functionality, you can check out the `examples/swarm/` folder of the cflib. From a1c47b9b05e146c324a0736ca4b7ae66a694acb3 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Tue, 9 Aug 2022 10:26:08 +0200 Subject: [PATCH 466/861] Fixed more typos --- docs/user-guides/sbs_swarm_interface.md | 76 +++++++++++++------------ examples/step-by-step/sbs_swarm.py | 12 ++-- 2 files changed, 46 insertions(+), 42 deletions(-) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index ff8f2135b..b91cf2be1 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -3,7 +3,7 @@ title: "Step-by-Step: Swarm Interface" page_id: sbs_swarm_interface --- -Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib.For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a positioning system (Flowdeck,Lighthouse,LPS etc.) that is able to provide data for the position estimation of the crazyflies. +Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib.For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a global positioning system (Lighthouse,LPS or MoCap) that is able to provide data for the position estimation of the crazyflies. You can also use the Flowdeck but keep in mind that you should command relative movements of each Crazyflie and due to its nature it may lead to accumulative errors and unexpected behavior over time. # Prerequisites @@ -16,7 +16,7 @@ We will assume that you already know this before you start with the tutorial: # Get the script started -Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up an new python script called `swarm_rectangle.py`. First you will start by adding the following import to the script: +Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up a new python script called `swarm_rectangle.py`. First you will start by adding the following import to the script: ```python import time @@ -39,36 +39,36 @@ if __name__ == '__main__': with Swarm(uris, factory=factory) as swarm: ``` -This will import all the necessary modules and open the necessary links for communication with all the Crazyflies of the swarms.`Swarm` is a wrapper class which facilitates the execution of functions given by the user for each copter and can execute them in parallel or sequentially.Each Crazyflie is treated as a `SyncCrazyflie` instance and as the first argument in swarm wide actions.There is no need to worry about threads since they are handled internally.To reduce connection time,the factory is chosen to be instance of the `CachedCfFactory` class that will cache the Crazyflie objects in the `./cache` directory . +This will import all the necessary modules and open the necessary links for communication with all the Crazyflies of the swarms. `Swarm` is a wrapper class which facilitates the execution of functions given by the user for each copter and can execute them in parallel or sequentially. Each Crazyflie is treated as a `SyncCrazyflie` instance and as the first argument in swarm wide actions. There is no need to worry about threads since they are handled internally. To reduce connection time,the factory is chosen to be instance of the `CachedCfFactory` class that will cache the Crazyflie objects in the `./cache` directory. The radio addresses of the copters are defined in the `uris` list and you can add more if you want to. # Step 1: Light Check -In order to verify everything is setup and working properly a light check will be performed.During this check,all the copters will light up red for a short period of time and then return to normal. +In order to verify everything is setup and working properly a light check will be performed. During this check, all the copters will light up red for a short period of time and then return to normal. This is achieved by setting the parameter `led.bitmask` to 255 which results to all the LED's of each copter light up simultaneously. -Add the helper functions `activateBitMask`,`deactivateBitMask` and the function`lightCheck` above `__main__`: +Add the helper functions `activate_led_bit_mask`,`deactivate_led_bit_mask` and the function`light_check` above `__main__`: ```python -def activateBitMask(scf): +def activate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 255) -def deactivateBitMask(scf): +def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) -def lightCheck(scf): +def light_check(scf): activateBitMask(scf) time.sleep(2) deactivateBitMask(scf) ``` -`lightCheck` will light up a copter red for 2 seconds and then return them to normal. +`light_check` will light up a copter red for 2 seconds and then return them to normal. Below `... Swarm(...)` in `__main__`, execute the light check for each copter: ```python swarm.parallel_safe(light_check) ``` -The `lightCheck()` is going to be called through the `parallel_safe()` method which will execute it for for all Crazyflies in the swarm, in parallel. One thread per Crazyflie is started to execute the function. The threads are joined at the end and if one or more of the threads raised an exception this function will also raise an exception. +The `light_check()` is going to be called through the `parallel_safe()` method which will execute it for for all Crazyflies in the swarm, in parallel. One thread per Crazyflie is started to execute the function. The threads are joined at the end and if one or more of the threads raised an exception this function will also raise an exception. ```python import time @@ -78,16 +78,16 @@ from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm -def activate_bit_mask(scf): +def activate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 255) -def deactivate_bit_mask(scf): +def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) def light_check(scf): - activate_bit_mask(scf) + activate_led_bit_mask(scf) time.sleep(2) - deactivate_bit_mask(scf) + deactivate_led_bit_mask(scf) time.sleep(2) @@ -109,7 +109,7 @@ if __name__ == '__main__': If everything is working properly, you can move to the next step . # Step 2: Security Before Flying -Before executing any take off and flight maneuvers, the copters need to make sure that they have a precise enough position estimation.Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. +Before executing any take off and flight maneuvers, the copters need to make sure that they have a precise enough position estimation. Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. ```python with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(lightCheck) @@ -117,7 +117,7 @@ with Swarm(uris, factory=factory) as swarm: ``` # Step 3: Taking off and Landing Sequentially -Now we are going to execute the fist take off and landing using the high level commander.The high level commander is a class that handles all the high level commands like takeoff,landing,hover,go to position, and others.You first need to enable it through the `commander.enHighLevel` parameter and then execute the take off and land commands through the below functions: +Now we are going to execute the fist take off and landing using the high level commander. The high level commander is a class that handles all the high level commands like takeoff, landing, hover, go to position and others. You first need to enable it through the `commander.enHighLevel` parameter and then execute the take off and land commands through the below functions: ```python def activate_high_level_commander(scf): scf.cf.param.set_value('commander.enHighLevel', '1') @@ -159,10 +159,10 @@ from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm -def activate_bit_mask(scf): +def activate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 255) -def deactivate_bit_mask(scf): +def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) def light_check(scf): @@ -211,7 +211,7 @@ if __name__ == '__main__': swarm.parallel_safe(activate_high_level_commander) swarm.sequential(hover_sequence) ``` -After executing it you will see all copters performing the light check and then each copter take off ,hover and land.This process is repeated for all copters in the swarm. +After executing it you will see all copters performing the light check and then each copter take off , hover and land. This process is repeated for all copters in the swarm. # Step 4: Taking off and Landing in Sync If you want to take off and land in sync, you can use the `parallel_safe()` method of the `Swarm` class. @@ -263,10 +263,10 @@ from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm -def activate_bit_mask(scf): +def activate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 255) -def deactivate_bit_mask(scf): +def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) def light_check(scf): @@ -318,7 +318,7 @@ args_dict = { } ``` -where the key is the radio address of the copter and the value is a list of optional arguments.In this way you can differentiate the behavior of each copter and execute different actions based on the copter and its particular parameters. +where the key is the radio address of the copter and the value is a list of optional arguments. In this way you can differentiate the behavior of each copter and execute different actions based on the copter and its particular parameters. In this example, the copters will be placed in a square shape as shown below (pay attention to the order of the Crazyflies) and each one of them will execute different relative movements. @@ -404,10 +404,10 @@ from cflib.crazyflie.swarm import Swarm from cflib.crazyflie import syncCrazyflie -def activate_bit_mask(scf): +def activate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 255) -def deactivate_bit_mask(scf): +def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) def light_check(scf): @@ -428,17 +428,21 @@ def run_square_sequence(scf): uris = ... -# The layout of the positions: -# ^ Y -# 0 | 1 -# | -# +------> X -# -# 3 2 +# The layout of the positions (1m apart from each other): +# <------ 1 m -----> +# 0 1 +# ^ ^ +# |Y | +# | | +# +------> X 1 m +# | +# | +# 3 2 . + -h = 0.0 -x0, y0 = +0.5, +0.5 -x1, y1 = -0.5, -0.5 +h = 0.0 # remain constant height similar to take off height +x0, y0 = +1.0, +1.0 +x1, y1 = -1.0, -1.0 # x y z time sequence0 = ... @@ -471,7 +475,7 @@ if __name__ == '__main__': swarm.parallel_safe(land) ``` -You’re done! The full code of this tutorial can be found in the example/step-by-step/ folder. +You’re done! The full code of this tutorial can be found in the `example/step-by-step/` folder. # What is next ? -Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality,simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications! For more examples and inspiration on the Swarm functionality, you can check out the `examples/swarm/` folder of the cflib. +Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality, simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications! For more examples and inspiration on the Swarm functionality, you can check out the `examples/swarm/` folder of the cflib. diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py index d7f45dd5e..eb90f55ba 100644 --- a/examples/step-by-step/sbs_swarm.py +++ b/examples/step-by-step/sbs_swarm.py @@ -29,18 +29,18 @@ from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -def activate_bit_mask(scf: SyncCrazyflie): +def activate_led_bit_mask(scf: SyncCrazyflie): scf.cf.param.set_value('led.bitmask', 255) -def deactivate_bit_mask(scf: SyncCrazyflie): +def deactivate_led_bit_mask(scf: SyncCrazyflie): scf.cf.param.set_value('led.bitmask', 0) def light_check(scf: SyncCrazyflie): - activate_bit_mask(scf) + activate_led_bit_mask(scf) time.sleep(2) - deactivate_bit_mask(scf) + deactivate_led_bit_mask(scf) time.sleep(2) @@ -135,8 +135,8 @@ def run_square_sequence(scf: SyncCrazyflie): seq_args = { uris[0]: [sequence0], uris[1]: [sequence1], - # uris[2]: [sequence2], - # uris[3]: [sequence3], + uris[2]: [sequence2], + uris[3]: [sequence3], } From 3dd9de28e1a3f9d0473d1d4298075b80f5556fd9 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 10 Aug 2022 10:34:43 +0200 Subject: [PATCH 467/861] remove opencv from cflib --- cflib/localization/lighthouse_bs_geo.py | 13 +++++++++++-- setup.py | 1 - 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index 8771063d2..e61f1095d 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -24,9 +24,14 @@ """ import math -import cv2 as cv import numpy as np - +import pkg_resources +installed = {pkg.key for pkg in pkg_resources.working_set} +if {'opencv-python-headless'} - installed: + OPENCV_INSTALLED = False +else: + import cv2 as cv + OPENCV_INSTALLED = True class LighthouseBsGeoEstimator: """ @@ -35,6 +40,10 @@ class LighthouseBsGeoEstimator: """ def __init__(self): + if OPENCV_INSTALLED is False: + raise Exception('OpenCV is not installed. To use this function,' + + 'do "pip3 install opencv-python-headless"') + self._directions = { self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), self._hash_sensor_order([2, 0, 3, 1]): math.radians(25), diff --git a/setup.py b/setup.py index 371705310..eaf44980f 100644 --- a/setup.py +++ b/setup.py @@ -37,7 +37,6 @@ 'libusb-package~=1.0', 'scipy~=1.7', 'numpy>=1.20,<1.25', - 'opencv-python-headless~=4.5.5' ], # $ pip install -e .[dev] From 3dfc552ff7387f53cc3f42b4175b27b48ca456f1 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 11 Aug 2022 10:14:35 +0200 Subject: [PATCH 468/861] flake8 issue fix --- cflib/localization/lighthouse_bs_geo.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index e61f1095d..2fdea329e 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -33,6 +33,7 @@ import cv2 as cv OPENCV_INSTALLED = True + class LighthouseBsGeoEstimator: """ This class is used to estimate the geometry (position and attitude) @@ -41,7 +42,7 @@ class LighthouseBsGeoEstimator: def __init__(self): if OPENCV_INSTALLED is False: - raise Exception('OpenCV is not installed. To use this function,' + + raise Exception('OpenCV is not installed. To use this function,' + 'do "pip3 install opencv-python-headless"') self._directions = { From 76b454d3327caf9abd2d1c417e9ed33bfff13d76 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 11 Aug 2022 10:21:11 +0200 Subject: [PATCH 469/861] Fixed pep8 failing --- cflib/bootloader/__init__.py | 2 +- cflib/cpx/__init__.py | 2 +- cflib/crtp/radiodriver.py | 10 +++++----- cflib/localization/_ippe.py | 6 +++--- examples/motors/multiramp.py | 2 +- examples/qualisys/qualisys_hl_commander.py | 2 +- sys_test/single_cf_grounded/test_link.py | 2 +- test/crazyflie/test_syncLogger.py | 2 +- 8 files changed, 14 insertions(+), 14 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index cf6823656..0a73b0778 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -144,7 +144,7 @@ def flash(self, filename: str, targets: List[Target], cf=None): content = open(filename, 'br').read() artifacts = [FlashArtifact(content, targets[0])] else: - raise(Exception('Cannot flash a .bin to more than one target!')) + raise (Exception('Cannot flash a .bin to more than one target!')) # Separate artifacts for flash and decks flash_artifacts = [a for a in artifacts if a.target.platform == platform] diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index a78158796..cda4ec1fd 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -134,7 +134,7 @@ def transport(self): return self._transport def run(self): - while(self._connected): + while (self._connected): # Read one packet from the transport # Packages might have been split up along the diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index a8e3f1707..db9ed9088 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -105,7 +105,7 @@ def set_data_rate(self, dr): self._datarate = dr def send_packet(self, data: List[int]) -> crazyradio._radio_ack: - assert(self._opened) + assert (self._opened) self._cmd_queue.put((self._instance_id, _RadioCommands.SEND_PACKET, (self._channel, @@ -116,13 +116,13 @@ def send_packet(self, data: List[int]) -> crazyradio._radio_ack: return ack def set_arc(self, arc): - assert(self._opened) + assert (self._opened) self._cmd_queue.put((self._instance_id, _RadioCommands.SET_ARC, arc)) def scan_selected(self, selected, packet): - assert(self._opened) + assert (self._opened) self._cmd_queue.put((self._instance_id, _RadioCommands.SCAN_SELECTED, (self._datarate, self._address, @@ -130,7 +130,7 @@ def scan_selected(self, selected, packet): return self._rsp_queue.get() def scan_channels(self, start: int, stop: int, packet: Iterable[int]): - assert(self._opened) + assert (self._opened) self._cmd_queue.put((self._instance_id, _RadioCommands.SCAN_CHANNELS, (self._datarate, self._address, @@ -138,7 +138,7 @@ def scan_channels(self, start: int, stop: int, packet: Iterable[int]): return self._rsp_queue.get() def close(self): - assert(self._opened) + assert (self._opened) self._cmd_queue.put((self._instance_id, _RadioCommands.STOP, None)) self._opened = False diff --git a/cflib/localization/_ippe.py b/cflib/localization/_ippe.py index 7ffd5b3ee..71ffc5137 100644 --- a/cflib/localization/_ippe.py +++ b/cflib/localization/_ippe.py @@ -58,9 +58,9 @@ def mat_run(U, Q, hEstMethod='DLT'): print('hEstMethod Error') # Inputs shape checking - assert(U.shape[0] == 2 or U.shape[0] == 3) - assert(U.shape[1] == Q.shape[1]) - assert(Q.shape[0] == 2) + assert (U.shape[0] == 2 or U.shape[0] == 3) + assert (U.shape[1] == Q.shape[1]) + assert (Q.shape[0] == 2) n = U.shape[1] modelDims = U.shape[0] diff --git a/examples/motors/multiramp.py b/examples/motors/multiramp.py index 404841e77..5e89bb1e8 100644 --- a/examples/motors/multiramp.py +++ b/examples/motors/multiramp.py @@ -110,5 +110,5 @@ def _ramp_motors(self): # 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): + while (le0.connected or le1.connected): time.sleep(0.1) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 3ec7d9ff1..3067beec5 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -93,7 +93,7 @@ def run(self): async def _life_cycle(self): await self._connect() - while(self._stay_open): + while (self._stay_open): await asyncio.sleep(1) await self._close() diff --git a/sys_test/single_cf_grounded/test_link.py b/sys_test/single_cf_grounded/test_link.py index da3cefc7d..c9b4ffc52 100644 --- a/sys_test/single_cf_grounded/test_link.py +++ b/sys_test/single_cf_grounded/test_link.py @@ -145,7 +145,7 @@ def error_cb(self, error): self.assertIsNone(None, error) def build_data(self, i, packet_size): - assert(packet_size % 4 == 0) + assert (packet_size % 4 == 0) repeats = packet_size // 4 return struct.pack('<' + 'I'*repeats, *[i]*repeats) diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index c791df7ad..43fd2b2cf 100644 --- a/test/crazyflie/test_syncLogger.py +++ b/test/crazyflie/test_syncLogger.py @@ -202,7 +202,7 @@ def test_connect_disconnect_with_context_management(self): # Fixture # Test - with(SyncLogger(self.cf_mock, self.log_config_mock)) as sut: + with (SyncLogger(self.cf_mock, self.log_config_mock)) as sut: # Assert self.assertTrue(sut.is_connected()) From a984e622dbb5b140b2cbf2e3f77a41b936d5130c Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 11 Aug 2022 11:35:43 +0200 Subject: [PATCH 470/861] Fixed oudatted high level commander enable --- docs/user-guides/sbs_swarm_interface.md | 25 +++---------------------- examples/step-by-step/sbs_swarm.py | 5 ----- 2 files changed, 3 insertions(+), 27 deletions(-) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index b91cf2be1..2e09121a7 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -51,7 +51,7 @@ This is achieved by setting the parameter `led.bitmask` to 255 which results to Add the helper functions `activate_led_bit_mask`,`deactivate_led_bit_mask` and the function`light_check` above `__main__`: ```python def activate_led_bit_mask(scf): - scf.cf.param.set_value('led.bitmask', 255) + scf.cf.param.set_value('led.bitmask', 255) def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) @@ -117,11 +117,8 @@ with Swarm(uris, factory=factory) as swarm: ``` # Step 3: Taking off and Landing Sequentially -Now we are going to execute the fist take off and landing using the high level commander. The high level commander is a class that handles all the high level commands like takeoff, landing, hover, go to position and others. You first need to enable it through the `commander.enHighLevel` parameter and then execute the take off and land commands through the below functions: -```python -def activate_high_level_commander(scf): - scf.cf.param.set_value('commander.enHighLevel', '1') - +Now we are going to execute the fist take off and landing using the high level commander. The high level commander (more information [here](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/sensor-to-control/commanders_setpoints/#high-level-commander)) is a class that handles all the high level commands like takeoff, landing, hover, go to position and others. The high level commander is usually preferred since it needs less communication and provides more autonomy on the Crazyflie. It is always on, but just in a lower priority so you just need to execute the take off and land commands through the below functions: +```python def take_off(scf): commander= scf.cf.high_level_commander @@ -141,12 +138,9 @@ def hover_sequence(scf): land(scf) ``` -Notice that after landing, the high level commander is disabled again. - Initially , we want only one copter at a time executing the hover_sequence so it is going to be called through the `sequential()` method of the `Swarm` in the following way: ```python -swarm.parallel_safe(activate_high_level_commander) swarm.sequential(hover_sequence) ``` Leading to the following code: @@ -171,9 +165,6 @@ def light_check(scf): deactivateBitMask(scf) time.sleep(2) -def activate_high_level_commander(scf): - scf.cf.param.set_value('commander.enHighLevel', '1') - def take_off(scf): commander= scf.cf.high_level_commander @@ -208,7 +199,6 @@ if __name__ == '__main__': swarm.parallel_safe(lightCheck) swarm.reset_estimators() - swarm.parallel_safe(activate_high_level_commander) swarm.sequential(hover_sequence) ``` After executing it you will see all copters performing the light check and then each copter take off , hover and land. This process is repeated for all copters in the swarm. @@ -272,9 +262,6 @@ def deactivate_led_bit_mask(scf): def light_check(scf): ... -def activate_high_level_commander(scf): - scf.cf.param.set_value('commander.enHighLevel', '1') - def take_off(scf): ... @@ -300,8 +287,6 @@ if __name__ == '__main__': swarm.parallel_safe(light_check) swarm.reset_estimators() - swarm.parallel_safe(activate_high_level_commander) - swarm.parallel_safe(take_off) swarm.parallel_safe(run_square_sequence) swarm.parallel_safe(land) @@ -413,9 +398,6 @@ def deactivate_led_bit_mask(scf): def light_check(scf): ... -def activate_high_level_commander(scf): - scf.cf.param.set_value('commander.enHighLevel', '1') - def take_off(scf): ... @@ -467,7 +449,6 @@ if __name__ == '__main__': swarm.parallel_safe(light_check) swarm.reset_estimators() - swarm.parallel_safe(activate_high_level_commander) swarm.parallel_safe(take_off) swarm.parallel_safe(run_square_sequence) diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py index eb90f55ba..7332950e9 100644 --- a/examples/step-by-step/sbs_swarm.py +++ b/examples/step-by-step/sbs_swarm.py @@ -44,10 +44,6 @@ def light_check(scf: SyncCrazyflie): time.sleep(2) -def activate_high_level_commander(scf: SyncCrazyflie): - scf.cf.param.set_value('commander.enHighLevel', '1') - - def take_off(scf: SyncCrazyflie): commander = scf.cf.high_level_commander @@ -164,7 +160,6 @@ def run_sequence(scf: SyncCrazyflie, sequence): swarm.reset_estimators() print('Estimators have been reset') - swarm.parallel_safe(activate_high_level_commander) swarm.parallel_safe(take_off) # swarm.parallel_safe(run_square_sequence) From 4a5601a47b3faac68bfcc7b445d058583f5d6a63 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 15 Aug 2022 11:33:33 +0200 Subject: [PATCH 471/861] exception when estimator is used, or else use var --- cflib/localization/lighthouse_bs_geo.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index 2fdea329e..5061a8bf1 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -39,11 +39,12 @@ class LighthouseBsGeoEstimator: This class is used to estimate the geometry (position and attitude) of a lighthouse base station, given angles measured using a lighthouse deck. """ - def __init__(self): + + self.lighthouse_bs_geo_estimator_available = True + if OPENCV_INSTALLED is False: - raise Exception('OpenCV is not installed. To use this function,' + - 'do "pip3 install opencv-python-headless"') + self.lighthouse_bs_geo_estimator_available = False self._directions = { self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), @@ -86,6 +87,9 @@ def __init__(self): # Sanity check maximum pos self._sanity_max_pos = 10 + def is_lighthouse_bs_geo_estimator_available(self): + return self.lighthouse_bs_geo_estimator_available + def estimate_geometry(self, bs_vectors): """ Estimate the full pose of a base station based on angles from the 4 sensors @@ -96,6 +100,12 @@ def estimate_geometry(self, bs_vectors): :return rot_bs_in_cf_coord: Rotation matrix of the BS in the CFs coordinate system :return pos_bs_in_cf_coord: Position vector of the BS in the CFs coordinate system """ + + if OPENCV_INSTALLED is False: + raise Exception('OpenCV is not installed. To use this function,' + + 'do "pip3 install opencv-python-headless"' + + ' and restart the cfclient') + guess_yaw = self._find_initial_yaw_guess(bs_vectors) rvec_guess, tvec_guess = self._convert_yaw_to_open_cv(guess_yaw) rw_ocv, tw_ocv = self._estimate_pose_by_pnp(bs_vectors, rvec_guess, tvec_guess) From 8c6b95a0bd638aacbd42429df7554e591f025652 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 15 Aug 2022 11:41:33 +0200 Subject: [PATCH 472/861] indicate that var is private --- cflib/localization/lighthouse_bs_geo.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index 5061a8bf1..f986f3adc 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -41,10 +41,9 @@ class LighthouseBsGeoEstimator: """ def __init__(self): - self.lighthouse_bs_geo_estimator_available = True - + self._lighthouse_bs_geo_estimator_available = True if OPENCV_INSTALLED is False: - self.lighthouse_bs_geo_estimator_available = False + self._lighthouse_bs_geo_estimator_available = False self._directions = { self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), @@ -88,7 +87,7 @@ def __init__(self): self._sanity_max_pos = 10 def is_lighthouse_bs_geo_estimator_available(self): - return self.lighthouse_bs_geo_estimator_available + return self._lighthouse_bs_geo_estimator_available def estimate_geometry(self, bs_vectors): """ From 6caca28b443fb5ba8815a60dcfbf49d298aeefc1 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 15 Aug 2022 11:58:18 +0200 Subject: [PATCH 473/861] flake8 --- cflib/localization/lighthouse_bs_geo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index f986f3adc..5752dfa90 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -104,7 +104,7 @@ def estimate_geometry(self, bs_vectors): raise Exception('OpenCV is not installed. To use this function,' + 'do "pip3 install opencv-python-headless"' + ' and restart the cfclient') - + guess_yaw = self._find_initial_yaw_guess(bs_vectors) rvec_guess, tvec_guess = self._convert_yaw_to_open_cv(guess_yaw) rw_ocv, tw_ocv = self._estimate_pose_by_pnp(bs_vectors, rvec_guess, tvec_guess) From dd45b2612599225d12afe909753b51866645fb10 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 15 Aug 2022 12:11:07 +0200 Subject: [PATCH 474/861] autopep8 --- cflib/localization/lighthouse_bs_geo.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index 5752dfa90..323046aab 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -39,6 +39,7 @@ class LighthouseBsGeoEstimator: This class is used to estimate the geometry (position and attitude) of a lighthouse base station, given angles measured using a lighthouse deck. """ + def __init__(self): self._lighthouse_bs_geo_estimator_available = True From 01b7361e44cf049eca12457cb7e145fe80d9d9cb Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 15 Aug 2022 13:41:42 +0200 Subject: [PATCH 475/861] simplify is_available for checking opencv cflib --- cflib/localization/lighthouse_bs_geo.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py index 323046aab..0041ee5a9 100644 --- a/cflib/localization/lighthouse_bs_geo.py +++ b/cflib/localization/lighthouse_bs_geo.py @@ -42,9 +42,9 @@ class LighthouseBsGeoEstimator: def __init__(self): - self._lighthouse_bs_geo_estimator_available = True + self._estimator_available = True if OPENCV_INSTALLED is False: - self._lighthouse_bs_geo_estimator_available = False + self._estimator_available = False self._directions = { self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), @@ -87,8 +87,8 @@ def __init__(self): # Sanity check maximum pos self._sanity_max_pos = 10 - def is_lighthouse_bs_geo_estimator_available(self): - return self._lighthouse_bs_geo_estimator_available + def is_available(self): + return self._estimator_available def estimate_geometry(self, bs_vectors): """ From 51eab0e23ce9c3d3ba4b66bbeef9225ea6dcae87 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 17 Aug 2022 16:11:15 +0200 Subject: [PATCH 476/861] Increase LH sample match time --- cflib/localization/lighthouse_sample_matcher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py index d16fb338e..a7bd06e19 100644 --- a/cflib/localization/lighthouse_sample_matcher.py +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -34,7 +34,7 @@ class LighthouseSampleMatcher: """ @classmethod - def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.010, + def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.020, min_nr_of_bs_in_match: int = 0) -> list[LhCfPoseSample]: """ Aggregate samples close in time into lists From a4ee2e131c0b932e947bcb65a68968eba0d56308 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 18 Aug 2022 13:59:45 +0200 Subject: [PATCH 477/861] Added outlier filter to LH geo estimation --- .../lighthouse_initial_estimator.py | 41 ++++++++++++++----- .../multi_bs_geometry_estimation.py | 8 ++-- 2 files changed, 35 insertions(+), 14 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 0468df666..5d784b448 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -37,8 +37,11 @@ class LighthouseInitialEstimator: calculations. """ + OUTLIER_DETECTION_ERROR = 0.5 + @classmethod - def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> LhBsCfPoses: + def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> tuple( + LhBsCfPoses, list[LhCfPoseSample]): """ Make a rough estimate of the poses of all base stations and CF poses found in the samples. @@ -47,14 +50,16 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A :param matched_samples: A list of samples with lighthouse angles. :param sensor_positions: An array with the sensor positions on the lighthouse deck (3D, CF ref frame) - :return: a + :return: an estimate of base station and Crazyflie poses, as well as a cleaned version of matched_samples where + outliers are removed. """ bs_positions = cls._find_solutions(matched_samples, sensor_positions) # bs_positions is a map from bs-id-pair to position, where the position is the position of the second # bs, as seen from the first bs (in the first bs ref frame). - bs_poses_ref_cfs = cls._angles_to_poses(matched_samples, sensor_positions, bs_positions) + bs_poses_ref_cfs, cleaned_matched_samples = cls._angles_to_poses( + matched_samples, sensor_positions, bs_positions) # Use the first CF pose as the global reference frame. The pose of the first base station (as estimated by ippe) # is used as the "true" position (reference) @@ -74,7 +79,7 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A # Now that we have estimated the base station poses, estimate the poses of the CF in all the samples cf_poses = cls._estimate_cf_poses(bs_poses_ref_cfs, bs_poses) - return LhBsCfPoses(bs_poses, cf_poses) + return LhBsCfPoses(bs_poses, cf_poses), cleaned_matched_samples @classmethod def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike @@ -146,7 +151,8 @@ def _add_solution_permutations(cls, solutions: dict[int, tuple[Pose, Pose]], @classmethod def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike, - bs_positions: dict[tuple(int, int), npt.NDArray]) -> list[dict[int, Pose]]: + bs_positions: dict[tuple(int, int), npt.NDArray]) -> tuple(list[dict[int, Pose]], + list[LhCfPoseSample]): """ Estimate the base station poses in the Crazyflie reference frames, for each sample. @@ -156,10 +162,13 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position :param matched_samples: List of samples :param sensor_positions: Positions of the sensors on the lighthouse deck (CF ref frame) :param bs_positions: Dictionary of base station positions (other base station ref frame) - :return: A list of dictionaries from base station to Pose of all base stations, for each sample + :return: A list of dictionaries from base station to Pose of all base stations, for each sample, as well as + a version of the matched_samples where outliers are removed """ result: list[dict[int, Pose]] = [] + cleaned_matched_samples: list[LhCfPoseSample] = [] + for sample in matched_samples: solutions: dict[int, tuple[Pose, Pose]] = {} for bs, angles in sample.angles_calibrated.items(): @@ -171,14 +180,24 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position poses: dict[int, Pose] = {} ids = sorted(solutions.keys()) first = ids[0] + for other in ids[1:]: pair = (first, other) expected = bs_positions[pair] - poses[first], poses[other] = cls._choose_solutions(solutions[first], solutions[other], expected) - result.append(poses) + firstPose, otherPose = cls._choose_solutions(solutions[first], solutions[other], expected) + if firstPose is not None: + poses[first] = firstPose + poses[other] = otherPose + else: + poses = None + break - return result + if poses is not None: + result.append(poses) + cleaned_matched_samples.append(sample) + + return result, cleaned_matched_samples @classmethod def _choose_solutions(cls, solutions_1: tuple[Pose, Pose], solutions_2: tuple[Pose, Pose], @@ -198,8 +217,8 @@ def _choose_solutions(cls, solutions_1: tuple[Pose, Pose], solutions_2: tuple[Po best1 = solution_1 best2 = solution_2 - # if min_dist > 0.5: - # print('large error:', min_dist) + if min_dist > cls.OUTLIER_DETECTION_ERROR: + return None, None return best1, best2 diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py index 69e8c3159..0de9c1b40 100644 --- a/examples/lighthouse/multi_bs_geometry_estimation.py +++ b/examples/lighthouse/multi_bs_geometry_estimation.py @@ -212,14 +212,16 @@ def estimate_geometry(origin: 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 = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + 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()) - print(f'{len(matched_samples)} samples will be used') - solution = LighthouseGeometrySolver.solve(initial_guess, matched_samples, LhDeck4SensorPositions.positions) + solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions) if not solution.success: print('Solution did not converge, it might not be good!') From b5be8ec09afb695b20fa1f7a3ea9230d01032d32 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 18 Aug 2022 15:23:59 +0200 Subject: [PATCH 478/861] Updated unit tests --- test/localization/test_lighthouse_geometry_solver.py | 10 ++++++---- test/localization/test_lighthouse_initial_estimator.py | 8 ++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py index 8b3c01240..ad2f2fd29 100644 --- a/test/localization/test_lighthouse_geometry_solver.py +++ b/test/localization/test_lighthouse_geometry_solver.py @@ -44,11 +44,12 @@ def test_that_two_bs_poses_in_one_sample_are_estimated(self): }), ] - initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, + LhDeck4SensorPositions.positions) # Test actual = LighthouseGeometrySolver.solve( - initial_guess, matched_samples, LhDeck4SensorPositions.positions) + initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions) # Assert bs_poses = actual.bs_poses @@ -77,11 +78,12 @@ def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self): }), ] - initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions) + initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, + LhDeck4SensorPositions.positions) # Test actual = LighthouseGeometrySolver.solve( - initial_guess, matched_samples, LhDeck4SensorPositions.positions) + initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions) # Assert bs_poses = actual.bs_poses diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py index 1b620293a..f0fd7cb8d 100644 --- a/test/localization/test_lighthouse_initial_estimator.py +++ b/test/localization/test_lighthouse_initial_estimator.py @@ -60,7 +60,7 @@ def test_that_two_bs_poses_in_same_sample_are_found(self): ] # Test - actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) # Assert self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3) @@ -89,7 +89,7 @@ def test_that_linked_bs_poses_in_multiple_samples_are_found(self): ] # Test - actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) # Assert self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3) @@ -120,7 +120,7 @@ def test_that_cf_poses_are_estimated(self): ] # Test - actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) # Assert self.assertPosesAlmostEqual(self.fixtures.CF_ORIGIN_POSE, actual.cf_poses[0], places=3) @@ -145,7 +145,7 @@ def test_that_the_global_ref_frame_is_used(self): ] # Test - actual = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) # Assert self.assertPosesAlmostEqual( From 8c83a7939a1ebff9e6aca7b41fadecbfae341e07 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 23 Aug 2022 10:07:05 +0200 Subject: [PATCH 479/861] Update menu.yml --- docs/_data/menu.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index 0138f49e3..e882b43b6 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -8,6 +8,7 @@ - {page_id: python_api} - {page_id: sbs_connect_log_param} - {page_id: sbs_motion_commander} + - {page_id: sbs_swarm_interface} - title: Functional areas subs: - {page_id: crazyradio_lib} From 5d241458fa1ce7da2412bc61ad5338146ac7739d Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 6 Sep 2022 12:57:24 +0200 Subject: [PATCH 480/861] update version number --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index eaf44980f..f9735cc03 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.19', + version='0.1.20', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From d7e879224d55325a1e3efc57861b544c59fd4aa2 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 6 Sep 2022 12:58:25 +0200 Subject: [PATCH 481/861] typo --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index f9735cc03..9e43050ef 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.20', + version='0.1.20', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From 6d9a969850dc9095a6fb13456c137eb5342f3576 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 6 Sep 2022 12:58:56 +0200 Subject: [PATCH 482/861] update version to 0.1.20 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 9e43050ef..f9735cc03 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.20', + version='0.1.20', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From f9d2df379d7442d03fec4461430eb716502551c4 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 6 Sep 2022 16:34:16 +0200 Subject: [PATCH 483/861] Setting rotation std dev --- examples/qualisys/qualisys_hl_commander.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 3067beec5..de6d6e4de 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -53,6 +53,10 @@ # True: send position and orientation; False: send position only send_full_pose = False +# 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 @@ -253,6 +257,9 @@ def reset_estimator(cf): # time.sleep(1) wait_for_position_estimator(cf) +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') @@ -316,6 +323,7 @@ def run_sequence(cf, trajectory_id, duration): 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_high_level_commander(cf) # activate_mellinger_controller(cf) From 012cb75d1b63f9664f661f6eb2df37ebad00bd1f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 6 Sep 2022 16:49:01 +0200 Subject: [PATCH 484/861] Use scipy for quaternion --- examples/qualisys/qualisys_hl_commander.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index de6d6e4de..ca8aba203 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -34,6 +34,7 @@ from threading import Thread import qtm +from scipy.spatial.transform import Rotation import cflib.crtp from cflib.crazyflie import Crazyflie @@ -51,7 +52,7 @@ rigid_body_name = 'cf' # True: send position and orientation; False: send position only -send_full_pose = False +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. @@ -235,16 +236,10 @@ def send_extpose_rot_matrix(cf, x, y, z, rot): rotaton matrix. This is going to be forwarded to the Crazyflie's position estimator. """ - qw = _sqrt(1 + rot[0][0] + rot[1][1] + rot[2][2]) / 2 - qx = _sqrt(1 + rot[0][0] - rot[1][1] - rot[2][2]) / 2 - qy = _sqrt(1 - rot[0][0] + rot[1][1] - rot[2][2]) / 2 - qz = _sqrt(1 - rot[0][0] - rot[1][1] + rot[2][2]) / 2 - - # Normalize the quaternion - ql = math.sqrt(qx ** 2 + qy ** 2 + qz ** 2 + qw ** 2) + quat = Rotation.from_matrix(rot).as_quat() if send_full_pose: - cf.extpos.send_extpose(x, y, z, qx / ql, qy / ql, qz / ql, qw / ql) + cf.extpos.send_extpose(x, y, z, quat[0], quat[1], quat[2], quat[3]) else: cf.extpos.send_extpos(x, y, z) @@ -257,6 +252,7 @@ def reset_estimator(cf): # time.sleep(1) wait_for_position_estimator(cf) + def adjust_orientation_sensitivity(cf): cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev) From f7ca7268871542f245a188c4624c5793a6c22b2b Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 11:05:28 +0200 Subject: [PATCH 485/861] Removed outdated requirements install --- docs/installation/install.md | 12 ++++++------ requirements-dev.txt | 6 ------ requirements.txt | 1 - 3 files changed, 6 insertions(+), 13 deletions(-) delete mode 100644 requirements-dev.txt delete mode 100644 requirements.txt diff --git a/docs/installation/install.md b/docs/installation/install.md index 2303ec6c2..30933d905 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -42,15 +42,15 @@ you can skip this section. * To deactivate the virtualenv when you are done using it `deactivate` -#### Install cflib dependencies -Install dependencies required by the lib: `pip install -r requirements.txt`. If you are planning on developing on the lib you should also run: `pip install -r requirements-dev.txt`. - -To verify the installation, connect the crazyflie and run an example: `python3 examples/logging/basiclog.py` - -### Pre commit hooks +### Pre commit hooks (Ubuntu) If you want some extra help with keeping to the mandated python coding style you can install hooks that verify your style at commit time. This is done by running: ``` +$ pip3 install pre-commit +``` +go to crazyflie-lib-python root folder and run +``` $ pre-commit install +$ pre-commit run --all-files ``` This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes and alert you if you need to change anything. diff --git a/requirements-dev.txt b/requirements-dev.txt deleted file mode 100644 index 774c6f9fb..000000000 --- a/requirements-dev.txt +++ /dev/null @@ -1,6 +0,0 @@ --e . - -coverage -mock -pre-commit -tox diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index 8ba626a4e..000000000 --- a/requirements.txt +++ /dev/null @@ -1 +0,0 @@ -pyusb>=1.0.0b2 From 1b6d096a659f531ee73e803802f6115b4bda8f95 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 11:21:53 +0200 Subject: [PATCH 486/861] update outdated URI overview --- docs/user-guides/python_api.md | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 3c0e20374..c36a965a0 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -34,13 +34,14 @@ a synchronous API by wrapping the asynchronous classes, see the All communication links are identified using an URI build up of the following: InterfaceType://InterfaceId/InterfaceChannel/InterfaceSpeed -Currently only *radio* and *debug* interfaces are used but there\'s -ideas for more like *udp*, *serial*, *usb*, etc\...Here are some -examples: +Currently we have *radio*, *serial*, *usb*, *debug*, *udp* interfaces are used. Here are some examples: - _radio://0/10/2M : Radio interface, USB dongle number 0, radio channel 10 and radio speed 2 Mbit/s: radio://0/10/2M - _debug://0/1_ : Debug interface, id 0, channel 1 +- _usb://0_ : USB cable to microusb port, id 0 +- _serial://ttyAMA0_ : Serial port, id ttyAMA0 +- _tcp://aideck-AABBCCDD.local:5000_ : TCP network connection, Name: aideck-AABBCCDD.local, port 5000 ### Variables and logging From 2f8a2125d42c07c086a90797367baa6496ae4489 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 13:59:56 +0200 Subject: [PATCH 487/861] updates to python api --- docs/user-guides/python_api.md | 36 ++++++++++++++++++++++++---------- 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index c36a965a0..0f575c7b4 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -77,12 +77,14 @@ The library supports reading and writing parameters at run-time to the firmware. This is intended to be used for data that is not continuously being changed by the firmware, like setting regulation parameters and reading out if the power-on self-tests passed. Parameters should only -change in the firmware when being set from the host or during start-up. +change in the firmware when being set from the host (cfclient or a cflib script) or during start-up. + The library doesn\'t continuously update the parameter values, this should only be done once after connecting. After each write to a parameter the firmware will send back the updated value and this will be -forwarded to callbacks registered for reading this parameter. The -parameters should be used in the following way: +forwarded to callbacks registered for reading this parameter. + +The parameters should be used in the following way: - Register parameter updated callbacks at any time in your application - Connect to your Crazyflie (this will download the parameter TOC) @@ -93,6 +95,8 @@ parameters should be used in the following way: - For each write all the callbacks registered for this parameter will be called back +There is an exception for experimental support to change the parameter from within [firmware's app layer](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/app_layer/#internal-log-and-param-system). However do mind that this functionality is not according to the design of the parameters framework so that the host might not be updated correctly on the parameter change. + ### Variable and parameter names All names of parameters and log variables use the same structure: @@ -201,7 +205,7 @@ object: ``` python crazyflie = Crazyflie() crazyflie.connected.add_callback(crazyflie_connected) - crazyflie.open_link("radio://0/10/250K") + crazyflie.open_link("radio://0/10/2M") ``` Then you can use the following to close the link again: @@ -210,11 +214,13 @@ Then you can use the following to close the link again: crazyflie.close_link() ``` -## Sending control setpoints +## Sending control setpoints with the commander framework The control setpoints are not implemented as parameters, instead they have a special API. +### Attitude Setpoints + ``` python def send_setpoint(self, roll, pitch, yawrate, thrust): ``` @@ -247,6 +253,16 @@ one command with thrust = 0 in order to unlock the command. This unlock procedure needs to be repeated if the watchdog describe above kicks-in. +### Other commander setpoints sending + +If your Crazyflie has a positioning system (LPS, flowdeck, MoCap, Lighthouse), you can also send velocity or position setpoints, like for instance: + +``` +send_hover_setpoint(self, vx, vy, yawrate, zdistance) +``` + +Check out the [automated API documentation](/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/commander/) for the Crazyflie cflib's commander frame work to find out what other functions you can use. + ## Parameters The parameter framework is used to read and set parameters. This @@ -302,12 +318,12 @@ It is also possible to get the current value of a parameter (when connected) wit value = get_value(complete_name) ``` -Note 1: If you call `set_value()` and then directly call `get_value()` for a parameter, you might not read back the new +>**Note 1** If you call `set_value()` and then directly call `get_value()` for a parameter, you might not read back the new value, but get the old one instead. The process is asynchronous and `get_value()` will not return the new value until the parameter value has propagated to the Crazyflie and back. Use the callback method if you need to be certain that you get the correct value after an update. -Note 2: `get_value()` and `set_value()` can not be called from callbacks until the Crazyflie is fully connected. +>**Note 2**: `get_value()` and `set_value()` can not be called from callbacks until the Crazyflie is fully connected. Most notably they can not be called from the `connected` callback as the parameter values have not been downloaded yet. Use the `fully_connected` callback to make sure the system is ready for parameter use. It is OK to call `get_value()` and `set_value()` from the `fully_connected` callback. @@ -492,7 +508,7 @@ and SyncCrazyflie instances. To get the log values, iterate the instance. ### MotionCommander -The MotionCommander is intended to simplify basic autonomous flight. The Crazyflie takes off +The MotionCommander class is intended to simplify basic autonomous flight, where the motion control is done from the host computer. The Crazyflie takes off when entering the "with" section, and lands when exiting. It has functions for basic movements that are blocking until the motion is finished. @@ -513,12 +529,12 @@ system, all positions are relative. It is mainly intended to be used with a Flow ### PositionHlCommander -The PositionHlCommander is intended to simplify basic autonomous flight. The Crazyflie takes off +The PositionHlCommander is intended to simplify basic autonomous flight, where all the high level commands exists inside the Crazyflie firmware. The Crazyflie takes off when entering the "with" section, and lands when exiting. It has functions for basic movements that are blocking until the motion is finished. The PositionHlCommander uses the high level commander in the Crazyflie and is -based on a global coordinate system and absolute positoinins. It is inteneded +based on a global coordinate system and absolute positions. It is intended to be used with a positioning system such as LPS, the lighthouse or a mocap system. ``` python From 2097eabb68ff53152d774ebf353d791df0052b71 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 14:05:48 +0200 Subject: [PATCH 488/861] move wireshakr info to development folder --- docs/{wireshark => development}/wireshark.md | 1 + 1 file changed, 1 insertion(+) rename docs/{wireshark => development}/wireshark.md (99%) diff --git a/docs/wireshark/wireshark.md b/docs/development/wireshark.md similarity index 99% rename from docs/wireshark/wireshark.md rename to docs/development/wireshark.md index cf18c8ead..a4c64c2fe 100644 --- a/docs/wireshark/wireshark.md +++ b/docs/development/wireshark.md @@ -1,6 +1,7 @@ --- title: Debugging CRTP using Wireshark page_id: wireshark_debugging +redirect: /wireshark/wireshark/ --- Wireshark is a free and open-source packet analyzer. It is used for network troubleshooting, analysis, software and communications protocol development, and education. It makes analyzing what is going on with packet based protocols easier. From 0ad7e90615cfb56c66fd7dde9e9c8528496d097f Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 14:16:55 +0200 Subject: [PATCH 489/861] convert to new menu style --- docs/_data/menu.yml | 21 ------------------- docs/api/index.md | 9 ++++++++ docs/development/index.md | 9 ++++++++ .../crazyradio_lib.md | 0 docs/functional-areas/index.md | 9 ++++++++ docs/installation/index.md | 9 ++++++++ docs/user-guides/index.md | 9 ++++++++ 7 files changed, 45 insertions(+), 21 deletions(-) delete mode 100644 docs/_data/menu.yml create mode 100644 docs/api/index.md create mode 100644 docs/development/index.md rename docs/{functonal-areas => functional-areas}/crazyradio_lib.md (100%) create mode 100644 docs/functional-areas/index.md create mode 100644 docs/installation/index.md create mode 100644 docs/user-guides/index.md diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml deleted file mode 100644 index e882b43b6..000000000 --- a/docs/_data/menu.yml +++ /dev/null @@ -1,21 +0,0 @@ -- page_id: home -- title: Installation - subs: - - {page_id: install} - - {page_id: usd_permissions} -- title: User guides - subs: - - {page_id: python_api} - - {page_id: sbs_connect_log_param} - - {page_id: sbs_motion_commander} - - {page_id: sbs_swarm_interface} -- title: Functional areas - subs: - - {page_id: crazyradio_lib} -- title: Development - subs: - - {page_id: api_reference} - - {page_id: wireshark_debugging} - - {page_id: matlab} - - {page_id: eeprom} - - {page_id: uart_communication} diff --git a/docs/api/index.md b/docs/api/index.md new file mode 100644 index 000000000..7f035176c --- /dev/null +++ b/docs/api/index.md @@ -0,0 +1,9 @@ +--- +title: Auto-generated API Documentation +page_id: api_index +sort_order: 5 +--- + +This section contains auto-generated documentation of the classes and functions of the Crazyflie python library. + +{% sub_page_menu %} \ No newline at end of file diff --git a/docs/development/index.md b/docs/development/index.md new file mode 100644 index 000000000..18b5b1f72 --- /dev/null +++ b/docs/development/index.md @@ -0,0 +1,9 @@ +--- +title: Development +page_id: development_index +sort_order: 4 +--- + +In this section you will find information about development of the Crazyflie python library + +{% sub_page_menu %} \ No newline at end of file diff --git a/docs/functonal-areas/crazyradio_lib.md b/docs/functional-areas/crazyradio_lib.md similarity index 100% rename from docs/functonal-areas/crazyradio_lib.md rename to docs/functional-areas/crazyradio_lib.md diff --git a/docs/functional-areas/index.md b/docs/functional-areas/index.md new file mode 100644 index 000000000..02f51f633 --- /dev/null +++ b/docs/functional-areas/index.md @@ -0,0 +1,9 @@ +--- +title: Functional Areas +page_id: functional_areas_index +sort_order: 3 +--- + +In this section you will find information about functional areas + +{% sub_page_menu %} \ No newline at end of file diff --git a/docs/installation/index.md b/docs/installation/index.md new file mode 100644 index 000000000..6530748e7 --- /dev/null +++ b/docs/installation/index.md @@ -0,0 +1,9 @@ +--- +title: installation +page_id: installation_index +sort_order: 1 +--- + +In this section you will find installation instructions + +{% sub_page_menu %} \ No newline at end of file diff --git a/docs/user-guides/index.md b/docs/user-guides/index.md new file mode 100644 index 000000000..5a1a37773 --- /dev/null +++ b/docs/user-guides/index.md @@ -0,0 +1,9 @@ +--- +title: User guides +page_id: user_guides_index +sort_order: 2 +--- + +In this section you will find user guides + +{% sub_page_menu %} \ No newline at end of file From 3a71684810c8457e84985eba3e4b5ac7a6b36450 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 14:37:22 +0200 Subject: [PATCH 490/861] Add line endings index files --- docs/api/index.md | 2 +- docs/development/index.md | 2 +- docs/functional-areas/index.md | 2 +- docs/installation/index.md | 2 +- docs/user-guides/index.md | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/api/index.md b/docs/api/index.md index 7f035176c..c934bc138 100644 --- a/docs/api/index.md +++ b/docs/api/index.md @@ -6,4 +6,4 @@ sort_order: 5 This section contains auto-generated documentation of the classes and functions of the Crazyflie python library. -{% sub_page_menu %} \ No newline at end of file +{% sub_page_menu %} diff --git a/docs/development/index.md b/docs/development/index.md index 18b5b1f72..c51c3de8c 100644 --- a/docs/development/index.md +++ b/docs/development/index.md @@ -6,4 +6,4 @@ sort_order: 4 In this section you will find information about development of the Crazyflie python library -{% sub_page_menu %} \ No newline at end of file +{% sub_page_menu %} diff --git a/docs/functional-areas/index.md b/docs/functional-areas/index.md index 02f51f633..651eecc6d 100644 --- a/docs/functional-areas/index.md +++ b/docs/functional-areas/index.md @@ -6,4 +6,4 @@ sort_order: 3 In this section you will find information about functional areas -{% sub_page_menu %} \ No newline at end of file +{% sub_page_menu %} diff --git a/docs/installation/index.md b/docs/installation/index.md index 6530748e7..b308ff986 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -6,4 +6,4 @@ sort_order: 1 In this section you will find installation instructions -{% sub_page_menu %} \ No newline at end of file +{% sub_page_menu %} diff --git a/docs/user-guides/index.md b/docs/user-guides/index.md index 5a1a37773..643028801 100644 --- a/docs/user-guides/index.md +++ b/docs/user-guides/index.md @@ -6,4 +6,4 @@ sort_order: 2 In this section you will find user guides -{% sub_page_menu %} \ No newline at end of file +{% sub_page_menu %} From 968a6ed32c446737b2d5e074205c997a8937883c Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 14:44:18 +0200 Subject: [PATCH 491/861] one url change --- docs/user-guides/python_api.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 0f575c7b4..b9b7f2b8e 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -261,7 +261,7 @@ If your Crazyflie has a positioning system (LPS, flowdeck, MoCap, Lighthouse), y send_hover_setpoint(self, vx, vy, yawrate, zdistance) ``` -Check out the [automated API documentation](/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/commander/) for the Crazyflie cflib's commander frame work to find out what other functions you can use. +Check out the [automated API documentation](/docs/api/cflib/crazyflie/commander.md) for the Crazyflie cflib's commander frame work to find out what other functions you can use. ## Parameters From 00d31b58db39ceac358076671ef851a3227322e1 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 7 Sep 2022 15:48:17 +0200 Subject: [PATCH 492/861] Changed LPS to Loco --- docs/user-guides/python_api.md | 8 ++++---- docs/user-guides/sbs_swarm_interface.md | 26 ++++++++++++------------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index b9b7f2b8e..bf5869877 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -77,7 +77,7 @@ The library supports reading and writing parameters at run-time to the firmware. This is intended to be used for data that is not continuously being changed by the firmware, like setting regulation parameters and reading out if the power-on self-tests passed. Parameters should only -change in the firmware when being set from the host (cfclient or a cflib script) or during start-up. +change in the firmware when being set from the host (cfclient or a cflib script) or during start-up. The library doesn\'t continuously update the parameter values, this should only be done once after connecting. After each write to a @@ -95,7 +95,7 @@ The parameters should be used in the following way: - For each write all the callbacks registered for this parameter will be called back -There is an exception for experimental support to change the parameter from within [firmware's app layer](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/app_layer/#internal-log-and-param-system). However do mind that this functionality is not according to the design of the parameters framework so that the host might not be updated correctly on the parameter change. +There is an exception for experimental support to change the parameter from within [firmware's app layer](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/app_layer/#internal-log-and-param-system). However do mind that this functionality is not according to the design of the parameters framework so that the host might not be updated correctly on the parameter change. ### Variable and parameter names @@ -255,7 +255,7 @@ kicks-in. ### Other commander setpoints sending -If your Crazyflie has a positioning system (LPS, flowdeck, MoCap, Lighthouse), you can also send velocity or position setpoints, like for instance: +If your Crazyflie has a positioning system (Loco, flowdeck, MoCap, Lighthouse), you can also send velocity or position setpoints, like for instance: ``` send_hover_setpoint(self, vx, vy, yawrate, zdistance) @@ -535,7 +535,7 @@ movements that are blocking until the motion is finished. The PositionHlCommander uses the high level commander in the Crazyflie and is based on a global coordinate system and absolute positions. It is intended -to be used with a positioning system such as LPS, the lighthouse or a mocap system. +to be used with a positioning system such as Loco, the lighthouse or a mocap system. ``` python with SyncCrazyflie(URI) as scf: diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index 2e09121a7..2afed5239 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -3,14 +3,14 @@ title: "Step-by-Step: Swarm Interface" page_id: sbs_swarm_interface --- -Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib.For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a global positioning system (Lighthouse,LPS or MoCap) that is able to provide data for the position estimation of the crazyflies. You can also use the Flowdeck but keep in mind that you should command relative movements of each Crazyflie and due to its nature it may lead to accumulative errors and unexpected behavior over time. +Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib.For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a global positioning system (Lighthouse, Loco or MoCap) that is able to provide data for the position estimation of the crazyflies. You can also use the Flowdeck but keep in mind that you should command relative movements of each Crazyflie and due to its nature it may lead to accumulative errors and unexpected behavior over time. # Prerequisites We will assume that you already know this before you start with the tutorial: * Some basic experience with python -* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) +* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) * Read the [high level commander](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/high_level_commander/), [swarm](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/swarm/) and [SyncCrazyflie](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/syncCrazyflie/) documentation . @@ -109,7 +109,7 @@ if __name__ == '__main__': If everything is working properly, you can move to the next step . # Step 2: Security Before Flying -Before executing any take off and flight maneuvers, the copters need to make sure that they have a precise enough position estimation. Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. +Before executing any take off and flight maneuvers, the copters need to make sure that they have a precise enough position estimation. Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. ```python with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(lightCheck) @@ -118,7 +118,7 @@ with Swarm(uris, factory=factory) as swarm: # Step 3: Taking off and Landing Sequentially Now we are going to execute the fist take off and landing using the high level commander. The high level commander (more information [here](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/sensor-to-control/commanders_setpoints/#high-level-commander)) is a class that handles all the high level commands like takeoff, landing, hover, go to position and others. The high level commander is usually preferred since it needs less communication and provides more autonomy on the Crazyflie. It is always on, but just in a lower priority so you just need to execute the take off and land commands through the below functions: -```python +```python def take_off(scf): commander= scf.cf.high_level_commander @@ -198,10 +198,10 @@ if __name__ == '__main__': print('Connected to Crazyflies') swarm.parallel_safe(lightCheck) swarm.reset_estimators() - + swarm.sequential(hover_sequence) ``` -After executing it you will see all copters performing the light check and then each copter take off , hover and land. This process is repeated for all copters in the swarm. +After executing it you will see all copters performing the light check and then each copter take off , hover and land. This process is repeated for all copters in the swarm. # Step 4: Taking off and Landing in Sync If you want to take off and land in sync, you can use the `parallel_safe()` method of the `Swarm` class. @@ -215,7 +215,7 @@ Now the same action is happening but for all the copters in parallel. # Step 5: Performing a square shape flight To make the swarm fly in a square shape, we will use the go_to method of the high level commander. Each copter executes 4 relative movements to its current position covering a square shape. -```python +```python def run_square_sequence(scf): box_size = 1 flight_time = 2 @@ -237,7 +237,7 @@ def run_square_sequence(scf): Keep in mind that the `go_to()` command does not block the code so you have to wait as long as the flight time of each movement to continue to the next one. Since we want these maneuvers to be synchronized, the `parallel_safe()` method is chosen to execute the sequence, in similar fashion with the above steps. You can include the sequence execution in the main code of the swarm in the following way: - + ```python swarm.parallel_safe(take_off) swarm.parallel_safe(run_square_sequence) @@ -270,7 +270,7 @@ def land(scf): def run_square_sequence(scf: SyncCrazyflie): ... - + uris = { 'radio://0/20/2M/E7E7E7E701', 'radio://0/20/2M/E7E7E7E702', @@ -286,7 +286,7 @@ if __name__ == '__main__': print('Connected to Crazyflies') swarm.parallel_safe(light_check) swarm.reset_estimators() - + swarm.parallel_safe(take_off) swarm.parallel_safe(run_square_sequence) swarm.parallel_safe(land) @@ -379,7 +379,7 @@ swarm.parallel_safe(run_sequence, args_dict=seq_args) ``` The final script is going to look like this : - + ```python import time @@ -439,7 +439,7 @@ seq_args = ... def run_sequence(scf: syncCrazyflie.SyncCrazyflie, sequence): ... - + if __name__ == '__main__': cflib.crtp.init_drivers() @@ -449,7 +449,7 @@ if __name__ == '__main__': swarm.parallel_safe(light_check) swarm.reset_estimators() - + swarm.parallel_safe(take_off) swarm.parallel_safe(run_square_sequence) swarm.parallel_safe(run_sequence, args_dict=seq_args) From 8d82a769952d1b49de0d6779a2b59ba94036151a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 7 Sep 2022 16:32:11 +0200 Subject: [PATCH 493/861] update version to 0.1.20.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index f9735cc03..c62ede4ed 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.20', + version='0.1.20.1', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From a3b9f77e8f94d5d42f6876a4e0c16298939fd648 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 2 Dec 2022 10:11:41 +0100 Subject: [PATCH 494/861] Refactoring of mem sub system --- cflib/crazyflie/mem/__init__.py | 394 +++++++++++++++----------------- 1 file changed, 189 insertions(+), 205 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 9dcd65753..d038a6b29 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -333,12 +333,10 @@ def write(self, memory, addr, data, flush_queue=False, progress_cb=None): def read(self, memory, addr, length): """ - Read the specified amount of bytes from the given memory at the given - address + Read the specified amount of bytes from the given memory at the given address """ if memory.id in self._read_requests: - logger.warning('There is already a read operation ongoing for ' - 'memory id {}'.format(memory.id)) + logger.warning('There is already a read operation ongoing for memory id {}'.format(memory.id)) return False rreq = _ReadRequest(memory, addr, length, self.cf) @@ -357,8 +355,7 @@ def refresh(self, refresh_done_callback): self.mem_read_cb.remove_callback(m.new_data) m.disconnect() except Exception as e: - logger.info( - 'Error when removing memory after update: {}'.format(e)) + logger.info('Error when removing memory after update: {}'.format(e)) self.mems = [] self.nbr_of_mems = 0 @@ -381,209 +378,196 @@ def _new_packet_cb(self, packet): payload = packet.data[1:] if chan == CHAN_INFO: - if cmd == CMD_INFO_NBR: - self.nbr_of_mems = payload[0] - logger.info('{} memories found'.format(self.nbr_of_mems)) - - # Start requesting information about the memories, - # if there are any... - if self.nbr_of_mems > 0: - if not self._getting_count: - self._getting_count = True - logger.debug('Requesting first id') - pk = CRTPPacket() - pk.set_header(CRTPPort.MEM, CHAN_INFO) - pk.data = (CMD_INFO_DETAILS, 0) - self.cf.send_packet(pk, expected_reply=( - CMD_INFO_DETAILS, 0)) - else: - self._refresh_callback() - - if cmd == CMD_INFO_DETAILS: - - # Did we get a good reply, otherwise try again: - if len(payload) < 5: - # Workaround for 1-wire bug when memory is detected - # but updating the info crashes the communication with - # the 1-wire. Fail by saying we only found 1 memory - # (the I2C). - logger.error( - '-------->Got good count, but no info on mem!') - self.nbr_of_mems = 1 - if self._refresh_callback: - self._refresh_callback() - self._refresh_callback = None - return - - # Create information about a new memory - # Id - 1 byte - mem_id = payload[0] - # Type - 1 byte - mem_type = payload[1] - # Size 4 bytes (as addr) - mem_size = struct.unpack('I', payload[2:6])[0] - # Addr (only valid for 1-wire?) - mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) - mem_addr = '' - for m in mem_addr_raw: - mem_addr += '{:02X}'.format(m) - - if (not self.get_mem(mem_id)): - if mem_type == MemoryElement.TYPE_1W: - mem = OWElement(id=mem_id, type=mem_type, - size=mem_size, - addr=mem_addr, mem_handler=self) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - self._ow_mems_left_to_update.append(mem.id) - elif mem_type == MemoryElement.TYPE_I2C: - mem = I2CElement(id=mem_id, type=mem_type, - size=mem_size, - mem_handler=self) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_DRIVER_LED: - mem = LEDDriverMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_LOCO: - mem = LocoMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - elif mem_type == MemoryElement.TYPE_TRAJ: - mem = TrajectoryMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_write_cb.add_callback(mem.write_done) - self.mem_write_failed_cb.add_callback(mem.write_failed) - elif mem_type == MemoryElement.TYPE_LOCO2: - mem = LocoMemory2(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - elif mem_type == MemoryElement.TYPE_LH: - mem = LighthouseMemory(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_read_failed_cb.add_callback( - mem.new_data_failed) - self.mem_write_cb.add_callback(mem.write_done) - self.mem_write_failed_cb.add_callback(mem.write_failed) - elif mem_type == MemoryElement.TYPE_MEMORY_TESTER: - mem = MemoryTester(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_DRIVER_LEDTIMING: - mem = LEDTimingsDriverMemory(id=mem_id, type=mem_type, - size=mem_size, - mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem.new_data) - self.mem_write_cb.add_callback(mem.write_done) - elif mem_type == MemoryElement.TYPE_DECK_MEMORY: - mem = DeckMemoryManager(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) - logger.debug(mem) - self.mem_read_cb.add_callback(mem._new_data) - self.mem_read_failed_cb.add_callback(mem._new_data_failed) - self.mem_write_cb.add_callback(mem._write_done) - self.mem_write_failed_cb.add_callback(mem._write_failed) - else: - mem = MemoryElement(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) - logger.debug(mem) - self.mems.append(mem) - self.mem_added_cb.call(mem) - - self._fetch_id = mem_id + 1 - - if self.nbr_of_mems - 1 >= self._fetch_id: - logger.debug( - 'Requesting information about memory {}'.format( - self._fetch_id)) - pk = CRTPPacket() - pk.set_header(CRTPPort.MEM, CHAN_INFO) - pk.data = (CMD_INFO_DETAILS, self._fetch_id) - self.cf.send_packet(pk, expected_reply=( - CMD_INFO_DETAILS, self._fetch_id)) - else: - logger.debug( - 'Done getting all the memories, start reading the OWs') - ows = self.get_mems(MemoryElement.TYPE_1W) - # If there are any OW mems start reading them, otherwise - # we are done - for ow_mem in ows: - ow_mem.update(self._mem_update_done) - if len(ows) == 0: - if self._refresh_callback: - self._refresh_callback() - self._refresh_callback = None - + self._handle_chan_info(cmd, payload) if chan == CHAN_WRITE: - id = cmd - (addr, status) = struct.unpack(' 0: - self._write_requests[id][0].start() - else: - logger.debug( - 'Status {}: write failed.'.format(status)) - # Remove from queue + self._handle_chan_write(cmd, payload) + if chan == CHAN_READ: + self._handle_chan_read(cmd, payload) + + def _handle_chan_info(self, cmd, payload): + if cmd == CMD_INFO_NBR: + self._handle_cmd_info_nbr(payload) + if cmd == CMD_INFO_DETAILS: + self._handle_cmd_info_details(payload) + + def _handle_cmd_info_nbr(self, payload): + self.nbr_of_mems = payload[0] + logger.info('{} memories found'.format(self.nbr_of_mems)) + + # Start requesting information about the memories, + if self.nbr_of_mems > 0: + if not self._getting_count: + self._getting_count = True + logger.debug('Requesting first id') + pk = CRTPPacket() + pk.set_header(CRTPPort.MEM, CHAN_INFO) + pk.data = (CMD_INFO_DETAILS, 0) + self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) + else: + self._refresh_callback() + + def _handle_cmd_info_details(self, payload): + # Did we get a good reply, otherwise try again: + if len(payload) < 5: + # Workaround for 1-wire bug when memory is detected + # but updating the info crashes the communication with + # the 1-wire. Fail by saying we only found 1 memory + # (the I2C). + logger.error('-------->Got good count, but no info on mem!') + self.nbr_of_mems = 1 + if self._refresh_callback: + self._refresh_callback() + self._refresh_callback = None + return + + # Create information about a new memory + # Id - 1 byte + mem_id = payload[0] + # Type - 1 byte + mem_type = payload[1] + # Size 4 bytes (as addr) + mem_size = struct.unpack('I', payload[2:6])[0] + # Addr (only valid for 1-wire?) + mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) + mem_addr = '' + for m in mem_addr_raw: + mem_addr += '{:02X}'.format(m) + + if (not self.get_mem(mem_id)): + if mem_type == MemoryElement.TYPE_1W: + mem = OWElement(id=mem_id, type=mem_type, + size=mem_size, + addr=mem_addr, mem_handler=self) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + self._ow_mems_left_to_update.append(mem.id) + elif mem_type == MemoryElement.TYPE_I2C: + mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_DRIVER_LED: + mem = LEDDriverMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_LOCO: + mem = LocoMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + elif mem_type == MemoryElement.TYPE_TRAJ: + mem = TrajectoryMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_write_cb.add_callback(mem.write_done) + self.mem_write_failed_cb.add_callback(mem.write_failed) + elif mem_type == MemoryElement.TYPE_LOCO2: + mem = LocoMemory2(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + elif mem_type == MemoryElement.TYPE_LH: + mem = LighthouseMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_read_failed_cb.add_callback(mem.new_data_failed) + self.mem_write_cb.add_callback(mem.write_done) + self.mem_write_failed_cb.add_callback(mem.write_failed) + elif mem_type == MemoryElement.TYPE_MEMORY_TESTER: + mem = MemoryTester(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_DRIVER_LEDTIMING: + mem = LEDTimingsDriverMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_write_cb.add_callback(mem.write_done) + elif mem_type == MemoryElement.TYPE_DECK_MEMORY: + mem = DeckMemoryManager(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem._new_data) + self.mem_read_failed_cb.add_callback(mem._new_data_failed) + self.mem_write_cb.add_callback(mem._write_done) + self.mem_write_failed_cb.add_callback(mem._write_failed) + else: + mem = MemoryElement(id=mem_id, type=mem_type, + size=mem_size, mem_handler=self) + logger.debug(mem) + self.mems.append(mem) + self.mem_added_cb.call(mem) + + self._fetch_id = mem_id + 1 + + if self.nbr_of_mems - 1 >= self._fetch_id: + logger.debug('Requesting information about memory {}'.format(self._fetch_id)) + pk = CRTPPacket() + pk.set_header(CRTPPort.MEM, CHAN_INFO) + pk.data = (CMD_INFO_DETAILS, self._fetch_id) + self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id)) + else: + logger.debug('Done getting all the memories, start reading the OWs') + ows = self.get_mems(MemoryElement.TYPE_1W) + # If there are any OW mems start reading them, otherwise + # we are done + for ow_mem in ows: + ow_mem.update(self._mem_update_done) + if len(ows) == 0: + if self._refresh_callback: + self._refresh_callback() + self._refresh_callback = None + + def _handle_chan_write(self, cmd, payload): + id = cmd + (addr, status) = struct.unpack(' 0: self._write_requests[id][0].start() - - self._write_requests_lock.release() - - # Call callbacks after the lock has been released to alow for new writes - # to be initiated from the callback. - if do_call_sucess_cb: - self.mem_write_cb.call(wreq.mem, wreq.addr) - if do_call_fail_cb: - self.mem_write_failed_cb.call(wreq.mem, wreq.addr) - - if chan == CHAN_READ: - id = cmd - (addr, status) = struct.unpack(' 0: + self._write_requests[id][0].start() + + self._write_requests_lock.release() + + # Call callbacks after the lock has been released to alow for new writes + # to be initiated from the callback. + if do_call_sucess_cb: + self.mem_write_cb.call(wreq.mem, wreq.addr) + if do_call_fail_cb: + self.mem_write_failed_cb.call(wreq.mem, wreq.addr) + + def _handle_chan_read(self, cmd, payload): + id = cmd + (addr, status) = struct.unpack(' Date: Fri, 2 Dec 2022 10:12:22 +0100 Subject: [PATCH 495/861] Fixed flake8 problem --- cflib/crtp/radiodriver.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index db9ed9088..f0bf50eb7 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -42,12 +42,10 @@ from threading import Semaphore from threading import Thread from typing import Any -from typing import Dict from typing import Iterable from typing import List from typing import Optional from typing import Tuple -from typing import Union from urllib.parse import parse_qs from urllib.parse import urlparse From 273099f5a1da447c68f8235b975da04ed7ccc996 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 2 Dec 2022 13:08:04 +0100 Subject: [PATCH 496/861] Handle disconnects when reading/writing memory mappings --- cflib/crazyflie/mem/__init__.py | 50 +++++++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 9 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index d038a6b29..b3767676b 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -256,6 +256,8 @@ def _clear_state(self): # Called when new memories have been added self.mem_added_cb = Caller() + self._clear_refresh_callbacks() + # Called to signal completion of read or write self.mem_read_cb = Caller() self.mem_read_failed_cb = Caller() @@ -263,6 +265,7 @@ def _clear_state(self): self.mem_write_failed_cb = Caller() self._refresh_callback = None + self._refresh_failed_callback = None self._fetch_id = 0 self.nbr_of_mems = 0 self._ow_mem_fetch_index = 0 @@ -272,6 +275,10 @@ def _clear_state(self): self._ow_mems_left_to_update = [] self._getting_count = False + def _clear_refresh_callbacks(self): + self._refresh_callback = None + self._refresh_failed_callback = None + def _mem_update_done(self, mem): """ Callback from each individual memory (only 1-wire) when reading of @@ -285,7 +292,7 @@ def _mem_update_done(self, mem): if len(self._ow_mems_left_to_update) == 0: if self._refresh_callback: self._refresh_callback() - self._refresh_callback = None + self._clear_refresh_callbacks() def get_mem(self, id): """Fetch the memory with the supplied id""" @@ -346,9 +353,10 @@ def read(self, memory, addr, length): return True - def refresh(self, refresh_done_callback): + def refresh(self, refresh_done_callback, refresh_failed_cb=None): """Start fetching all the detected memories""" self._refresh_callback = refresh_done_callback + self._refresh_failed_callback = refresh_failed_cb self._fetch_id = 0 for m in self.mems: try: @@ -369,8 +377,32 @@ def refresh(self, refresh_done_callback): def _disconnected(self, uri): """The link to the Crazyflie has been broken. Reset state""" + self._call_all_failed_callbacks() self._clear_state() + def _call_all_failed_callbacks(self): + # Read requests + read_requests = list(self._read_requests.values()) + self._read_requests.clear() + for rreq in read_requests: + self.mem_read_failed_cb.call(rreq.mem, rreq.addr, rreq.data) + + # Write requests + write_requests = [] + self._write_requests_lock.acquire() + for requests in self._write_requests.values(): + write_requests += requests + self._write_requests.clear() + self._write_requests_lock.release() + + for wreq in write_requests: + self.mem_write_failed_cb.call(wreq.mem, wreq.addr) + + # Info + if self._refresh_failed_callback: + self._refresh_failed_callback() + self._clear_refresh_callbacks() + def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel @@ -404,7 +436,9 @@ def _handle_cmd_info_nbr(self, payload): pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) else: - self._refresh_callback() + if self._refresh_callback: + self._refresh_callback() + self._clear_refresh_callbacks() def _handle_cmd_info_details(self, payload): # Did we get a good reply, otherwise try again: @@ -417,7 +451,7 @@ def _handle_cmd_info_details(self, payload): self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() - self._refresh_callback = None + self._clear_refresh_callbacks() return # Create information about a new memory @@ -488,8 +522,7 @@ def _handle_cmd_info_details(self, payload): self.mem_write_cb.add_callback(mem._write_done) self.mem_write_failed_cb.add_callback(mem._write_failed) else: - mem = MemoryElement(id=mem_id, type=mem_type, - size=mem_size, mem_handler=self) + mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) self.mems.append(mem) self.mem_added_cb.call(mem) @@ -512,7 +545,7 @@ def _handle_cmd_info_details(self, payload): if len(ows) == 0: if self._refresh_callback: self._refresh_callback() - self._refresh_callback = None + self._clear_refresh_callbacks() def _handle_chan_write(self, cmd, payload): id = cmd @@ -569,5 +602,4 @@ def _handle_chan_read(self, cmd, payload): else: logger.debug('Status {}: read failed.'.format(status)) self._read_requests.pop(id, None) - self.mem_read_failed_cb.call( - rreq.mem, rreq.addr, rreq.data) + self.mem_read_failed_cb.call(rreq.mem, rreq.addr, rreq.data) From 80000e2d16315625f2a5f5ce1f3ff6c9be405cdf Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 6 Dec 2022 12:28:47 +0100 Subject: [PATCH 497/861] Added console logs to bootloader --- cflib/bootloader/__init__.py | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 0a73b0778..40d49aa63 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -131,7 +131,7 @@ def start_bootloader(self, warm_boot=False, cf=None): def get_target(self, target_id): return self._cload.request_info_update(target_id) - def flash(self, filename: str, targets: List[Target], cf=None): + def flash(self, filename: str, targets: List[Target], cf=None, enable_console_log: Optional[bool] = False): # Separate flash targets from decks platform = self._get_platform_id() flash_targets = [t for t in targets if t.platform == platform] @@ -170,7 +170,8 @@ def flash(self, filename: str, targets: List[Target], cf=None): # Flash all decks and reboot after each deck current_index = 0 while current_index != -1: - current_index = self._flash_deck_incrementally(deck_artifacts, deck_targets, current_index) + current_index = self._flash_deck_incrementally(deck_artifacts, deck_targets, current_index, + enable_console_log=enable_console_log) if self.progress_cb: self.progress_cb('Deck updated! Restarting...', int(100)) if current_index != -1: @@ -198,7 +199,8 @@ def flash_full(self, cf: Optional[Crazyflie] = None, targets: Optional[Tuple[str, ...]] = None, info_cb: Optional[Callable[[int, TargetTypes], NoReturn]] = None, progress_cb: Optional[Callable[[str, int], NoReturn]] = None, - terminate_flash_cb: Optional[Callable[[], bool]] = None): + terminate_flash_cb: Optional[Callable[[], bool]] = None, + enable_console_log: Optional[bool] = False): """ Flash .zip or bin .file to list of targets. Reset to firmware when done. @@ -218,7 +220,7 @@ def flash_full(self, cf: Optional[Crazyflie] = None, info_cb(self.protocol_version, connected) if filename is not None: - self.flash(filename, targets, cf) + self.flash(filename, targets, cf, enable_console_log=enable_console_log) self.reset_to_firmware() def _get_flash_artifacts_from_zip(self, filename): @@ -396,14 +398,15 @@ def console_callback(self, text: str): # Crazyflie at appropriate places. print(text, end='') - def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: List[Target], start_index: int): + def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: List[Target], start_index: int, + enable_console_log: Optional[bool] = False): flash_all_targets = len(targets) == 0 if self.progress_cb: self.progress_cb('Identifying deck to be updated', 0) with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: - # Uncomment to enable console logs from the CF. - # scf.cf.console.receivedChar.add_callback(self.console_callback) + if enable_console_log: + scf.cf.console.receivedChar.add_callback(self.console_callback) deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) deck_mems_count = len(deck_mems) @@ -499,6 +502,10 @@ def progress_cb(msg: str, percent: int): self.progress_cb(f'Failed to update deck {deck.name}', int(0)) raise RuntimeError(f'Failed to update deck {deck.name}') + if enable_console_log: + # Wait a bit to let the console log print + time.sleep(4) + # We flashed a deck, return for re-boot next_index = deck_index + 1 if next_index >= len(decks): From d9c5bdda01fc72e378ed2144a13ae9add0178125 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 7 Dec 2022 10:35:14 +0100 Subject: [PATCH 498/861] Avoid possible deadlock in param --- cflib/crazyflie/param.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index c529fea21..d9b0cad8d 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -33,7 +33,7 @@ import logging import struct from collections import namedtuple -from queue import Queue +from queue import Queue, Empty from threading import Event from threading import Lock from threading import Thread @@ -556,13 +556,17 @@ def request_extended_types(self, elements): def _close(self): # First empty the queue from all packets - while not self.request_queue.empty(): - self.request_queue.get() + try: + while True: + self.request_queue.get(block=False) + except Empty: + pass + # Then force an unlock of the mutex if we are waiting for a packet # we didn't get back due to a disconnect for example. try: self._lock.release() - except Exception: + except RuntimeError: pass def run(self): @@ -595,13 +599,17 @@ def __init__(self, cf, useV2, updated_callback): def close(self): # First empty the queue from all packets - while not self.request_queue.empty(): - self.request_queue.get() + try: + while True: + self.request_queue.get(block=False) + except Empty: + pass + # Then force an unlock of the mutex if we are waiting for a packet # we didn't get back due to a disconnect for example. try: self.wait_lock.release() - except Exception: + except RuntimeError: pass def request_param_setvalue(self, pk): From 644a5a02bf24165e8eda3b77eee1c6e5a112232e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 7 Dec 2022 10:38:07 +0100 Subject: [PATCH 499/861] Fix flake8 --- cflib/crazyflie/param.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index d9b0cad8d..4f77c9457 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -33,7 +33,8 @@ import logging import struct from collections import namedtuple -from queue import Queue, Empty +from queue import Empty +from queue import Queue from threading import Event from threading import Lock from threading import Thread From ef8d67b6cf3c8846f96ca769f153531270f34a4b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 7 Dec 2022 11:30:24 +0100 Subject: [PATCH 500/861] Added blocking method for writing to the trajectory memory. Removed Uploader class in examples. --- cflib/crazyflie/mem/trajectory_memory.py | 16 ++++++++++- .../autonomous_sequence_high_level.py | 27 +------------------ ...tonomous_sequence_high_level_compressed.py | 27 +------------------ examples/qualisys/qualisys_hl_commander.py | 27 +------------------ 4 files changed, 18 insertions(+), 79 deletions(-) diff --git a/cflib/crazyflie/mem/trajectory_memory.py b/cflib/crazyflie/mem/trajectory_memory.py index d9460b28f..e64510204 100644 --- a/cflib/crazyflie/mem/trajectory_memory.py +++ b/cflib/crazyflie/mem/trajectory_memory.py @@ -24,6 +24,7 @@ import struct from .memory_element import MemoryElement +from cflib.utils.callbacks import Syncer logger = logging.getLogger(__name__) @@ -183,7 +184,7 @@ def write_data(self, write_finished_cb, write_failed_cb=None, start_addr=0x00): Write trajectory data to the Crazyflie. The trajectory in self.trajectory is written to the Crazyflie. By default the trajectory is written to address 0 of the Crazyflie trajectory memory, but it is possible to - use a different address. This can be interesting if you want to define more than one trajectory but requires + use a different address. This can be interesting if you want to define more than one trajectory but it requires careful handling of the addresses to avoid overwriting trajectories and staying within the trajectory memory. @param write_finished_cb A callback that is called when the write trajectory is uploaded. @@ -201,6 +202,19 @@ def write_data(self, write_finished_cb, write_failed_cb=None, start_addr=0x00): self.mem_handler.write(self, start_addr, data, flush_queue=True) return len(data) + def write_data_sync(self, start_addr=0x00): + """ + Same functionality as write_data() but synchronous (blocking) + + Args: + start_addr (hexadecimal, optional): The address in the trajectory memory to upload the trajectory to. + Defaults to 0x00. + """ + syncer = Syncer() + self.write_data(syncer.success_cb, write_failed_cb=syncer.failure_cb, start_addr=start_addr) + syncer.wait() + return syncer.is_success + def write_done(self, mem, addr): if self._write_finished_cb and mem.id == self.id: logger.debug('Write trajectory data done') diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 90e650b71..c827ab947 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -62,31 +62,6 @@ ] -class Uploader: - def __init__(self): - self._is_done = False - self._success = True - - def upload(self, trajectory_mem): - print('Uploading data') - trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed) - - while not self._is_done: - time.sleep(0.2) - - return self._success - - def _upload_done(self, mem, addr): - print('Data uploaded') - self._is_done = True - self._success = True - - def _upload_failed(self, mem, addr): - print('Data upload failed') - self._is_done = True - self._success = False - - def wait_for_position_estimator(scf): print('Waiting for estimator to find position...') @@ -158,7 +133,7 @@ def upload_trajectory(cf, trajectory_id, trajectory): trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw)) total_duration += duration - upload_result = Uploader().upload(trajectory_mem) + upload_result = trajectory_mem.write_data_sync() if not upload_result: print('Upload failed, aborting!') sys.exit(1) diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index d4e9ece0c..bc8dc84c7 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -58,31 +58,6 @@ ] -class Uploader: - def __init__(self): - self._is_done = False - self._success = True - - def upload(self, trajectory_mem): - print('Uploading data') - trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed) - - while not self._is_done: - time.sleep(0.2) - - return self._success - - def _upload_done(self, mem, addr): - print('Data uploaded') - self._is_done = True - self._success = True - - def _upload_failed(self, mem, addr): - print('Data upload failed') - self._is_done = True - self._success = False - - def wait_for_position_estimator(scf): print('Waiting for estimator to find position...') @@ -145,7 +120,7 @@ def upload_trajectory(cf, trajectory_id, trajectory): trajectory_mem.trajectory = trajectory - upload_result = Uploader().upload(trajectory_mem) + upload_result = trajectory_mem.write_data_sync() if not upload_result: print('Upload failed, aborting!') sys.exit(1) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index ca8aba203..51fc99364 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -154,31 +154,6 @@ async def _close(self): self.connection.disconnect() -class Uploader: - def __init__(self): - self._is_done = False - self._success = True - - def upload(self, trajectory_mem): - print('Uploading data') - trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed) - - while not self._is_done: - time.sleep(0.2) - - return self._success - - def _upload_done(self, mem, addr): - print('Data uploaded') - self._is_done = True - self._success = True - - def _upload_failed(self, mem, addr): - print('Data upload failed') - self._is_done = True - self._success = False - - def wait_for_position_estimator(scf): print('Waiting for estimator to find position...') @@ -287,7 +262,7 @@ def upload_trajectory(cf, trajectory_id, trajectory): trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw)) total_duration += duration - Uploader().upload(trajectory_mem) + trajectory_mem.write_data_sync() cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.trajectory)) return total_duration From 1c07945e7f93587d9e5afd860258aa304b61555d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 7 Dec 2022 15:02:16 +0100 Subject: [PATCH 501/861] Adjusted delay for console log when flashing --- cflib/bootloader/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 40d49aa63..663d73809 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -504,7 +504,7 @@ def progress_cb(msg: str, percent: int): if enable_console_log: # Wait a bit to let the console log print - time.sleep(4) + time.sleep(6) # We flashed a deck, return for re-boot next_index = deck_index + 1 From e5d2cd0eb407365f09f4197a5ecfe1d0df428468 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 12 Dec 2022 13:41:02 +0100 Subject: [PATCH 502/861] Update version to 0.1.21 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index c62ede4ed..c01e5a98b 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.20.1', + version='0.1.21', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From 53fad70f891904bc3d612566f7100cb292a91e76 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 15 Dec 2022 14:25:29 +0100 Subject: [PATCH 503/861] Updated build scripts --- .github/workflows/CI.yml | 5 ++++- .github/workflows/dependency_check.yml | 4 ++-- .github/workflows/python-publish.yml | 2 +- .github/workflows/test-python-publish.yml | 2 +- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index f2a0e408b..f5658f071 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -6,6 +6,9 @@ on: branches: [ master ] pull_request: branches: [ master ] + schedule: + # Weekly build to make sure dependencies are OK + - cron: '30 16 * * 6' jobs: build: @@ -13,7 +16,7 @@ jobs: steps: - name: Checkout repo - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Build run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index eb825e556..30271539d 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -12,7 +12,7 @@ jobs: image: python:latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: install lib run: pip install -e . @@ -23,7 +23,7 @@ jobs: image: python:3.7 steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: install lib run: pip install -e . diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml index c88f03fd9..30be46c12 100644 --- a/.github/workflows/python-publish.yml +++ b/.github/workflows/python-publish.yml @@ -22,7 +22,7 @@ jobs: steps: - uses: actions/checkout@v3 - name: Set up Python - uses: actions/setup-python@v3 + uses: actions/setup-python@v4 with: python-version: '3.x' - name: Install dependencies diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index 707fef98c..dd15af6cb 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -22,7 +22,7 @@ jobs: steps: - uses: actions/checkout@v3 - name: Set up Python - uses: actions/setup-python@v3 + uses: actions/setup-python@v4 with: python-version: '3.x' - name: Install dependencies From 5a96387225c0ef37e4a22bedcf261a5276db443f Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 3 Jan 2023 13:57:29 +0100 Subject: [PATCH 504/861] Include start and size when for CPX UART CRC RX --- cflib/cpx/transports.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cflib/cpx/transports.py b/cflib/cpx/transports.py index 94d6a4efc..830c46b9b 100644 --- a/cflib/cpx/transports.py +++ b/cflib/cpx/transports.py @@ -140,7 +140,9 @@ def readPacket(self): else: data = self._serial.read(size) # Size is excluding start (0xFF) and checksum at end crc = self._serial.read(1) - if self._calcXORchecksum(data) != crc: + # CRC includes start and size + calculated_crc = self._calcXORchecksum(bytes([start, size]) + data) + if calculated_crc != ord(crc): print('CRC error!') # Send CTS self._serial.write([0xFF, 0x00]) From 783ffdde706a4fcdfdceb53044823fc1027b4d4c Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 3 Jan 2023 14:38:02 +0100 Subject: [PATCH 505/861] Update uart_communication.md --- docs/development/uart_communication.md | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/docs/development/uart_communication.md b/docs/development/uart_communication.md index 07193302c..5c5af35ea 100644 --- a/docs/development/uart_communication.md +++ b/docs/development/uart_communication.md @@ -13,6 +13,7 @@ If you are connecting to a Raspberry Pi look for the UART pins there connect the - Crazyflie TX2 -- Raspberry Pi RX - Crazyflie RX2 -- Raspberry Pi TX +- Crazyflie Gdn -- Raspberry Pi Gdn (if not both connected to same powersource) ## Crazyflie Firmware @@ -71,15 +72,19 @@ URI = 'serial://ttyAMA0' # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) +def console_callback(text: str): + print(text, end='') + if __name__ == '__main__': # Initialize the low-level drivers including the serial driver cflib.crtp.init_drivers(enable_serial_driver=True) with SyncCrazyflie(URI) as scf: - # We take off when the commander is created - with MotionCommander(scf) as mc: - print('Taking off!') - time.sleep(0.1) - # We land when the MotionCommander goes out of scope - print('Landing!') + print('[host] Connected, use ctrl-c to quit.') + while True: + time.sleep(1) ``` + +## Troubleshooting + +If you see the a `CRC error` when running the script, make sure to install the cflib from source. From 0d73698e9ab33064646a6553a2ab4f532abae241 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 3 Jan 2023 14:47:37 +0100 Subject: [PATCH 506/861] Update uart_communication.md --- docs/development/uart_communication.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/docs/development/uart_communication.md b/docs/development/uart_communication.md index 5c5af35ea..017e85605 100644 --- a/docs/development/uart_communication.md +++ b/docs/development/uart_communication.md @@ -54,7 +54,7 @@ Once everything is set up you should be able to control the Crazyflie via UART. Add the parameter `enable_serial_driver=True` to `cflib.crtp.init_drivers()` and connect to the Crazyflie using a serial URI. The serial URI has the form `serial://` (e.g. `serial://ttyAMA0`, `serial://ttyUSB5`) or if the OS of the controlling device does not provide the name `serial://` (e.g. `serial:///dev/ttyAMA0`). -The following script might give an idea on how a first test of the setup might look like. +The following script might give an idea on how a first test of the setup might look like to print the console output of the Crazyflie on the Raspberry pi ```python #!/usr/bin/env python3 @@ -63,6 +63,7 @@ import logging import time import cflib.crtp +from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander @@ -78,6 +79,8 @@ def console_callback(text: str): if __name__ == '__main__': # Initialize the low-level drivers including the serial driver cflib.crtp.init_drivers(enable_serial_driver=True) + cf = Crazyflie(rw_cache='./cache') + cf.console.receivedChar.add_callback(console_callback) with SyncCrazyflie(URI) as scf: print('[host] Connected, use ctrl-c to quit.') @@ -87,4 +90,5 @@ if __name__ == '__main__': ## Troubleshooting -If you see the a `CRC error` when running the script, make sure to install the cflib from source. +* If you see the a `CRC error` when running the script, make sure to install the cflib from source. +* If the script hangs with connecting, restart the crazyflie From 5d5890811983d2411e0074a882a567ed475b74ca Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 9 Jan 2023 15:08:21 +0100 Subject: [PATCH 507/861] update script uart communication --- docs/development/uart_communication.md | 28 +++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/docs/development/uart_communication.md b/docs/development/uart_communication.md index 017e85605..ec56c7fe2 100644 --- a/docs/development/uart_communication.md +++ b/docs/development/uart_communication.md @@ -54,7 +54,7 @@ Once everything is set up you should be able to control the Crazyflie via UART. Add the parameter `enable_serial_driver=True` to `cflib.crtp.init_drivers()` and connect to the Crazyflie using a serial URI. The serial URI has the form `serial://` (e.g. `serial://ttyAMA0`, `serial://ttyUSB5`) or if the OS of the controlling device does not provide the name `serial://` (e.g. `serial:///dev/ttyAMA0`). -The following script might give an idea on how a first test of the setup might look like to print the console output of the Crazyflie on the Raspberry pi +The following script might give an idea on how a first test of the setup might look like to print the log variables of the Crazyflie on the Raspberry pi ```python #!/usr/bin/env python3 @@ -65,14 +65,12 @@ 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.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger # choose the serial URI that matches the setup serial device URI = 'serial://ttyAMA0' -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - def console_callback(text: str): print(text, end='') @@ -81,11 +79,27 @@ if __name__ == '__main__': cflib.crtp.init_drivers(enable_serial_driver=True) cf = Crazyflie(rw_cache='./cache') cf.console.receivedChar.add_callback(console_callback) + + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') + with SyncCrazyflie(URI) as scf: print('[host] Connected, use ctrl-c to quit.') - while True: - time.sleep(1) + with SyncLogger(scf, lg_stab) as logger: + endTime = time.time() + 10 + for log_entry in logger: + timestamp = log_entry[0] + data = log_entry[1] + logconf_name = log_entry[2] + + print('[%d][%s]: %s' % (timestamp, logconf_name, data)) + + if time.time() > endTime: + break + ``` ## Troubleshooting From a09e6fe157e9af214b1295a72108eb1b01b5cb6a Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Wed, 1 Feb 2023 13:04:44 +0200 Subject: [PATCH 508/861] Fix typos in python_api.md --- docs/user-guides/python_api.md | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index bf5869877..6118d2a1a 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -66,7 +66,7 @@ following way: There\'s a few limitations that needs to be taken into account: -- The maximum byte lenght for a log packet is of 26 bytes. This for +- The maximum byte length for a log packet is of 26 bytes. This for for example allows to log 6 floats and one uint16_t (6*4 + 2 bytes) in a single packet. - The minimum period of a for a log configuration is multiples of 10ms @@ -237,14 +237,14 @@ To send a new control set-point use the following: Thrust is an integer value ranging from 10001 (next to no power) to 60000 (full power). It corresponds to the mean thrust that will be -appied to the motors. There is a battery compensation algorythm +applied to the motors. There is a battery compensation algorithm applied to make the thrust mostly independent of battery voltage. -Roll/pitch are in degree and yarate in degree/seconds. +Roll/pitch are in degree and yawrate in degree/seconds. This command will set the attitude controller setpoint for the next 500ms. After 500ms without net setpoint, the Crazyflie will apply a setpoint with the same thrust but with roll/pitch/yawrate = 0, this -will make the Crazyflie stop accelerate. After 2secons without new +will make the Crazyflie stop accelerate. After 2 seconds without new setpoint the Crazyflie will cut power to the motors. Note that this command implements a motor lock mechanism that is @@ -428,12 +428,12 @@ The logging cannot be started until your are connected to a Crazyflie: print "Error when logging %s" % logconf.name ``` -The values of log varibles are transferred from the Crazyflie using CRTP -packets, where all varibles belonging to one logging configuration are -transfered in the same packet. A CRTP packet has a maximum data size of +The values of log variables are transferred from the Crazyflie using CRTP +packets, where all variables belonging to one logging configuration are +transferred in the same packet. A CRTP packet has a maximum data size of 30 bytes, which sets an upper limit to the number of variables that can be used in one logging configuration. If the desired log variables -do not fit in one logging configuration, a second cofiguration may +do not fit in one logging configuration, a second configuration may be added. ``` python @@ -442,7 +442,7 @@ be added. ## Synchronous API -The synchronous classes are wrappers around the asynchronouse API, where the asynchronous +The synchronous classes are wrappers around the asynchronous API, where the asynchronous calls/callbacks are replaced with blocking calls. The synchronous API does not provide the full flexibility of the asynchronous API, but is useful when writing small scripts for logging for instance. @@ -453,12 +453,12 @@ exiting it, for instance a connection or take off/landing of a Crazyflie. ### SyncCrazyflie -The SyncCrazyflie class wrapps a Crazyflie instance and mainly simplifies connect/disconnect. +The SyncCrazyflie class wraps a Crazyflie instance and mainly simplifies connect/disconnect. Basic usage ``` python with SyncCrazyflie(uri) as scf: - # A Crazyflie instance is created and is now connected. If the connection failes, + # A Crazyflie instance is created and is now connected. If the connection fails, # an exception is raised. # The underlying crazyflie object can be accessed through the cf member From 876391d5eeee48dfbbd6c880f142802a627105ae Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Thu, 2 Feb 2023 09:16:29 +0200 Subject: [PATCH 509/861] Fix typos crazyradio_lib.md --- docs/functional-areas/crazyradio_lib.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/functional-areas/crazyradio_lib.md b/docs/functional-areas/crazyradio_lib.md index 5de3cf450..2c0be82bc 100644 --- a/docs/functional-areas/crazyradio_lib.md +++ b/docs/functional-areas/crazyradio_lib.md @@ -128,7 +128,7 @@ None | -------------| --------------------------------------------------------| | Parameters | (bool) active| | Returns | None| -| Description | Enable or disable the continious carrier mode. In continious carrier the Crazyradio transmit a constant sinus at the currently set frequency (channel) and power. This is a test mode that can affect other 2.4GHz devices (ex. wifi) it should only be used in a lab for test purposes.| +| Description | Enable or disable the continuous carrier mode. In continuous carrier the Crazyradio transmit a constant sinus at the currently set frequency (channel) and power. This is a test mode that can affect other 2.4GHz devices (ex. wifi) it should only be used in a lab for test purposes.| ##### scan\_channels(self, start, stop, packet) From 690360da07c9f1a85895cc44f049d751e11bbb78 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Thu, 2 Feb 2023 09:18:47 +0200 Subject: [PATCH 510/861] Fix typos in sbs_connect_log_param.md --- docs/user-guides/sbs_connect_log_param.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index 08b380179..c3a27d6f4 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -411,7 +411,7 @@ Then add the following to the `def simple_param_async(...)` function: time.sleep(1) ``` -The sleep function is to give the script a bit more time to wait for the Crazyflies response and not lose the connection immediatly. +The sleep function is to give the script a bit more time to wait for the Crazyflies response and not lose the connection immediately. If you would like to test out the script now already, replace `simple_log_async(...)` with `simple_param_async(scf, group, name)` and run the script. You can see that it will print out the variable name and value: `The crazyflie has parameter stabilizer.estimator set at number: 1` @@ -436,7 +436,7 @@ What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) para ## Finishing and running the script -It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physcially restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. +It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physically restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. So finish the `simple_param_async(...)` function by adding the next few lines: ```python From 9b05b3f879226e2d02a4788e6aa3a8492fd04362 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Thu, 2 Feb 2023 09:19:47 +0200 Subject: [PATCH 511/861] Fix typos sbs_motion_commander.md --- docs/user-guides/sbs_motion_commander.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 7d44985e1..f1a49ce85 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -155,7 +155,7 @@ The reason for the crazyflie to immediately take off, is that the motion command ## Changing the height -Currently the motion commander had 0.3 meters height as default but this can ofcourse be changed. +Currently the motion commander had 0.3 meters height as default but this can of course be changed. Change the following line in `take_off_simple(...)`: ```python @@ -257,7 +257,7 @@ Now we are going to add a turn into it. Replace the content under motion command time.sleep(1) ``` -Try to run the script again. Now you can see the crazyflie take off, go forward, turn 180 degrees and go forward again to its initial position. The `mc.back()` needed to be replaced with the forward since the motion commander sends the velocity setpoints in the __body fixed coordinated__ system. This means that the commands forward will go forward to whereever the current heading (the front) of the crazyflie points to. +Try to run the script again. Now you can see the crazyflie take off, go forward, turn 180 degrees and go forward again to its initial position. The `mc.back()` needed to be replaced with the forward since the motion commander sends the velocity setpoints in the __body fixed coordinated__ system. This means that the commands forward will go forward to wherever the current heading (the front) of the crazyflie points to. Double check if your code code is still correct: From 3ef6c8fb52f0819aaeeaf8aa47a4b6ff28e1d6fa Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Thu, 2 Feb 2023 09:20:18 +0200 Subject: [PATCH 512/861] Fix typos in sbs_swarm_interface.md --- docs/user-guides/sbs_swarm_interface.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index 2afed5239..4a04f6943 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -109,7 +109,7 @@ if __name__ == '__main__': If everything is working properly, you can move to the next step . # Step 2: Security Before Flying -Before executing any take off and flight maneuvers, the copters need to make sure that they have a precise enough position estimation. Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. +Before executing any take off and flight manoeuvres, the copters need to make sure that they have a precise enough position estimation. Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. ```python with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(lightCheck) From 9db3de1e78904d026ddea85f6923edd9f566720d Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Thu, 2 Feb 2023 09:21:04 +0200 Subject: [PATCH 513/861] Fix typo in comment --- examples/autonomy/position_commander_demo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py index 49af08705..6908138fc 100644 --- a/examples/autonomy/position_commander_demo.py +++ b/examples/autonomy/position_commander_demo.py @@ -75,7 +75,7 @@ def land_on_elevated_surface(): # 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_lanidng_height, then stoppig motors + # land() will be called on context exit, gradually lowering to default_landing_height, then stopping motors def simple_sequence(): From 94689de0e210bfd211e81b40a968214ef0d94ad2 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Thu, 2 Feb 2023 09:22:16 +0200 Subject: [PATCH 514/861] Use human readible URL in comment --- examples/positioning/bezier_trajectory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/positioning/bezier_trajectory.py b/examples/positioning/bezier_trajectory.py index e2b8ca0cf..486cb5ab9 100644 --- a/examples/positioning/bezier_trajectory.py +++ b/examples/positioning/bezier_trajectory.py @@ -25,7 +25,7 @@ autonomous_sequence_high_level.py example. This code uses Bezier curves of degree 7, that is with 8 control points. -See https://en.wikipedia.org/wiki/B%C3%A9zier_curve +See https://en.wikipedia.org/wiki/Bézier_curve All coordinates are (x, y, z, yaw) """ From cc5916bcce95b54ca60d3988cfac5cb0e90693c5 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Thu, 9 Feb 2023 17:09:06 +0200 Subject: [PATCH 515/861] Update python_api.md --- docs/user-guides/python_api.md | 64 +++++++++++++++++----------------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 6118d2a1a..8136f4417 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -3,14 +3,14 @@ title: The Crazyflie Python API explanation page_id: python_api --- -In order to easily use and control the Crazyflie there\'s an library +In order to easily use and control the Crazyflie there\'s a library made in Python that gives high-level functions and hides the details. This page contains generic information about how to use this library and the API that it implements. If you are interested in more details look in the PyDoc in the code or: -- Communication protocol for +- Communication protocol for [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_log/) or [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_parameters/) - [Automated documentation for Python API](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/) @@ -25,20 +25,20 @@ will be called when the link is opened. The library doesn\'t contain any threads or locks that will keep the application running, it\'s up to the application that is using the library to do this. -There are a few synchronous wrappers for selected classes that creates +There are a few synchronous wrappers for selected classes that create a synchronous API by wrapping the asynchronous classes, see the [Synchronous API section](#synchronous-api) ### Uniform Resource Identifier (URI) -All communication links are identified using an URI build up of the +All communication links are identified using an URI built up of the following: InterfaceType://InterfaceId/InterfaceChannel/InterfaceSpeed -Currently we have *radio*, *serial*, *usb*, *debug*, *udp* interfaces are used. Here are some examples: +Currently we have *radio*, *serial*, *usb*, *debug*, *udp* interfaces. Here are some examples: -- _radio://0/10/2M : Radio interface, USB dongle number 0, radio channel 10 and radio +- _radio://0/10/2M_ : Radio interface, USB dongle number 0, radio channel 10 and radio speed 2 Mbit/s: radio://0/10/2M -- _debug://0/1_ : Debug interface, id 0, channel 1 +- _debug://0/1_ : Debug interface, id 0, channel 1 - _usb://0_ : USB cable to microusb port, id 0 - _serial://ttyAMA0_ : Serial port, id ttyAMA0 - _tcp://aideck-AABBCCDD.local:5000_ : TCP network connection, Name: aideck-AABBCCDD.local, port 5000 @@ -61,13 +61,13 @@ following way: are in the TOC) - After checking that the configuration is valid, set up callbacks for the data in your application and start the log configuration -- Each time the firmware sends data back to the host the callback will - the called with a time-stamp and the data +- Each time the firmware sends data back to the host, the callback will + be called with a time-stamp and the data -There\'s a few limitations that needs to be taken into account: +There\'s are few limitations that need to be taken into account: -- The maximum byte length for a log packet is of 26 bytes. This for - for example allows to log 6 floats and one uint16_t (6*4 + 2 bytes) +- The maximum length for a log packet is 26 bytes. This, for + for example, allows to log 6 floats and one uint16_t (6\*4 + 2 bytes) in a single packet. - The minimum period of a for a log configuration is multiples of 10ms @@ -76,7 +76,7 @@ There\'s a few limitations that needs to be taken into account: The library supports reading and writing parameters at run-time to the firmware. This is intended to be used for data that is not continuously being changed by the firmware, like setting regulation parameters and -reading out if the power-on self-tests passed. Parameters should only +reading out if the power-on self-tests passed. Parameters should only be change in the firmware when being set from the host (cfclient or a cflib script) or during start-up. The library doesn\'t continuously update the parameter values, this @@ -100,10 +100,10 @@ There is an exception for experimental support to change the parameter from with ### Variable and parameter names All names of parameters and log variables use the same structure: -`group.name` +`group.name`. The group should be used to bundle together logical groups, like -everything that has to do with the stabilizer should be in the group +everything that deals with the stabilizer should be in the group `stabilizer`. There\'s a limit of 28 chars in total and here are some examples: @@ -152,7 +152,7 @@ be used. Enable it in the call to `init_drivers()` ## Connection- and link-callbacks -Operations on the link and connection will return directly and will call +Operations on the link and connection will return immediately and will call the following callbacks when events occur: ``` python @@ -199,7 +199,7 @@ available interfaces (currently the debug and radio interface). print "Interface with URI [%s] found and name/comment [%s]" % (i[0], i[1]) ``` -Opening and closing a communication link is doing by using the Crazyflie +Opening and closing a communication link is done by using the Crazyflie object: ``` python @@ -231,7 +231,7 @@ To send a new control set-point use the following: roll = 0.0 pitch = 0.0 yawrate = 0 - thrust = 0 + thrust = 10001 crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) ``` @@ -239,18 +239,18 @@ Thrust is an integer value ranging from 10001 (next to no power) to 60000 (full power). It corresponds to the mean thrust that will be applied to the motors. There is a battery compensation algorithm applied to make the thrust mostly independent of battery voltage. -Roll/pitch are in degree and yawrate in degree/seconds. +Roll/pitch are in degree and yawrate is in degree/seconds. This command will set the attitude controller setpoint for the next -500ms. After 500ms without net setpoint, the Crazyflie will apply a +500ms. After 500ms without next setpoint, the Crazyflie will apply a setpoint with the same thrust but with roll/pitch/yawrate = 0, this -will make the Crazyflie stop accelerate. After 2 seconds without new -setpoint the Crazyflie will cut power to the motors. +will make the Crazyflie stop accelerating. After 2 seconds without new +setpoint the Crazyflie will cut power from the motors. Note that this command implements a motor lock mechanism that is intended to avoid flyaway when connecting a gamepad. You must send one command with thrust = 0 in order to unlock the command. This -unlock procedure needs to be repeated if the watchdog describe above +unlock procedure needs to be repeated if the watchdog described above kicks-in. ### Other commander setpoints sending @@ -261,7 +261,7 @@ If your Crazyflie has a positioning system (Loco, flowdeck, MoCap, Lighthouse), send_hover_setpoint(self, vx, vy, yawrate, zdistance) ``` -Check out the [automated API documentation](/docs/api/cflib/crazyflie/commander.md) for the Crazyflie cflib's commander frame work to find out what other functions you can use. +Check out the [automated API documentation](/docs/api/cflib/crazyflie/commander.md) for the Crazyflie cflib's commander framework to find out what other functions you can use. ## Parameters @@ -274,7 +274,7 @@ functionality should be used when: If this is not the case then the logging framework should be used instead. -To set a parameter you must have the connected to the Crazyflie. A +To set a parameter you must be connected to the Crazyflie. A parameter is set using: ``` python @@ -292,7 +292,7 @@ Crazyflie). ``` python add_update_callback(group, name=None, cb=None) """ - Add a callback for a specific parameter name or group. If not name is specified then + Add a callback for a specific parameter name or group. If name is not specified then all parameters in the group will trigger the callback. This callback will be executed when a new value is read from the Crazyflie. """ @@ -356,11 +356,11 @@ The API to create and get information from LogConfig: add_variable(name, fetch_as=None) """Add a new variable to the configuration. - name - Complete name of the variable in the form group.name + name - Full name of the variable in the form group.name fetch_as - String representation of the type the variable should be fetched as (i.e uint8_t, float, FP16, etc) - If no fetch_as type is supplied, then the stored as type will be used + If no fetch_as type is supplied, then the stored type will be used (i.e the type of the fetched variable is the same as it's stored in the Crazyflie).""" @@ -404,7 +404,7 @@ internal type to transferred type before transfers: - uint8\_t and int8\_t - uint16\_t and int16\_t - uint32\_t and int32\_t -- FP16: 16bit version of floating point, allows to pack more variable +- FP16: 16bit version of floating point, allows to pack more variables in one packet at the expense of precision. The logging cannot be started until your are connected to a Crazyflie: @@ -445,7 +445,7 @@ be added. The synchronous classes are wrappers around the asynchronous API, where the asynchronous calls/callbacks are replaced with blocking calls. The synchronous API does not provide the full flexibility of the asynchronous API, but is useful when writing -small scripts for logging for instance. +small scripts, for logging for instance. The synchronous API uses the python Context manager concept, that is the ```with``` keyword. A resource is allocated when entering a ```with``` section and automatically released when @@ -508,7 +508,7 @@ and SyncCrazyflie instances. To get the log values, iterate the instance. ### MotionCommander -The MotionCommander class is intended to simplify basic autonomous flight, where the motion control is done from the host computer. The Crazyflie takes off +The MotionCommander class is intended to simplify basic autonomous flight, where the motion control is done from the host computer. The Crazyflie takes off and makes when entering the "with" section, and lands when exiting. It has functions for basic movements that are blocking until the motion is finished. @@ -529,7 +529,7 @@ system, all positions are relative. It is mainly intended to be used with a Flow ### PositionHlCommander -The PositionHlCommander is intended to simplify basic autonomous flight, where all the high level commands exists inside the Crazyflie firmware. The Crazyflie takes off +The PositionHlCommander is intended to simplify basic autonomous flight, where all the high level commands exist inside the Crazyflie firmware. The Crazyflie takes off when entering the "with" section, and lands when exiting. It has functions for basic movements that are blocking until the motion is finished. From fc95d953b8d22738ad0ae0f22d3acb47c106818a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 15 Feb 2023 14:18:20 +0100 Subject: [PATCH 516/861] rename fp16 to encoding utils --- cflib/crazyflie/localization.py | 2 +- cflib/utils/encoding.py | 53 +++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 1 deletion(-) create mode 100644 cflib/utils/encoding.py diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 072c6be95..8134956cf 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -32,7 +32,7 @@ from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort from cflib.utils.callbacks import Caller -from cflib.utils.fp16 import fp16_to_float +from cflib.utils.encoding import fp16_to_float __author__ = 'Bitcraze AB' __all__ = ['Localization', 'LocalizationPacket'] diff --git a/cflib/utils/encoding.py b/cflib/utils/encoding.py new file mode 100644 index 000000000..5eb47ee5b --- /dev/null +++ b/cflib/utils/encoding.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2020 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 . +import struct + + +# Code from davidejones at https://gamedev.stackexchange.com/a/28756 +def fp16_to_float(float16): + s = int((float16 >> 15) & 0x00000001) # sign + e = int((float16 >> 10) & 0x0000001f) # exponent + f = int(float16 & 0x000003ff) # fraction + + if e == 0: + if f == 0: + return int(s << 31) + else: + while not (f & 0x00000400): + f <<= 1 + e -= 1 + e += 1 + f &= ~0x00000400 + # print(s,e,f) + elif e == 31: + if f == 0: + return int((s << 31) | 0x7f800000) + else: + return int((s << 31) | 0x7f800000 | (f << 13)) + + e += 127 - 15 + f <<= 13 + result = int((s << 31) | (e << 23) | f) + return struct.unpack('f', struct.pack('I', result))[0] From d5d58050ae0927f643ab5b2a236c525808151508 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 15 Feb 2023 14:44:08 +0100 Subject: [PATCH 517/861] add compression quaterion --- cflib/utils/encoding.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/cflib/utils/encoding.py b/cflib/utils/encoding.py index 5eb47ee5b..ba7b6f042 100644 --- a/cflib/utils/encoding.py +++ b/cflib/utils/encoding.py @@ -24,6 +24,7 @@ # along with this program. If not, see . import struct +from math import sqrt # Code from davidejones at https://gamedev.stackexchange.com/a/28756 def fp16_to_float(float16): @@ -51,3 +52,29 @@ def fp16_to_float(float16): f <<= 13 result = int((s << 31) | (e << 23) | f) return struct.unpack('f', struct.pack('I', result))[0] + + + +# compress a quaternion, see quatcompress.h in firmware +# input: q = [x,y,z,w], output: 32-bit number +def compress_quaternion(qx, qy, qz, qw): + + q = [qx, qy, qz, qw] + + i_largest = 0 + for i in range(1, 4): + if abs(q[i]) > abs(q[i_largest]): + i_largest = i + + negate = q[i_largest] < 0 + + comp = i_largest + m_sqrt_2 = 1.0 / np.sqrt(2) + + for i in range(0,4): + if i != i_largest: + negbit = (q[i] < 0) ^ negate + mag = ((1 << 9) - 1) * (abs(q[i]) / m_sqrt_2) * 0.5 + comp = (comp << 10) | (negbit << 9) | mag + + return comp From 2ce09d5574476066a53d95a519f0d3dda852d8d7 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 15 Feb 2023 14:49:54 +0100 Subject: [PATCH 518/861] Added docs on deprecation --- cflib/crazyflie/mem/trajectory_memory.py | 4 ++-- docs/development/apis_versions_deprecation.md | 11 +++++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) create mode 100644 docs/development/apis_versions_deprecation.md diff --git a/cflib/crazyflie/mem/trajectory_memory.py b/cflib/crazyflie/mem/trajectory_memory.py index e64510204..e4bec3415 100644 --- a/cflib/crazyflie/mem/trajectory_memory.py +++ b/cflib/crazyflie/mem/trajectory_memory.py @@ -169,12 +169,12 @@ def __init__(self, id, type, size, mem_handler): # elements in the same trajectory. self.trajectory = [] - # Deprecated. replaced by self.trajectory + # Deprecated (removed after August 2023). replaced by self.trajectory @property def poly4Ds(self): return self.trajectory - # Deprecated. replaced by self.trajectory + # Deprecated (removed after August 2023). replaced by self.trajectory @poly4Ds.setter def poly4Ds(self, trajectory): self.trajectory = trajectory diff --git a/docs/development/apis_versions_deprecation.md b/docs/development/apis_versions_deprecation.md new file mode 100644 index 000000000..3894ad7b3 --- /dev/null +++ b/docs/development/apis_versions_deprecation.md @@ -0,0 +1,11 @@ +--- +title: APIs, versions and deprecations +page_id: api-deprecation +--- + +There are a number of APIs in the Crazyflie eco system on multiple levels, some might be obvious while others are a bit +more subtle. Some of the APIs are documented and versioned while there is room for improvement in other cases. + +Please see the section in the firmware documentation related to +[APIs and versions](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/development/apis_versions_deprecation/) +for more information. From 5d2dd19289809e5c707f425caa7ed40492c22966 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 15 Feb 2023 15:41:30 +0100 Subject: [PATCH 519/861] Removed use of commander.enHighLevel --- cflib/positioning/position_hl_commander.py | 4 ---- examples/autonomy/autonomous_sequence_high_level.py | 5 ----- .../autonomous_sequence_high_level_compressed.py | 5 ----- examples/qualisys/qualisys_hl_commander.py | 5 ----- examples/swarm/hl-commander-swarm.py | 5 ----- examples/swarm/synchronizedSequence.py | 5 ----- examples/tuning/PID_controller_tuner.py | 1 - test/positioning/test_position_hl_commander.py | 11 ----------- 8 files changed, 41 deletions(-) diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index ab6ad34e6..e11aa8751 100644 --- a/cflib/positioning/position_hl_commander.py +++ b/cflib/positioning/position_hl_commander.py @@ -74,7 +74,6 @@ def __init__(self, crazyflie, self._controller = controller self._activate_controller() - self._activate_high_level_commander() self._hl_commander = self._cf.high_level_commander self._x = x @@ -278,9 +277,6 @@ def get_position(self): """ return self._x, self._y, self._z - def _activate_high_level_commander(self): - self._cf.param.set_value('commander.enHighLevel', '1') - def _activate_controller(self): if self._controller is not None: value = str(self._controller) diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index c827ab947..1880bb767 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -111,10 +111,6 @@ def reset_estimator(cf): wait_for_position_estimator(cf) -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') @@ -161,7 +157,6 @@ def run_sequence(cf, trajectory_id, duration): cf = scf.cf trajectory_id = 1 - activate_high_level_commander(cf) # activate_mellinger_controller(cf) duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index bc8dc84c7..7aea72cba 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -107,10 +107,6 @@ def reset_estimator(cf): wait_for_position_estimator(cf) -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') @@ -158,7 +154,6 @@ def run_sequence(cf, trajectory_id, duration): cf = scf.cf trajectory_id = 1 - activate_high_level_commander(cf) # activate_mellinger_controller(cf) duration = upload_trajectory(cf, trajectory_id, trajectory) print('The sequence is {:.1f} seconds long'.format(duration)) diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/qualisys/qualisys_hl_commander.py index 51fc99364..59eadc4f3 100644 --- a/examples/qualisys/qualisys_hl_commander.py +++ b/examples/qualisys/qualisys_hl_commander.py @@ -240,10 +240,6 @@ def activate_kalman_estimator(cf): cf.param.set_value('locSrv.extQuatStdDev', 0.06) -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') @@ -296,7 +292,6 @@ def run_sequence(cf, trajectory_id, duration): adjust_orientation_sensitivity(cf) activate_kalman_estimator(cf) - activate_high_level_commander(cf) # activate_mellinger_controller(cf) duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index 0adc53a08..346d21aeb 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -37,10 +37,6 @@ from cflib.crazyflie.swarm import Swarm -def activate_high_level_commander(scf): - scf.cf.param.set_value('commander.enHighLevel', '1') - - def activate_mellinger_controller(scf, use_mellinger): controller = 1 if use_mellinger: @@ -87,6 +83,5 @@ def run_shared_sequence(scf): cflib.crtp.init_drivers() factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: - swarm.parallel_safe(activate_high_level_commander) swarm.reset_estimators() swarm.parallel_safe(run_shared_sequence) diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index 1bf3d8556..17661a0e0 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -98,10 +98,6 @@ ] -def activate_high_level_commander(scf): - scf.cf.param.set_value('commander.enHighLevel', '1') - - def activate_mellinger_controller(scf, use_mellinger): controller = 1 if use_mellinger: @@ -185,7 +181,6 @@ def control_thread(): cflib.crtp.init_drivers() factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: - swarm.parallel_safe(activate_high_level_commander) swarm.reset_estimators() print('Starting sequence!') diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 917db4eb4..21a160d23 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -174,7 +174,6 @@ def __init__(self, pid_gui, scf): self.update_scale_info() self.commander = cf.high_level_commander - self.cf.param.set_value('commander.enHighLevel', '1') self.take_off(STANDARD_HEIGHT) def update_scale_info(self): diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index afec5377d..dd5ea929b 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -43,17 +43,6 @@ def setUp(self): self.sut = PositionHlCommander(self.cf_mock) - def test_that_the_hi_level_commander_is_activated_on_creation( - self, sleep_mock): - # Fixture - - # Test - - # Assert - self.param_mock.set_value.assert_has_calls([ - call('commander.enHighLevel', '1') - ]) - def test_that_controller_is_selected_on_creation( self, sleep_mock): # Fixture From 0ec1006e561d155aab0f7e75665bc6def9ca3598 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 15 Feb 2023 16:12:17 +0100 Subject: [PATCH 520/861] add full state setpoint in commander --- cflib/crazyflie/commander.py | 24 ++++++++++++++++++++++++ cflib/utils/encoding.py | 3 ++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index c75128ee5..f616f82cb 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -29,6 +29,7 @@ from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort +from cflib.utils.encoding import compress_quaternion __author__ = 'Bitcraze AB' __all__ = ['Commander'] @@ -37,6 +38,7 @@ TYPE_VELOCITY_WORLD = 1 TYPE_ZDISTANCE = 2 TYPE_HOVER = 5 +TYPE_FULL_STATE = 6 TYPE_POSITION = 7 @@ -136,6 +138,28 @@ def send_hover_setpoint(self, vx, vy, yawrate, zdistance): vx, vy, yawrate, zdistance) self._cf.send_packet(pk) + def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, rollrate, pitchrate, yawrate): + """ + Control mode where the position, velocity, acceleration, orientation, angular + velocity are sent as absolute (world) values. + + x, y, z are in m + vx, vy, vz are in m/s + ax, ay, az are in m/s^2 + qx, qy, qz, qw are the quaternion components of the orientation + rollrate, pitchrate, yawrate are in degrees/s + """ + + pk = CRTPPacket() + pk.port = CRTPPort.COMMANDER_GENERIC + pk.data = struct.pack(' Date: Wed, 15 Feb 2023 16:12:25 +0100 Subject: [PATCH 521/861] make a demo script --- examples/autonomy/full_state_setpoint_demo.py | 165 ++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100644 examples/autonomy/full_state_setpoint_demo.py diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py new file mode 100644 index 000000000..342cc02f9 --- /dev/null +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -0,0 +1,165 @@ +# -*- 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 . +""" +Used for sending full state control setpoints to the Crazyflie +""" +import time + +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.utils import uri_helper +import math + + + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/65/2M/E7E7E7E7F2') + +def quaternion_from_euler(roll, pitch, yaw): + + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + + return q + + +def wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +def reset_estimator(scf): + cf = scf.cf + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + wait_for_position_estimator(cf) + + +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 run_sequence(scf): + cf = scf.cf + + # quaternion from roll pitch yaw + roll = 0.0 + pitch = 0.0 + yaw = 0.0 + q = quaternion_from_euler(roll, pitch, yaw) + + cf.commander.send_full_state_setpoint(0.0,1.0,0.0, + 0.0,0.0,0.0, + 0.0,0.0,0.0, + q[0],q[1],q[2],q[3], + 0.0,0.0,0.0) + + time.sleep(2.0) + + cf.commander.send_full_state_setpoint(0.0,0.2,0.0, + 0.0,0.0,0.0, + 0.0,0.0,0.0, + q[0],q[1],q[2],q[3], + 0.0,0.0,0.0) + + time.sleep(2.0) + + + cf.commander.send_stop_setpoint() + # 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) + run_sequence(scf) From f54ad87cb44649463573ea0a1e76860dd1c554a9 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 09:54:55 +0100 Subject: [PATCH 522/861] fix flake8 and structure symbols --- cflib/crazyflie/commander.py | 2 +- cflib/utils/encoding.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index f616f82cb..dcf5d378b 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -152,7 +152,7 @@ def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC - pk.data = struct.pack(' Date: Thu, 16 Feb 2023 10:00:26 +0100 Subject: [PATCH 523/861] remove unused package --- cflib/utils/encoding.py | 1 - 1 file changed, 1 deletion(-) diff --git a/cflib/utils/encoding.py b/cflib/utils/encoding.py index 2f1939029..43aeff101 100644 --- a/cflib/utils/encoding.py +++ b/cflib/utils/encoding.py @@ -25,7 +25,6 @@ import struct from math import sqrt -import numpy as np # Code from davidejones at https://gamedev.stackexchange.com/a/28756 def fp16_to_float(float16): From dec2cb68769fc9ba6976080d04042220a983e99c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 16 Feb 2023 10:43:45 +0100 Subject: [PATCH 524/861] Removed md file --- docs/development/apis_versions_deprecation.md | 11 ----------- 1 file changed, 11 deletions(-) delete mode 100644 docs/development/apis_versions_deprecation.md diff --git a/docs/development/apis_versions_deprecation.md b/docs/development/apis_versions_deprecation.md deleted file mode 100644 index 3894ad7b3..000000000 --- a/docs/development/apis_versions_deprecation.md +++ /dev/null @@ -1,11 +0,0 @@ ---- -title: APIs, versions and deprecations -page_id: api-deprecation ---- - -There are a number of APIs in the Crazyflie eco system on multiple levels, some might be obvious while others are a bit -more subtle. Some of the APIs are documented and versioned while there is room for improvement in other cases. - -Please see the section in the firmware documentation related to -[APIs and versions](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/development/apis_versions_deprecation/) -for more information. From 5a70fd7ab219ea3a0c310a3512b7aaa1a15beea0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 12:11:09 +0100 Subject: [PATCH 525/861] fix order of quarternion --- examples/autonomy/full_state_setpoint_demo.py | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 342cc02f9..7e89d7ef5 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -49,10 +49,10 @@ def quaternion_from_euler(roll, pitch, yaw): sr = math.sin(roll * 0.5) q = [0] * 4 - q[0] = cy * cp * cr + sy * sp * sr - q[1] = cy * cp * sr - sy * sp * cr - q[2] = sy * cp * sr + cy * sp * cr - q[3] = sy * cp * cr - cy * sp * sr + q[0] = sr * cp * cy - cr * sp * sy + q[1] = cr * sp * cy + sr * cp * sy + q[2] = cr * cp * sy - sr * sp * cy + q[3] = cr * cp * cy + sr * sp * sy return q @@ -133,16 +133,22 @@ def run_sequence(scf): pitch = 0.0 yaw = 0.0 q = quaternion_from_euler(roll, pitch, yaw) - - cf.commander.send_full_state_setpoint(0.0,1.0,0.0, + print('takeoff') + cf.commander.send_full_state_setpoint(0.0,0.0,1.0, 0.0,0.0,0.0, 0.0,0.0,0.0, q[0],q[1],q[2],q[3], 0.0,0.0,0.0) time.sleep(2.0) - - cf.commander.send_full_state_setpoint(0.0,0.2,0.0, + cf.commander.send_full_state_setpoint(0.0,0.0,1.0, + 0.0,0.0,0.0, + 0.0,0.0,0.0, + q[0],q[1],q[2],q[3], + 0.0,0.0,0.0) + time.sleep(2.0) + print('land') + cf.commander.send_full_state_setpoint(0.0,0.0,0.2, 0.0,0.0,0.0, 0.0,0.0,0.0, q[0],q[1],q[2],q[3], From c173effa74e8c5e8d7c999e5095fe12a478d9877 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 16:32:55 +0100 Subject: [PATCH 526/861] more testing, why weird angles in PID? --- cflib/crazyflie/commander.py | 3 +- cflib/utils/encoding.py | 37 +++++++++++++------ examples/autonomy/full_state_setpoint_demo.py | 29 +++++++++++++-- 3 files changed, 53 insertions(+), 16 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index dcf5d378b..f4c8f4a23 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -149,10 +149,9 @@ def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qx, qy, qz, qw are the quaternion components of the orientation rollrate, pitchrate, yawrate are in degrees/s """ - pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC - pk.data = struct.pack('> 30 + sum_squares = 0 + for i in range(3, -1, -1): + if i != i_largest: + mag = comp & mask + negbit = (comp >> 9) & 0x1 + comp = comp >> 10 + q[i] = mag / mask / np.sqrt(2) + if negbit == 1: + q[i] = -q[i] + sum_squares += q[i] * q[i] + q[i_largest] = np.sqrt(1.0 - sum_squares) + return q # compress a quaternion, see quatcompress.h in firmware -# input: q = [x,y,z,w], output: 32-bit number +# input: 32-bit number, output q = [x,y,z,w] def compress_quaternion(qx, qy, qz, qw): - q = [qx, qy, qz, qw] - i_largest = 0 for i in range(1, 4): if abs(q[i]) > abs(q[i_largest]): i_largest = i - negate = q[i_largest] < 0 - comp = i_largest - m_sqrt_2 = 1.0 / sqrt(2) + M_SQRT1_2 = 1.0 / np.sqrt(2) - for i in range(0,4): + comp = i_largest + for i in range(4): if i != i_largest: negbit = (q[i] < 0) ^ negate - mag = ((1 << 9) - 1) * (abs(q[i]) / m_sqrt_2) * 0.5 - comp = (comp << 10) | (negbit << 9) | int(mag) + mag = int(((1 << 9) - 1) * (abs(q[i]) / M_SQRT1_2) + 0.5) + comp = (comp << 10) | (negbit << 9) | mag - return comp + return comp \ No newline at end of file diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 7e89d7ef5..a25bbdc0d 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -25,6 +25,7 @@ Used for sending full state control setpoints to the Crazyflie """ import time +import logging import cflib.crtp from cflib.crazyflie import Crazyflie @@ -33,6 +34,7 @@ from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper import math +from cflib.crazyflie.log import LogConfig @@ -128,10 +130,13 @@ def start_position_printing(scf): def run_sequence(scf): cf = scf.cf + # Set to mellinger controller + # cf.param.set_value('stabilizer.controller', '2') + # quaternion from roll pitch yaw roll = 0.0 pitch = 0.0 - yaw = 0.0 + yaw = 0.0 #20.0*math.pi/180.0 q = quaternion_from_euler(roll, pitch, yaw) print('takeoff') cf.commander.send_full_state_setpoint(0.0,0.0,1.0, @@ -162,10 +167,28 @@ def run_sequence(scf): # 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'])) if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - reset_estimator(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() + + #reset_estimator(scf) run_sequence(scf) From f7d9061a3ae79fe9df022ef5aa61a363b45ac30c Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 17:52:23 +0100 Subject: [PATCH 527/861] add notify setpoint notification command --- cflib/crazyflie/commander.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index c75128ee5..11893266d 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -33,12 +33,16 @@ __author__ = 'Bitcraze AB' __all__ = ['Commander'] +SET_SETPOINT_CHANNEL = 0 +META_COMMAND_CHANNEL = 1 + TYPE_STOP = 0 TYPE_VELOCITY_WORLD = 1 TYPE_ZDISTANCE = 2 TYPE_HOVER = 5 TYPE_POSITION = 7 +TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0 class Commander(): """ @@ -82,6 +86,18 @@ def send_setpoint(self, roll, pitch, yawrate, thrust): pk.data = struct.pack(' Date: Thu, 16 Feb 2023 17:52:35 +0100 Subject: [PATCH 528/861] example for setpoint priority mixing --- examples/commander/setpoint_mixing.py | 80 +++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 examples/commander/setpoint_mixing.py diff --git a/examples/commander/setpoint_mixing.py b/examples/commander/setpoint_mixing.py new file mode 100644 index 000000000..b2fc343cd --- /dev/null +++ b/examples/commander/setpoint_mixing.py @@ -0,0 +1,80 @@ +# -*- 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 . +""" +Simple example that connects to one crazyflie (check the address at the top +and update it to your crazyflie address) and a combination of high level +and low level commands to make the crazyflie move. + +The high level commands (low priority) are used to take off, land and go to a specific location +The low level commands (high priority) are used to make the crazyflie move in a specific direction +The landing command from the high level commander will first need to follow a setpoint priority + relaxation before being able to land. +""" + + +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie + +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + +def run_sequence(cf): + commander_high_level = cf.high_level_commander + commander_low_level = cf.commander + standard_height = 1.0 + vel_max = 0.5 + + print('take off') + commander_high_level.takeoff(standard_height, 2.0) + time.sleep(3.0) + print('go forward high level commander') + commander_high_level.go_to(0.0, 1.0, standard_height, 0.0, 2.0) + time.sleep(1.0) + print('take over low level commander, which has higher priority') + commander_low_level.send_hover_setpoint(vel_max, 0.0, 0.0, standard_height) + time.sleep(2.0) + print('second setpoint to make sure that it does not stop') + commander_low_level.send_hover_setpoint(-vel_max, 0.0, 0.0, standard_height) + time.sleep(2.0) + print('Stop on the spot') + commander_low_level.send_hover_setpoint(0.0, 0.0, 0.0, standard_height) + time.sleep(1.0) + print('lower priority of low level commander') + commander_low_level.send_notify_setpoint_stop() + time.sleep(0.1) + print('land again with high level commander') + commander_high_level.land(0.0, 2.0) + time.sleep(2) + commander_high_level.stop() + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + run_sequence(cf) From 67e27368e160173d0a7797d4076068315065153a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 17:53:44 +0100 Subject: [PATCH 529/861] bump date --- examples/commander/setpoint_mixing.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/commander/setpoint_mixing.py b/examples/commander/setpoint_mixing.py index b2fc343cd..b64f0e5f5 100644 --- a/examples/commander/setpoint_mixing.py +++ b/examples/commander/setpoint_mixing.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2018 Bitcraze AB +# 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 From 311c14091bdab11a0f43d38bd5024d741081ab36 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Feb 2023 13:16:59 +0100 Subject: [PATCH 530/861] remove example script and fix flake8 --- cflib/crazyflie/commander.py | 4 +- examples/commander/setpoint_mixing.py | 80 --------------------------- 2 files changed, 2 insertions(+), 82 deletions(-) delete mode 100644 examples/commander/setpoint_mixing.py diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 11893266d..38253ff19 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -88,8 +88,8 @@ def send_setpoint(self, roll, pitch, yawrate, thrust): def send_notify_setpoint_stop(self, remain_valid_milliseconds=0): """ - Sends a packet so that the priority of the current setpoint to the lowest non-disabled value, - so any new setpoint regardless of source will overwrite it. + Sends a packet so that the priority of the current setpoint to the lowest non-disabled value, + so any new setpoint regardless of source will overwrite it. """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC diff --git a/examples/commander/setpoint_mixing.py b/examples/commander/setpoint_mixing.py deleted file mode 100644 index b64f0e5f5..000000000 --- a/examples/commander/setpoint_mixing.py +++ /dev/null @@ -1,80 +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 example that connects to one crazyflie (check the address at the top -and update it to your crazyflie address) and a combination of high level -and low level commands to make the crazyflie move. - -The high level commands (low priority) are used to take off, land and go to a specific location -The low level commands (high priority) are used to make the crazyflie move in a specific direction -The landing command from the high level commander will first need to follow a setpoint priority - relaxation before being able to land. -""" - - -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie - -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -def run_sequence(cf): - commander_high_level = cf.high_level_commander - commander_low_level = cf.commander - standard_height = 1.0 - vel_max = 0.5 - - print('take off') - commander_high_level.takeoff(standard_height, 2.0) - time.sleep(3.0) - print('go forward high level commander') - commander_high_level.go_to(0.0, 1.0, standard_height, 0.0, 2.0) - time.sleep(1.0) - print('take over low level commander, which has higher priority') - commander_low_level.send_hover_setpoint(vel_max, 0.0, 0.0, standard_height) - time.sleep(2.0) - print('second setpoint to make sure that it does not stop') - commander_low_level.send_hover_setpoint(-vel_max, 0.0, 0.0, standard_height) - time.sleep(2.0) - print('Stop on the spot') - commander_low_level.send_hover_setpoint(0.0, 0.0, 0.0, standard_height) - time.sleep(1.0) - print('lower priority of low level commander') - commander_low_level.send_notify_setpoint_stop() - time.sleep(0.1) - print('land again with high level commander') - commander_high_level.land(0.0, 2.0) - time.sleep(2) - commander_high_level.stop() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - run_sequence(cf) From 716e1d9fdc8dd5e2632eb11d7b3191c8ebbced09 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Feb 2023 13:30:18 +0100 Subject: [PATCH 531/861] remove whitespace --- cflib/crazyflie/commander.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 38253ff19..79b5c93e7 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -89,7 +89,7 @@ def send_setpoint(self, roll, pitch, yawrate, thrust): def send_notify_setpoint_stop(self, remain_valid_milliseconds=0): """ Sends a packet so that the priority of the current setpoint to the lowest non-disabled value, - so any new setpoint regardless of source will overwrite it. + so any new setpoint regardless of source will overwrite it. """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC From 3a4dff96bb063685acb6eb776c77da0d9fe2a6e0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Feb 2023 14:55:01 +0100 Subject: [PATCH 532/861] fix autopep8 --- cflib/crazyflie/commander.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 79b5c93e7..c8dc46e9e 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -44,6 +44,7 @@ TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0 + class Commander(): """ Used for sending control setpoints to the Crazyflie From 9c9b3ec6d452728dd87c13db5d2936e64d40fb13 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Feb 2023 15:18:08 +0100 Subject: [PATCH 533/861] Update version to 0.1.22 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index c01e5a98b..118ad7a63 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.21', + version='0.1.22', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From 3124c66f8d3dd77c5bc587e1abe57f728c0ee3b8 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 28 Feb 2023 09:57:18 +0100 Subject: [PATCH 534/861] Updated docs headers --- docs/user-guides/sbs_connect_log_param.md | 46 +++++++++++------------ docs/user-guides/sbs_motion_commander.md | 22 +++++------ docs/user-guides/sbs_swarm_interface.md | 18 ++++----- 3 files changed, 43 insertions(+), 43 deletions(-) diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index c3a27d6f4..3fc365e2f 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -6,7 +6,7 @@ redirect: /step-by-step/connect_log_param/ On this step by step guide we will show you how to connect to your Crazyflie through the Crazyflie python library by a python script. This is the starting point for developing for the Crazyflie for off-board enabled flight. -# Prerequisites +## Prerequisites We will assume that you already know this before you start with the tutorial: @@ -15,7 +15,7 @@ We will assume that you already know this before you start with the tutorial: * Able to connect the crazyflie to the CFClient and look at the log tabs and parameters (here is a [userguide](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/)). -## Install the cflib +### Install the cflib Make sure that you have [python3](https://www.python.org), which should contain pip3. In a terminal please write the following: @@ -23,9 +23,9 @@ Make sure that you have [python3](https://www.python.org), which should contain This should have been installed if you installed the cfclient already (on a linux system), but it is always good to double check this :) -# Step 1. Connecting with the crazyflie +## Step 1. Connecting with the crazyflie -## Begin the python script +### Begin the python script Open up a python script anywhere that is convenient for you. We use Visual Studio code ourselves but you can use the Python editor IDLE3 if you want. @@ -57,7 +57,7 @@ from a Crazyflie. class. It handles the asynchronous nature of the Crazyflie API and turns it into blocking function. -## URI of the Crazyflie +### URI of the Crazyflie After these imports, start the script with: @@ -68,7 +68,7 @@ uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') This is the radio uri of the crazyflie, it can be set by setting the environment variable `CFLIB_URI`, if not set it uses the default. It should be probably fine but if you do not know what the uri of your Crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) -## Main +### Main Write the following in the script: ```python @@ -83,7 +83,7 @@ if __name__ == '__main__': The `syncCrazyflie` will create a synchronous Crazyflie instance with the specified link_uri. As you can see we are currently calling an non-existing function, so you will need to make that function first before you run the script. -## Function for connecting with the crazyflie +### Function for connecting with the crazyflie Start a function above the main function (but below the URI) which you call simple connect: @@ -97,7 +97,7 @@ def simple_connect(): -## Run the script +### Run the script Now run the script in your terminal: @@ -144,7 +144,7 @@ if __name__ == '__main__': simple_connect() ``` -# Step 2a. Logging (synchronous) +## Step 2a. Logging (synchronous) @@ -152,7 +152,7 @@ Alright, now taking a step up. We will now add to the script means to read out l -## More imports +### More imports Now we need to add several imports on the top of the script connect_log_param.py @@ -174,7 +174,7 @@ Also add the following underneath URI logging.basicConfig(level=logging.ERROR) ``` -## Add logging config +### Add logging config Now we are going to define the logging configuration. So add `lg_stab` in the `__main__` function : ```python @@ -194,7 +194,7 @@ Now we are going to define the logging configuration. So add `lg_stab` in the `_ Here you will add the logs variables you would like to read out. If you are unsure how your variable is called, this can be checked by connecting to Crazyflie to the cfclient and look at the log TOC tab. If the variables don't match, you get a `KeyError` (more on that later.) -## Make the logging function +### Make the logging function Use the same connect_log_param.py script, and add the following function above `simple_connect()` and underneath URI: ```python @@ -219,7 +219,7 @@ Now the logging instances will be inserted by adding the following after you con ``` -## Test the script: +### Test the script: First change the `simple_connect()` in _main_ in `simple_log(scf, lg_stab)`. Now run the script (`python3 connect_log_param.py`) like before. @@ -280,13 +280,13 @@ if __name__ == '__main__': ``` -# Step 2b. Logging (Asynchronous) +## Step 2b. Logging (Asynchronous) The logging we have showed you before was in a synchronous manner, so it reads out the logging in the function directly in the loop. Eventhough the SyncLogger does not take much time in general, for application purposes it might be preferred to receive the logging variables separately from this function, in a callback independently of the main loop-rate. Here we will explain how this asynchronous logging can be set up in the script. -## Start a new function +### Start a new function Above `def simple_log(..)`, begin a new function: @@ -300,7 +300,7 @@ Here you add the logging configuration to to the logging framework of the Crazyf `KeyError: 'Variable not.real not in TOC'` -## Add a callback function +### Add a callback function First we will make the callback function like follows: ```python @@ -322,7 +322,7 @@ Then the log configuration would need to be started manually, and then stopped a logconf.stop() ``` -# Run the script +## Run the script Make sure to replace the `simple_log(...)` to `simple_log_async(...)` in the `__main__` function. Run the script with `python3 connect_log_param.py` in a terminal and you should see several messages of the following: @@ -374,7 +374,7 @@ if __name__ == '__main__': simple_log_async(scf, lg_stab) ``` -# Step 3. Parameters +## Step 3. Parameters Next to logging variables, it is possible to read and set parameters settings. That can be done within the cfclient, but here we will look at how we can change the state estimator method in the python script. @@ -385,7 +385,7 @@ First add the group parameter name just above `with SyncCrazyflie(...` in `__mai name = "estimator" ``` -## Start the function +### Start the function Start the following function above `def log_stab_callback(...)`: @@ -395,7 +395,7 @@ def simple_param_async(scf, groupstr, namestr): full_name = groupstr+ "." +namestr ``` -## Add the callback for parameters +### Add the callback for parameters In a similar way as in the previous section for the Async logging, we are going to make a callback function for the parameters. Add the callback function above `def simple_param_async`: @@ -417,7 +417,7 @@ If you would like to test out the script now already, replace `simple_log_async( `The crazyflie has parameter stabilizer.estimator set at number: 1` -## Set a parameter +### Set a parameter Now we will set a variable in a parameter. Add the following to the `simple_param_async(...)` function: @@ -434,7 +434,7 @@ What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) para `AttributeError: cpu.flash is read-only!` -## Finishing and running the script +### Finishing and running the script It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physically restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. @@ -517,7 +517,7 @@ The crazyflie has parameter stabilizer.estimator set at number: 1 You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. -# What's next? +## What's next? Now you know how to connect to the Crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the Crazyflie fly by giving it setpoints which is one step closer to making your own application! diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index f1a49ce85..ab8f72b42 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -5,7 +5,7 @@ page_id: sbs_motion_commander Here we will go through step-by-step how to make your crazyflie move based on a motion script. For the first part of this tutorial, you just need the crazyflie and the flowdeck. For the second part, it would be handy to have the multiranger present. -# Prerequisites +## Prerequisites We will assume that you already know this before you start with the tutorial: @@ -13,7 +13,7 @@ We will assume that you already know this before you start with the tutorial: * Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) and [the connecting, logging and parameters tutorial](/docs/user-guides/sbs_connect_log_param.md). -# Get the script started +## Get the script started Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up an new python script called `motion_flying.py`. First you will start by adding the following import to the script: @@ -48,7 +48,7 @@ This probably all looks pretty familiar, except for one thing line, namely: This imports the motion commander, which is pretty much a wrapper around the position setpoint frame work of the crazyflie. You probably have unknowingly experienced this a when trying out the assist modes in this [tutorial with the flowdeck in the cfclient](https://www.bitcraze.io/documentation/tutorials/getting-started-with-flow-deck/) -# Step 1: Security before flying +## Step 1: Security before flying Since this tutorial won't be a table top tutorial like last time, but an actual flying one, we need to put some securities in place. The flowdeck or any other positioning deck that you are using, should be correctly attached to the crazyflie. If it is not, it will try to fly anyway without a good position estimate and for sure is going to crash. @@ -125,7 +125,7 @@ if __name__ == '__main__': ``` -# Step 2: Take off function +## Step 2: Take off function So now we are going to start up the SyncCrazyflie and start a function in the `__main__` function: @@ -153,7 +153,7 @@ If you run the python script, you will see the crazyflie connect and immediately The reason for the crazyflie to immediately take off, is that the motion commander if intialized with a take off function that will already start sending position setpoints to the crazyflie. Once the script goes out of the instance, the motion commander instance will close with a land function. -## Changing the height +### Changing the height Currently the motion commander had 0.3 meters height as default but this can of course be changed. @@ -227,7 +227,7 @@ if __name__ == '__main__': ``` -# Step 3 Go Forward, Turn and Go back +## Step 3 Go Forward, Turn and Go back So now we know how to take off, so the second step is to move in a direction! Start a new function above `def take_off_simple(scf)`: @@ -315,7 +315,7 @@ if __name__ == '__main__': move_linear_simple(scf) ``` -# Step 4: Logging while flying +## Step 4: Logging while flying When the motion commander commands have been executed, the script stops and the crazyflie lands... however that is a bit boring. Maybe you would like for it to keep flying and responding certain elements in the mean time! @@ -429,11 +429,11 @@ if __name__ == '__main__': ``` -# Step 5: Combine logging and motion commander +## Step 5: Combine logging and motion commander There is a reason why we put the position_estimate to catch the positions from the log, since we would like to now do something with it! -## Back and forth with limits +### Back and forth with limits Lets start with a new function above `move_linear_simple(scf)`: ```python @@ -539,7 +539,7 @@ if __name__ == '__main__': logconf.stop() ``` -## Bouncing in a bounding box +### Bouncing in a bounding box Let's take it up a notch! Replace the content in the while loop with the following: ```python @@ -669,6 +669,6 @@ if __name__ == '__main__': You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. -# What is next ? +## What is next ? Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index 4a04f6943..6cd098f37 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -5,7 +5,7 @@ page_id: sbs_swarm_interface Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib.For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a global positioning system (Lighthouse, Loco or MoCap) that is able to provide data for the position estimation of the crazyflies. You can also use the Flowdeck but keep in mind that you should command relative movements of each Crazyflie and due to its nature it may lead to accumulative errors and unexpected behavior over time. -# Prerequisites +## Prerequisites We will assume that you already know this before you start with the tutorial: @@ -14,7 +14,7 @@ We will assume that you already know this before you start with the tutorial: * Read the [high level commander](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/high_level_commander/), [swarm](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/swarm/) and [SyncCrazyflie](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/crazyflie/syncCrazyflie/) documentation . -# Get the script started +## Get the script started Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up a new python script called `swarm_rectangle.py`. First you will start by adding the following import to the script: @@ -43,7 +43,7 @@ This will import all the necessary modules and open the necessary links for comm The radio addresses of the copters are defined in the `uris` list and you can add more if you want to. -# Step 1: Light Check +## Step 1: Light Check In order to verify everything is setup and working properly a light check will be performed. During this check, all the copters will light up red for a short period of time and then return to normal. This is achieved by setting the parameter `led.bitmask` to 255 which results to all the LED's of each copter light up simultaneously. @@ -108,7 +108,7 @@ if __name__ == '__main__': ``` If everything is working properly, you can move to the next step . -# Step 2: Security Before Flying +## Step 2: Security Before Flying Before executing any take off and flight manoeuvres, the copters need to make sure that they have a precise enough position estimation. Otherwise it will take off anyway and it is very likely to crash. This is done through `reset_estimators()` by resetting the internal position estimator of each copter and waiting until the variance of the position estimation drops below a certain threshold. ```python with Swarm(uris, factory=factory) as swarm: @@ -116,7 +116,7 @@ with Swarm(uris, factory=factory) as swarm: swarm.reset_estimators() ``` -# Step 3: Taking off and Landing Sequentially +## Step 3: Taking off and Landing Sequentially Now we are going to execute the fist take off and landing using the high level commander. The high level commander (more information [here](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/sensor-to-control/commanders_setpoints/#high-level-commander)) is a class that handles all the high level commands like takeoff, landing, hover, go to position and others. The high level commander is usually preferred since it needs less communication and provides more autonomy on the Crazyflie. It is always on, but just in a lower priority so you just need to execute the take off and land commands through the below functions: ```python def take_off(scf): @@ -203,7 +203,7 @@ if __name__ == '__main__': ``` After executing it you will see all copters performing the light check and then each copter take off , hover and land. This process is repeated for all copters in the swarm. -# Step 4: Taking off and Landing in Sync +## Step 4: Taking off and Landing in Sync If you want to take off and land in sync, you can use the `parallel_safe()` method of the `Swarm` class. ```python @@ -212,7 +212,7 @@ If you want to take off and land in sync, you can use the `parallel_safe()` meth Now the same action is happening but for all the copters in parallel. -# Step 5: Performing a square shape flight +## Step 5: Performing a square shape flight To make the swarm fly in a square shape, we will use the go_to method of the high level commander. Each copter executes 4 relative movements to its current position covering a square shape. ```python @@ -292,7 +292,7 @@ if __name__ == '__main__': swarm.parallel_safe(land) ``` -# Step 6: Performing a flight with different arguments +## Step 6: Performing a flight with different arguments You can also feed different arguments to each Crazyflie in the swarm. This can be done by providing a dictionary `args_dict` to the `parallel_safe()`,`parallel()` and `sequential()` methods following the below format. ```python @@ -458,5 +458,5 @@ if __name__ == '__main__': You’re done! The full code of this tutorial can be found in the `example/step-by-step/` folder. -# What is next ? +## What is next ? Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality, simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications! For more examples and inspiration on the Swarm functionality, you can check out the `examples/swarm/` folder of the cflib. From cab23c89c3f060e7cd82b8981571005ac49cbc44 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 7 Mar 2023 16:06:44 +0100 Subject: [PATCH 535/861] Removed obsolete bezier script --- examples/positioning/bezier_trajectory.py | 423 ---------------------- 1 file changed, 423 deletions(-) delete mode 100644 examples/positioning/bezier_trajectory.py diff --git a/examples/positioning/bezier_trajectory.py b/examples/positioning/bezier_trajectory.py deleted file mode 100644 index 486cb5ab9..000000000 --- a/examples/positioning/bezier_trajectory.py +++ /dev/null @@ -1,423 +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 generate trajectories for the High Level commander using -Bezier curves. The output from this script is intended to be pasted into the -autonomous_sequence_high_level.py example. - -This code uses Bezier curves of degree 7, that is with 8 control points. -See https://en.wikipedia.org/wiki/Bézier_curve - -All coordinates are (x, y, z, yaw) -""" -import math - -import numpy as np - - -class Node: - """ - A node represents the connection point between two Bezier curves - (called Segments). - It holds 4 control points for each curve and the positions of the control - points are set to join the curves with continuity in c0, c1, c2, c3. - See https://www.cl.cam.ac.uk/teaching/2000/AGraphHCI/SMEG/node3.html - - The control points are named - p4, p5, p6 and p7 for the tail of the first curve - q0, q1, q2, q3 for the head of the second curve - """ - - def __init__(self, q0, q1=None, q2=None, q3=None): - """ - Create a Node. Pass in control points to define the shape of the - two segments that share the Node. The control points are for the - second segment, that is the four first control points of the Bezier - curve after the node. The control points for the Bezier curve before - the node are calculated from the existing control points. - The control points are for scale = 1, that is if the Bezier curve - after the node has scale = 1 it will have exactly these handles. If the - curve after the node has a different scale the handles will be moved - accordingly when the Segment is created. - - q0 is required, the other points are optional. - if q1 is missing it will be set to generate no velocity in q0. - If q2 is missing it will be set to generate no acceleration in q0. - If q3 is missing it will be set to generate no jerk in q0. - - If only q0 is set, the node will represent a point where the Crazyflie - has no velocity. Good for starting and stopping. - - To get a fluid motion between segments, q1 must be set. - - :param q0: The position of the node - :param q1: The position of the first control point - :param q2: The position of the second control point - :param q3: The position of the third control point - """ - self._control_points = np.zeros([2, 4, 4]) - - q0 = np.array(q0) - - if q1 is None: - q1 = q0 - else: - q1 = np.array(q1) - # print('q1 generated:', q1) - - d = q1 - q0 - - if q2 is None: - q2 = q0 + 2 * d - # print('q2 generated:', q2) - else: - q2 = np.array(q2) - - e = 3 * q2 - 2 * q0 - 6 * d - - if q3 is None: - q3 = e + 3 * d - # print('q3 generated:', q3) - else: - q3 = np.array(q3) - - p7 = q0 - p6 = q1 - 2 * d - p5 = q2 - 4 * d - p4 = 2 * e - q3 - - self._control_points[0][0] = q0 - self._control_points[0][1] = q1 - self._control_points[0][2] = q2 - self._control_points[0][3] = q3 - - self._control_points[1][3] = p7 - self._control_points[1][2] = p6 - self._control_points[1][1] = p5 - self._control_points[1][0] = p4 - - def get_head_points(self): - return self._control_points[0] - - def get_tail_points(self): - return self._control_points[1] - - def draw_unscaled_controlpoints(self, visualizer): - color = (0.8, 0.8, 0.8) - for p in self._control_points[0]: - visualizer.marker(p[0:3], color=color) - for p in self._control_points[1]: - visualizer.marker(p[0:3], color=color) - - def print(self): - print('Node ---') - print('Tail:') - for c in self._control_points[1]: - print(c) - print('Head:') - for c in self._control_points[0]: - print(c) - - -class Segment: - """ - A Segment represents a Bezier curve of degree 7. It uses two Nodes to - define the shape. The scaling of the segment will move the handles compared - to the Node to maintain continuous position, velocity, acceleration and - jerk through the Node. - A Segment can generate a polynomial that is compatible with the High Level - Commander, either in python to be sent to the Crazyflie, or as C code to be - used in firmware. - A Segment can also be rendered in Vispy. - """ - - def __init__(self, head_node, tail_node, scale): - self._scale = scale - - unscaled_points = np.concatenate( - [head_node.get_head_points(), tail_node.get_tail_points()]) - - self._points = self._scale_control_points(unscaled_points, self._scale) - - polys = self._convert_to_polys() - self._polys = self._stretch_polys(polys, self._scale) - - self._vel = self._deriv(self._polys) - self._acc = self._deriv(self._vel) - self._jerk = self._deriv(self._acc) - - def _convert_to_polys(self): - n = len(self._points) - 1 - result = np.zeros([4, 8]) - - for d in range(4): - for j in range(n + 1): - s = 0.0 - for i in range(j + 1): - s += ((-1) ** (i + j)) * self._points[i][d] / ( - math.factorial(i) * math.factorial(j - i)) - - c = s * math.factorial(n) / math.factorial(n - j) - result[d][j] = c - - return result - - def draw_trajectory(self, visualizer): - self._draw(self._polys, 'black', visualizer) - - def draw_vel(self, visualizer): - self._draw(self._vel, 'green', visualizer) - - def draw_acc(self, visualizer): - self._draw(self._acc, 'red', visualizer) - - def draw_jerk(self, visualizer): - self._draw(self._jerk, 'blue', visualizer) - - def draw_control_points(self, visualizer): - for p in self._points: - visualizer.marker(p[0:3]) - - def _draw(self, polys, color, visualizer): - step = self._scale / 32 - prev = None - for t in np.arange(0.0, self._scale + step, step): - p = self._eval_xyz(polys, t) - - if prev is not None: - visualizer.line(p, prev, color=color) - - prev = p - - def velocity(self, t): - return self._eval_xyz(self._vel, t) - - def acceleration(self, t): - return self._eval_xyz(self._acc, t) - - def jerk(self, t): - return self._eval_xyz(self._jerk, t) - - def _deriv(self, p): - result = [] - for i in range(3): - result.append([ - 1 * p[i][1], - 2 * p[i][2], - 3 * p[i][3], - 4 * p[i][4], - 5 * p[i][5], - 6 * p[i][6], - 7 * p[i][7], - 0 - ]) - - return result - - def _eval(self, p, t): - result = 0 - for part in range(8): - result += p[part] * (t ** part) - return result - - def _eval_xyz(self, p, t): - return np.array( - [self._eval(p[0], t), self._eval(p[1], t), self._eval(p[2], t)]) - - def print_poly_python(self): - s = ' [' + str(self._scale) + ', ' - - for axis in range(4): - for d in range(8): - s += str(self._polys[axis][d]) + ', ' - - s += '], # noqa' - print(s) - - def print_poly_c(self): - s = '' - - for axis in range(4): - for d in range(8): - s += str(self._polys[axis][d]) + ', ' - - s += str(self._scale) - print(s) - - def print_points(self): - print(self._points) - - def print_peak_vals(self): - peak_v = 0.0 - peak_a = 0.0 - peak_j = 0.0 - - step = 0.05 - for t in np.arange(0.0, self._scale + step, step): - peak_v = max(peak_v, np.linalg.norm(self._eval_xyz(self._vel, t))) - peak_a = max(peak_a, np.linalg.norm(self._eval_xyz(self._acc, t))) - peak_j = max(peak_j, np.linalg.norm(self._eval_xyz(self._jerk, t))) - - print('Peak v:', peak_v, 'a:', peak_a, 'j:', peak_j) - - def _stretch_polys(self, polys, time): - result = np.zeros([4, 8]) - - recip = 1.0 / time - - for p in range(4): - scale = 1.0 - for t in range(8): - result[p][t] = polys[p][t] * scale - scale *= recip - - return result - - def _scale_control_points(self, unscaled_points, scale): - s = scale - l_s = 1 - s - p = unscaled_points - - result = [None] * 8 - - result[0] = p[0] - result[1] = l_s * p[0] + s * p[1] - result[2] = l_s ** 2 * p[0] + 2 * l_s * s * p[1] + s ** 2 * p[2] - result[3] = l_s ** 3 * p[0] + 3 * l_s ** 2 * s * p[ - 1] + 3 * l_s * s ** 2 * p[2] + s ** 3 * p[3] - result[4] = l_s ** 3 * p[7] + 3 * l_s ** 2 * s * p[ - 6] + 3 * l_s * s ** 2 * p[5] + s ** 3 * p[4] - result[5] = l_s ** 2 * p[7] + 2 * l_s * s * p[6] + s ** 2 * p[5] - result[6] = l_s * p[7] + s * p[6] - result[7] = p[7] - - return result - - -class Visualizer: - def __init__(self): - self.canvas = scene.SceneCanvas(keys='interactive', size=(800, 600), - show=True) - view = self.canvas.central_widget.add_view() - view.bgcolor = '#ffffff' - view.camera = TurntableCamera(fov=10.0, distance=40.0, up='+z', - center=(0.0, 0.0, 1.0)) - XYZAxis(parent=view.scene) - self.scene = view.scene - - def marker(self, pos, color='black', size=8): - Markers(pos=np.array(pos, ndmin=2), face_color=color, - parent=self.scene, size=size) - - def lines(self, points, color='black'): - LinePlot(points, color=color, parent=self.scene) - - def line(self, a, b, color='black'): - self.lines([a, b], color) - - def run(self): - self.canvas.app.run() - - -segment_time = 2 -z = 1 -yaw = 0 - -segments = [] - -# Nodes with one control point has not velocity, this is similar to calling -# goto in the High-level commander - -n0 = Node((0, 0, z, yaw)) -n1 = Node((1, 0, z, yaw)) -n2 = Node((1, 1, z, yaw)) -n3 = Node((0, 1, z, yaw)) - -segments.append(Segment(n0, n1, segment_time)) -segments.append(Segment(n1, n2, segment_time)) -segments.append(Segment(n2, n3, segment_time)) -segments.append(Segment(n3, n0, segment_time)) - - -# By setting the q1 control point we get velocity through the nodes -# Increase d to 0.7 to get some more action -d = 0.1 - -n5 = Node((1, 0, z, yaw), q1=(1 + d, 0 + d, z, yaw)) -n6 = Node((1, 1, z, yaw), q1=(1 - d, 1 + d, z, yaw)) -n7 = Node((0, 1, z, yaw), q1=(0 - d, 1 - d, z, yaw)) - -segments.append(Segment(n0, n5, segment_time)) -segments.append(Segment(n5, n6, segment_time)) -segments.append(Segment(n6, n7, segment_time)) -segments.append(Segment(n7, n0, segment_time)) - - -# When setting q2 we can also control acceleration and get more action. -# Yaw also adds to the fun. - -d2 = 0.2 -dyaw = 2 -f = -0.3 - -n8 = Node( - (1, 0, z, yaw), - q1=(1 + d2, 0 + d2, z, yaw), - q2=(1 + 2 * d2, 0 + 2 * d2 + 0*f * d2, 1, yaw)) -n9 = Node( - (1, 1, z, yaw + dyaw), - q1=(1 - d2, 1 + d2, z, yaw + dyaw), - q2=(1 - 2 * d2 + f * d2, 1 + 2 * d2 + f * d2, 1, yaw + dyaw)) -n10 = Node( - (0, 1, z, yaw - dyaw), - q1=(0 - d2, 1 - d2, z, yaw - dyaw), - q2=(0 - 2 * d2, 1 - 2 * d2, 1, yaw - dyaw)) - -segments.append(Segment(n0, n8, segment_time)) -segments.append(Segment(n8, n9, segment_time)) -segments.append(Segment(n9, n10, segment_time)) -segments.append(Segment(n10, n0, segment_time)) - - -print('Paste this code into the autonomous_sequence_high_level.py example to ' - 'see it fly') -for s in segments: - s.print_poly_python() - - -# Enable this if you have Vispy installed and want a visualization of the -# trajectory -if False: - # Import here to avoid problems for users that do not have Vispy - from vispy import scene - from vispy.scene import XYZAxis, LinePlot, TurntableCamera, Markers - - visualizer = Visualizer() - for s in segments: - s.draw_trajectory(visualizer) - # s.draw_vel(visualizer) - # s.draw_control_points(visualizer) - - for n in [n0, n1, n2, n3, n5, n6, n7, n8, n9, n10]: - n.draw_unscaled_controlpoints(visualizer) - - visualizer.run() From 789d0dc6341c98bece1510b4f778e0efab2bef09 Mon Sep 17 00:00:00 2001 From: hmllr Date: Tue, 7 Mar 2023 15:31:04 +0100 Subject: [PATCH 536/861] circling square demo with colors First turn off LEDs, then take off as first thing in swarm cmd to avoid delays --- examples/autonomy/circling_square_demo.py | 233 ++++++++++++++++++++++ 1 file changed, 233 insertions(+) create mode 100644 examples/autonomy/circling_square_demo.py diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py new file mode 100644 index 000000000..e1487578a --- /dev/null +++ b/examples/autonomy/circling_square_demo.py @@ -0,0 +1,233 @@ +# -*- 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 . +""" +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 import Crazyflie +from cflib.crazyflie.high_level_commander import HighLevelCommander +from cflib.crazyflie.log import LogConfig +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.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper +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 wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +def reset_estimator(cf): + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + wait_for_position_estimator(cf) + + +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): + 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*3.14*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:[3.14/4,r2], + URI3:[3.14/2,r1], + URI4:[3.14/4*3,r2], + URI5:[3.14,r1], + URI6:[3.14/4*5,r2], + URI7:[3.14/4*6,r1], + URI8:[3.14/4*7,r2]} + + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + 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) From e3dec6825889c316a36e6089887e4e42d063630e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 16 Mar 2023 14:37:00 +0100 Subject: [PATCH 537/861] Add log of python version --- .github/workflows/CI.yml | 3 +++ .github/workflows/dependency_check.yml | 6 ++++++ 2 files changed, 9 insertions(+) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index f5658f071..a3c5e6769 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -18,6 +18,9 @@ jobs: - name: Checkout repo uses: actions/checkout@v3 + - name: python version + run: python --version + - name: Build run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 30271539d..ee23767e7 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -14,6 +14,9 @@ jobs: steps: - uses: actions/checkout@v3 + - name: python version + run: python --version + - name: install lib run: pip install -e . @@ -25,5 +28,8 @@ jobs: steps: - uses: actions/checkout@v3 + - name: python version + run: python --version + - name: install lib run: pip install -e . From 99dccc7eed82eae31715d47b07c487592f073b28 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 16 Mar 2023 14:40:41 +0100 Subject: [PATCH 538/861] Removed version --- .github/workflows/CI.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index a3c5e6769..f5658f071 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -18,9 +18,6 @@ jobs: - name: Checkout repo uses: actions/checkout@v3 - - name: python version - run: python --version - - name: Build run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build From 48f8a1228935a72684599353de3aaef0683d1f65 Mon Sep 17 00:00:00 2001 From: hmllr Date: Tue, 21 Mar 2023 14:42:07 +0100 Subject: [PATCH 539/861] Adds cpx versioning --- cflib/cpx/__init__.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index cda4ec1fd..a2a2c7a2a 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -24,12 +24,15 @@ # along with this program. If not, see . """ CPX Router and discovery""" import enum +import logging import queue import struct import threading __author__ = 'Bitcraze AB' __all__ = ['CPXRouter'] +logger = logging.getLogger(__name__) + class CPXTarget(enum.Enum): @@ -59,6 +62,7 @@ class CPXPacket(object): """ A packet with routing and data """ + CPX_VERSION = 0 def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytearray()): """ @@ -68,6 +72,7 @@ def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytear self.source = source self.destination = destination self.function = function + self.version = self.CPX_VERSION self.length = len(data) self.lastPacket = False @@ -79,18 +84,22 @@ def _get_wire_data(self): if self.lastPacket: targetsAndFlags |= 0x40 - function = self.function.value & 0xFF - raw.extend(struct.pack('> 6) & 0x3 + if self.version != self.CPX_VERSION: + logging.error(f'Unsupported CPX version {self.version} instead of {self.CPX_VERSION}') + raise RuntimeError(f'Unsupported CPX version {self.version} instead of {self.CPX_VERSION}') self.source = CPXTarget((targetsAndFlags >> 3) & 0x07) self.destination = CPXTarget(targetsAndFlags & 0x07) self.lastPacket = targetsAndFlags & 0x40 != 0 - self.function = CPXFunction(self.function) + self.function = CPXFunction(functionAndVersion & 0x3F) self.data = data[2:] self.length = len(self.data) From 42fa1522c1db025efa6cdd87fdc6dfdc92b25562 Mon Sep 17 00:00:00 2001 From: hmllr Date: Tue, 21 Mar 2023 16:27:56 +0100 Subject: [PATCH 540/861] Fix styling --- cflib/cpx/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/cflib/cpx/__init__.py b/cflib/cpx/__init__.py index a2a2c7a2a..f8a961115 100644 --- a/cflib/cpx/__init__.py +++ b/cflib/cpx/__init__.py @@ -34,7 +34,6 @@ logger = logging.getLogger(__name__) - class CPXTarget(enum.Enum): """ List of CPX targets From f12e36b4dd734b23d60dcdb48bb68b73c0affacd Mon Sep 17 00:00:00 2001 From: hmllr Date: Wed, 22 Mar 2023 10:08:38 +0100 Subject: [PATCH 541/861] style guide fixes --- examples/autonomy/circling_square_demo.py | 59 ++++++++++++----------- 1 file changed, 31 insertions(+), 28 deletions(-) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index e1487578a..3eb7fd74f 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -33,18 +33,16 @@ import time import numpy as np + import cflib.crtp -from cflib.crazyflie import Crazyflie from cflib.crazyflie.high_level_commander import HighLevelCommander from cflib.crazyflie.log import LogConfig 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.crazyflie.syncLogger import SyncLogger -from cflib.utils import uri_helper from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie.syncLogger import SyncLogger URI1 = 'radio://0/60/2M/E7E7E7E710' URI2 = 'radio://0/60/2M/E7E7E7E711' @@ -56,18 +54,19 @@ 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 +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): + 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 @@ -152,6 +151,7 @@ def upload_trajectory(cf, trajectory_id, trajectory): return total_duration + def turn_off_leds(scf): # Set solid color effect scf.cf.param.set_value('ring.effect', '7') @@ -160,13 +160,14 @@ def turn_off_leds(scf): scf.cf.param.set_value('ring.solidGreen', '0') scf.cf.param.set_value('ring.solidBlue', '0') + def run_sequence(scf, alpha, r): 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) + 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 @@ -187,24 +188,26 @@ def run_sequence(scf, alpha, r): 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 + 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) + 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], []), + 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) @@ -216,14 +219,14 @@ def upload_trajectories(scf, alpha, r): uris = [URI1, URI2, URI3, URI4, URI5, URI6, URI7, URI8] # uris = [URI1, URI5] position_params = { - URI1:[0.0,r1], - URI2:[3.14/4,r2], - URI3:[3.14/2,r1], - URI4:[3.14/4*3,r2], - URI5:[3.14,r1], - URI6:[3.14/4*5,r2], - URI7:[3.14/4*6,r1], - URI8:[3.14/4*7,r2]} + URI1: [0.0, r1], + URI2: [3.14/4, r2], + URI3: [3.14/2, r1], + URI4: [3.14/4*3, r2], + URI5: [3.14, r1], + URI6: [3.14/4*5, r2], + URI7: [3.14/4*6, r1], + URI8: [3.14/4*7, r2]} factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: From d3b57573a1ee41e2b23fe4c855e18b405f8c3c2f Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 22 Mar 2023 15:09:29 +0100 Subject: [PATCH 542/861] ported c code wall following from app to python --- .../wall_following_multiranger.py | 239 ++++++++++++++++++ 1 file changed, 239 insertions(+) create mode 100644 examples/multiranger/wall_following/wall_following_multiranger.py diff --git a/examples/multiranger/wall_following/wall_following_multiranger.py b/examples/multiranger/wall_following/wall_following_multiranger.py new file mode 100644 index 000000000..d4bfc39dc --- /dev/null +++ b/examples/multiranger/wall_following/wall_following_multiranger.py @@ -0,0 +1,239 @@ +from enum import Enum +import math + +class WallFollowingMultiranger(): + class StateWF(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 + + def __init__(self, ref_distance_from_wall=0.0, + max_forward_speed=0.5, + max_turn_rate=0.5, + direction=1.0, + 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, + ranger_threshold_lost=0.3, + in_corner_angle=0.8, + wait_for_measurement_seconds=1.0, + state_wf = StateWF.FORWARD): + + self.ref_distance_from_wall = ref_distance_from_wall + self.max_forward_speed = max_forward_speed + self.max_turn_rate = max_turn_rate + self.direction = direction + 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.ranger_threshold_lost = ranger_threshold_lost + self.in_corner_angle = in_corner_angle + self.wait_for_measurement_seconds = wait_for_measurement_seconds + + self.first_run = True + self.state_wf = state_wf + self.time_now = 0.0 + + def logic_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 wraptopi(self, number): + if number > math.pi: + return number - 2 * math.pi + elif number < -math.pi: + return number + 2 * math.pi + else: + return number + + def command_turn(self, ref_rate): + cmd_vel_x = 0.0 + cmd_ang_w = self.direction * ref_rate + return cmd_vel_x, cmd_ang_w + + def command_align_corner(self, ref_rate, range, wanted_distance_from_corner): + if range > wanted_distance_from_corner + 0.3: + cmd_ang_w = self.direction * ref_rate + cmd_vel_y = 0 + else: + if range > wanted_distance_from_corner: + cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 3.0) + else: + cmd_vel_y = self.direction * (self.max_forward_speed / 3.0) + cmd_ang_w = 0 + return cmd_vel_y, cmd_ang_w + + def command_hover(self, ref_rate): + cmd_vel_x = 0.0 + cmd_vel_y = 0.0 + cmd_ang_w = 0.0 + return cmd_vel_x, cmd_vel_y, cmd_ang_w + + def command_forward_along_wall(self, ref_rate, range): + cmd_vel_x = self.max_forward_speed + cmd_vel_y = 0 + check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) + if not check_distance_wall: + if range > self.ref_distance_from_wall: + cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 2.0) + else: + cmd_vel_y = self.direction * (self.max_forward_speed / 2.0) + return cmd_vel_x, cmd_vel_y + + def command_turn_around_corner_and_adjust(self, radius, range): + cmd_vel_x = self.max_forward_speed + cmd_ang_w = self.direction * (-1 * cmd_vel_x / radius) + check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) + if not check_distance_wall: + if range > self.ref_distance_from_wall: + cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 3.0) + else: + cmd_vel_y = self.direction * (self.max_forward_speed / 3.0) + return cmd_vel_x, cmd_vel_y, cmd_ang_w + + def command_turn_and_adjust(self, rate, range): + cmd_ang_w = self.direction * rate + cmd_vel_y = 0.0 + return cmd_vel_y, cmd_ang_w + + def transition(self, new_state): + self.state_start_time = self.time_now + return new_state + + def adjust_distance_wall(self, distance_wall_new): + self.ref_distance_from_wall = distance_wall_new + + + def wall_follower(self, front_range, side_range, current_heading, + direction_turn, time_outer_loop): + + self.direction = direction_turn + 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 + match self.state_wf: + case self.StateWF.FORWARD: + if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: + self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) + case self.StateWF.HOVER: + print('hover') + case self.StateWF.TURN_TO_FIND_WALL: + side_range_check = side_range < (self.ref_distance_from_wall / math.cos(0.78) + self.ranger_value_buffer) + front_range_check = front_range < (self.ref_distance_from_wall / math.cos(0.78) + self.ranger_value_buffer) + if side_range_check and front_range_check: + self.prev_heading = current_heading + self.wall_angle = self.direction * (1.57 - math.atan(front_range / side_range) + self.angle_value_buffer) + self.state_wf = self.transition(self.StateWF.TURN_TO_ALIGN_TO_WALL) + # If went too far in heading + if side_range < self.ref_distance_from_wall + self.ranger_value_buffer and front_range > self.ref_distance_from_wall + self.ranger_threshold_lost: + self.around_corner_back_track = False + self.prev_heading = current_heading + self.state_wf = self.transition(self.StateWF.FIND_CORNER) + case self.StateWF.TURN_TO_ALIGN_TO_WALL: + align_wall_check = self.logic_is_close_to(current_heading - self.prev_heading, self.wall_angle, self.angle_value_buffer) + if align_wall_check: + self.state_wf = self.transition(self.StateWF.FOLLOW_WALL) + case self.StateWF.FOLLOW_WALL: + # If side range is out of reach, + # end of the wall is reached + if side_range > self.ref_distance_from_wall + self.ranger_threshold_lost: + self.state_wf = self.transition(self.StateWF.FIND_CORNER) + # If front range is small + # then corner is reached + if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: + self.prev_heading = current_heading + self.state_wf = self.transition(self.StateWF.ROTATE_IN_CORNER) + case self.StateWF.ROTATE_IN_CORNER: + if front_range > self.ref_distance_from_wall + self.ranger_value_buffer: + self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) + case self.StateWF.FIND_CORNER: + if side_range <= self.ref_distance_from_wall: + self.state_wf = self.transition(self.StateWF.ROTATE_AROUND_WALL) + case _: + self.state_wf = self.transition(self.StateWF.HOVER) + + # Handle state actions + cmd_vel_x_temp = 0.0 + cmd_vel_y_temp = 0.0 + cmd_ang_w_temp = 0.0 + + match self.state_wf: + case self.StateWF.FORWARD: + cmd_vel_x_temp = self.max_forward_speed + cmd_vel_y_temp = 0.0 + cmd_ang_w_temp = 0.0 + case self.StateWF.HOVER: + cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() + case self.StateWF.TURN_TO_FIND_WALL: + cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) + cmd_vel_y_temp = 0.0 + case self.StateWF.TURN_TO_ALIGN_TO_WALL: + if self.time_now - self.state_start_time < self.wait_for_measurement_seconds: + cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() + else: + cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) + cmd_vel_y_temp = 0.0 + case self.StateWF.FORWARD_ALONG_WALL: + cmd_vel_x_temp, cmd_vel_y_temp = self.command_forward_along_wall(side_range) + cmd_ang_w_temp = 0.0 + case self.StateWF.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.ref_distance_from_wall + self.ranger_threshold_lost: + # check if scanning already occured + if self.wraptopi(math.abs(self.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 + cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust(-1 * self.max_turn_rate, side_range) + cmd_vel_x_temp = 0.0 + else: + cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust(self.max_turn_rate, side_range) + cmd_vel_x_temp = 0.0 + else: + # continue to turn around corner + self.prev_heading = self.current_heading + self.around_corner_back_track = False + cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_around_corner_and_adjust(self.ref_distance_from_wall, side_range) + case self.StateWF.ROTATE_IN_CORNER: + cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) + cmd_vel_y_temp = 0.0 + case self.StateWF.FIND_CORNER: + cmd_vel_y_temp, cmd_ang_w_temp = self.command_align_corner(-1 * self.max_turn_rate, side_range, self.ref_distance_from_wall) + cmd_vel_x_temp = 0.0 + case _: + # state does not exist, so hover! + cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() + + + cmd_vel_x = cmd_vel_x_temp + cmd_vel_y = cmd_vel_y_temp + cmd_ang_w = cmd_ang_w_temp + + return cmd_vel_x, cmd_vel_y, cmd_ang_w, self.state_wf + + + From cd93de8a2851eb519a15f6c3cfb6fe9e2f39db07 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 22 Mar 2023 15:14:14 +0100 Subject: [PATCH 543/861] flake8 format --- .../wall_following_multiranger.py | 36 +++++++++++-------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following_multiranger.py b/examples/multiranger/wall_following/wall_following_multiranger.py index d4bfc39dc..c63bfe104 100644 --- a/examples/multiranger/wall_following/wall_following_multiranger.py +++ b/examples/multiranger/wall_following/wall_following_multiranger.py @@ -1,6 +1,7 @@ from enum import Enum import math + class WallFollowingMultiranger(): class StateWF(Enum): FORWARD = 1 @@ -26,7 +27,7 @@ def __init__(self, ref_distance_from_wall=0.0, ranger_threshold_lost=0.3, in_corner_angle=0.8, wait_for_measurement_seconds=1.0, - state_wf = StateWF.FORWARD): + state_wf=StateWF.FORWARD): self.ref_distance_from_wall = ref_distance_from_wall self.max_forward_speed = max_forward_speed @@ -118,7 +119,6 @@ def transition(self, new_state): def adjust_distance_wall(self, distance_wall_new): self.ref_distance_from_wall = distance_wall_new - def wall_follower(self, front_range, side_range, current_heading, direction_turn, time_outer_loop): @@ -138,19 +138,24 @@ def wall_follower(self, front_range, side_range, current_heading, case self.StateWF.HOVER: print('hover') case self.StateWF.TURN_TO_FIND_WALL: - side_range_check = side_range < (self.ref_distance_from_wall / math.cos(0.78) + self.ranger_value_buffer) - front_range_check = front_range < (self.ref_distance_from_wall / math.cos(0.78) + self.ranger_value_buffer) + side_range_check = side_range < (self.ref_distance_from_wall / + math.cos(0.78) + self.ranger_value_buffer) + front_range_check = front_range < (self.ref_distance_from_wall / + math.cos(0.78) + self.ranger_value_buffer) if side_range_check and front_range_check: self.prev_heading = current_heading - self.wall_angle = self.direction * (1.57 - math.atan(front_range / side_range) + self.angle_value_buffer) + self.wall_angle = self.direction * \ + (1.57 - math.atan(front_range / side_range) + self.angle_value_buffer) self.state_wf = self.transition(self.StateWF.TURN_TO_ALIGN_TO_WALL) # If went too far in heading - if side_range < self.ref_distance_from_wall + self.ranger_value_buffer and front_range > self.ref_distance_from_wall + self.ranger_threshold_lost: + if side_range < self.ref_distance_from_wall + self.ranger_value_buffer and \ + front_range > self.ref_distance_from_wall + self.ranger_threshold_lost: self.around_corner_back_track = False self.prev_heading = current_heading self.state_wf = self.transition(self.StateWF.FIND_CORNER) case self.StateWF.TURN_TO_ALIGN_TO_WALL: - align_wall_check = self.logic_is_close_to(current_heading - self.prev_heading, self.wall_angle, self.angle_value_buffer) + align_wall_check = self.logic_is_close_to( + current_heading - self.prev_heading, self.wall_angle, self.angle_value_buffer) if align_wall_check: self.state_wf = self.transition(self.StateWF.FOLLOW_WALL) case self.StateWF.FOLLOW_WALL: @@ -203,12 +208,14 @@ def wall_follower(self, front_range, side_range, current_heading, # if side range is larger than preffered distance from wall if side_range > self.ref_distance_from_wall + self.ranger_threshold_lost: # check if scanning already occured - if self.wraptopi(math.abs(self.current_heading - self.prev_heading)) > self.in_corner_angle: + if self.wraptopi(math.abs(self.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 - cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust(-1 * self.max_turn_rate, side_range) + cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust( + -1 * self.max_turn_rate, side_range) cmd_vel_x_temp = 0.0 else: cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust(self.max_turn_rate, side_range) @@ -217,23 +224,22 @@ def wall_follower(self, front_range, side_range, current_heading, # continue to turn around corner self.prev_heading = self.current_heading self.around_corner_back_track = False - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_around_corner_and_adjust(self.ref_distance_from_wall, side_range) + cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = \ + self.command_turn_around_corner_and_adjust( + self.ref_distance_from_wall, side_range) case self.StateWF.ROTATE_IN_CORNER: cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) cmd_vel_y_temp = 0.0 case self.StateWF.FIND_CORNER: - cmd_vel_y_temp, cmd_ang_w_temp = self.command_align_corner(-1 * self.max_turn_rate, side_range, self.ref_distance_from_wall) + cmd_vel_y_temp, cmd_ang_w_temp = self.command_align_corner( + -1 * self.max_turn_rate, side_range, self.ref_distance_from_wall) cmd_vel_x_temp = 0.0 case _: # state does not exist, so hover! cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() - cmd_vel_x = cmd_vel_x_temp cmd_vel_y = cmd_vel_y_temp cmd_ang_w = cmd_ang_w_temp return cmd_vel_x, cmd_vel_y, cmd_ang_w, self.state_wf - - - From babfc7c41871632a7e3c7acacd5f1359c5bc6987 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 22 Mar 2023 17:06:37 +0100 Subject: [PATCH 544/861] fixed things in simulation --- .../wall_following_multiranger.py | 32 ++++++++++++------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following_multiranger.py b/examples/multiranger/wall_following/wall_following_multiranger.py index c63bfe104..45680b538 100644 --- a/examples/multiranger/wall_following/wall_following_multiranger.py +++ b/examples/multiranger/wall_following/wall_following_multiranger.py @@ -14,7 +14,7 @@ class StateWF(Enum): FIND_CORNER = 8 def __init__(self, ref_distance_from_wall=0.0, - max_forward_speed=0.5, + max_forward_speed=0.2, max_turn_rate=0.5, direction=1.0, first_run=False, @@ -23,11 +23,11 @@ def __init__(self, ref_distance_from_wall=0.0, around_corner_back_track=False, state_start_time=0.0, ranger_value_buffer=0.2, - angle_value_buffer=0.1, + angle_value_buffer=0.01, ranger_threshold_lost=0.3, in_corner_angle=0.8, wait_for_measurement_seconds=1.0, - state_wf=StateWF.FORWARD): + init_state=StateWF.FORWARD): self.ref_distance_from_wall = ref_distance_from_wall self.max_forward_speed = max_forward_speed @@ -45,7 +45,7 @@ def __init__(self, ref_distance_from_wall=0.0, self.wait_for_measurement_seconds = wait_for_measurement_seconds self.first_run = True - self.state_wf = state_wf + self.state_wf = init_state self.time_now = 0.0 def logic_is_close_to(self, real_value, checked_value, margin): @@ -79,13 +79,13 @@ def command_align_corner(self, ref_rate, range, wanted_distance_from_corner): cmd_ang_w = 0 return cmd_vel_y, cmd_ang_w - def command_hover(self, ref_rate): + def command_hover(self): cmd_vel_x = 0.0 cmd_vel_y = 0.0 cmd_ang_w = 0.0 return cmd_vel_x, cmd_vel_y, cmd_ang_w - def command_forward_along_wall(self, ref_rate, range): + def command_forward_along_wall(self, range): cmd_vel_x = self.max_forward_speed cmd_vel_y = 0 check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) @@ -99,6 +99,7 @@ def command_forward_along_wall(self, ref_rate, range): def command_turn_around_corner_and_adjust(self, radius, range): cmd_vel_x = self.max_forward_speed cmd_ang_w = self.direction * (-1 * cmd_vel_x / radius) + cmd_vel_y = 0.0 check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) if not check_distance_wall: if range > self.ref_distance_from_wall: @@ -130,6 +131,8 @@ def wall_follower(self, front_range, side_range, current_heading, self.around_corner_back_track = False self.first_run = False + print(self.state_wf) + # Handle state transitions match self.state_wf: case self.StateWF.FORWARD: @@ -154,11 +157,13 @@ def wall_follower(self, front_range, side_range, current_heading, self.prev_heading = current_heading self.state_wf = self.transition(self.StateWF.FIND_CORNER) case self.StateWF.TURN_TO_ALIGN_TO_WALL: + print(self.wall_angle) + print(current_heading - self.prev_heading) align_wall_check = self.logic_is_close_to( - current_heading - self.prev_heading, self.wall_angle, self.angle_value_buffer) + self.wraptopi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer) if align_wall_check: - self.state_wf = self.transition(self.StateWF.FOLLOW_WALL) - case self.StateWF.FOLLOW_WALL: + self.state_wf = self.transition(self.StateWF.FORWARD_ALONG_WALL) + case self.StateWF.FORWARD_ALONG_WALL: # If side range is out of reach, # end of the wall is reached if side_range > self.ref_distance_from_wall + self.ranger_threshold_lost: @@ -168,6 +173,11 @@ def wall_follower(self, front_range, side_range, current_heading, if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: self.prev_heading = current_heading self.state_wf = self.transition(self.StateWF.ROTATE_IN_CORNER) + case self.StateWF.ROTATE_AROUND_WALL: + if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: + self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) + + case self.StateWF.ROTATE_IN_CORNER: if front_range > self.ref_distance_from_wall + self.ranger_value_buffer: self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) @@ -208,7 +218,7 @@ def wall_follower(self, front_range, side_range, current_heading, # if side range is larger than preffered distance from wall if side_range > self.ref_distance_from_wall + self.ranger_threshold_lost: # check if scanning already occured - if self.wraptopi(math.abs(self.current_heading - self.prev_heading)) > \ + if self.wraptopi(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 @@ -222,7 +232,7 @@ def wall_follower(self, front_range, side_range, current_heading, cmd_vel_x_temp = 0.0 else: # continue to turn around corner - self.prev_heading = self.current_heading + self.prev_heading = current_heading self.around_corner_back_track = False cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = \ self.command_turn_around_corner_and_adjust( From 3937e4190be5f2b35701c1fb633ae074395b2602 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 22 Mar 2023 17:30:00 +0100 Subject: [PATCH 545/861] fixed in corner behavior --- .../wall_following/wall_following_multiranger.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following_multiranger.py b/examples/multiranger/wall_following/wall_following_multiranger.py index 45680b538..29d18f3d6 100644 --- a/examples/multiranger/wall_following/wall_following_multiranger.py +++ b/examples/multiranger/wall_following/wall_following_multiranger.py @@ -23,7 +23,7 @@ def __init__(self, ref_distance_from_wall=0.0, around_corner_back_track=False, state_start_time=0.0, ranger_value_buffer=0.2, - angle_value_buffer=0.01, + angle_value_buffer=0.1, ranger_threshold_lost=0.3, in_corner_angle=0.8, wait_for_measurement_seconds=1.0, @@ -54,7 +54,7 @@ def logic_is_close_to(self, real_value, checked_value, margin): else: return False - def wraptopi(self, number): + def wrap_to_pi(self, number): if number > math.pi: return number - 2 * math.pi elif number < -math.pi: @@ -131,8 +131,6 @@ def wall_follower(self, front_range, side_range, current_heading, self.around_corner_back_track = False self.first_run = False - print(self.state_wf) - # Handle state transitions match self.state_wf: case self.StateWF.FORWARD: @@ -157,10 +155,8 @@ def wall_follower(self, front_range, side_range, current_heading, self.prev_heading = current_heading self.state_wf = self.transition(self.StateWF.FIND_CORNER) case self.StateWF.TURN_TO_ALIGN_TO_WALL: - print(self.wall_angle) - print(current_heading - self.prev_heading) align_wall_check = self.logic_is_close_to( - self.wraptopi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer) + self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer) if align_wall_check: self.state_wf = self.transition(self.StateWF.FORWARD_ALONG_WALL) case self.StateWF.FORWARD_ALONG_WALL: @@ -179,7 +175,9 @@ def wall_follower(self, front_range, side_range, current_heading, case self.StateWF.ROTATE_IN_CORNER: - if front_range > self.ref_distance_from_wall + self.ranger_value_buffer: + check_heading_corner = self.logic_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_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) case self.StateWF.FIND_CORNER: if side_range <= self.ref_distance_from_wall: @@ -218,7 +216,7 @@ def wall_follower(self, front_range, side_range, current_heading, # if side range is larger than preffered distance from wall if side_range > self.ref_distance_from_wall + self.ranger_threshold_lost: # check if scanning already occured - if self.wraptopi(math.fabs(current_heading - self.prev_heading)) > \ + 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 From 28b7b7b902e6138ceff8efda8cc4a7b0fa994152 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 22 Mar 2023 17:41:01 +0100 Subject: [PATCH 546/861] added cflib multiranger follower script --- .../multiranger/multiranger_wall_following.py | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 examples/multiranger/multiranger_wall_following.py diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py new file mode 100644 index 000000000..fec3a9705 --- /dev/null +++ b/examples/multiranger/multiranger_wall_following.py @@ -0,0 +1,93 @@ +#!/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 + +from wall_following import WallFollowingMultiranger + +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) + +wall_following = WallFollowingMultiranger(angle_value_buffer= 0.01, ref_distance_from_wall = 0.5, max_forward_speed = 0.3, init_state = WallFollowingMultiranger.StateWF.FORWARD) + + +if __name__ == '__main__': + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + cf = Crazyflie(rw_cache='./cache') + with SyncCrazyflie(URI, cf=cf) as scf: + 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 + yaw_rate = 0.0 + + + #need yaw + actual_yaw = 0.0;#motion_commander._yaw_estimator.get_yaw() + + velocity_x, velocity_y, yaw_rate = wall_following.wall_follower(multiranger.front, multiranger.right,actual_yaw,1,time.time()) + + motion_commander.start_linear_motion( + velocity_x, velocity_y, yaw_rate) + + time.sleep(0.1) + + print('Demo terminated!') From ded4281370274ac02bd6c4640e264ae2c0005174 Mon Sep 17 00:00:00 2001 From: hmllr Date: Thu, 23 Mar 2023 11:22:24 +0100 Subject: [PATCH 547/861] Update circling_square_demo.py change 3.14 to pi and copyrightyear --- examples/autonomy/circling_square_demo.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index 3eb7fd74f..88f2ed495 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2018 Bitcraze AB +# 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 @@ -177,7 +177,7 @@ def run_sequence(scf, alpha, r): start_time = time.time() end_time = start_time + duration while True: - color_angle = alpha + ((time.time() - start_time_leds)/duration)*2.0*3.14*color_move_factor + 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') @@ -220,13 +220,13 @@ def upload_trajectories(scf, alpha, r): # uris = [URI1, URI5] position_params = { URI1: [0.0, r1], - URI2: [3.14/4, r2], - URI3: [3.14/2, r1], - URI4: [3.14/4*3, r2], - URI5: [3.14, r1], - URI6: [3.14/4*5, r2], - URI7: [3.14/4*6, r1], - URI8: [3.14/4*7, r2]} + 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: From 8fdfd903cdcf277d93680bf74489cd65cb00e234 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 Mar 2023 13:32:51 +0200 Subject: [PATCH 548/861] clean up of examples --- .../multiranger/multiranger_wall_following.py | 95 +++++++++++++------ .../wall_following_multiranger.py | 36 ++++++- 2 files changed, 99 insertions(+), 32 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index fec3a9705..507b04282 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -7,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2017 Bitcraze AB +# Copyright (C) 2023 Bitcraze AB # # Crazyflie Python Library # @@ -22,16 +22,13 @@ # 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. +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 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. +in all directions and do wall following. Straight walls with corners +are advised to have in the test environment For the example to run the following hardware is needed: * Crazyflie 2.0 @@ -42,6 +39,7 @@ import logging import sys import time +import keyboard import cflib.crtp from cflib.crazyflie import Crazyflie @@ -49,8 +47,10 @@ from cflib.positioning.motion_commander import MotionCommander from cflib.utils import uri_helper from cflib.utils.multiranger import Multiranger +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger -from wall_following import WallFollowingMultiranger +from wall_following.wall_following_multiranger import WallFollowingMultiranger URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -60,34 +60,69 @@ # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) -wall_following = WallFollowingMultiranger(angle_value_buffer= 0.01, ref_distance_from_wall = 0.5, max_forward_speed = 0.3, init_state = WallFollowingMultiranger.StateWF.FORWARD) +wall_following = WallFollowingMultiranger( + angle_value_buffer= 0.1, ref_distance_from_wall = 0.3, + max_forward_speed = 0.3, init_state = WallFollowingMultiranger.StateWF.FORWARD) if __name__ == '__main__': # Initialize the low-level drivers cflib.crtp.init_drivers() + # 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: 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 - yaw_rate = 0.0 - - - #need yaw - actual_yaw = 0.0;#motion_commander._yaw_estimator.get_yaw() - - velocity_x, velocity_y, yaw_rate = wall_following.wall_follower(multiranger.front, multiranger.right,actual_yaw,1,time.time()) - - motion_commander.start_linear_motion( - velocity_x, velocity_y, yaw_rate) - - time.sleep(0.1) - - print('Demo terminated!') + with SyncLogger(scf, lg_stab) as logger: + + keep_flying = True + while keep_flying: + + # initialize variables + velocity_x = 0.0 + velocity_y = 0.0 + yaw_rate = 0.0 + state_wf = WallFollowingMultiranger.StateWF.HOVER + + # Get Yaw + log_entry = logger.next() + data = log_entry[1] + actual_yaw = data['stabilizer.yaw'] + actual_yaw_rad = actual_yaw * 3.1415 / 180 + + # get from range in milimieters + front_range = multiranger.front + + # choose here the direction that you want the wall following to turn to + # direction = -1 turning right and follow wall with left-range + # direction = 1 turning left and follow wall with right-range + direction = -1 + side_range = multiranger.left # Get range in milimeters + + if front_range is None: + front_range = 999 + if right_range is None: + right_range = 999 + if left_range is None: + left_range = 999 + + # 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,-1,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 + # the negative sign is because of this ticket: https://github.com/bitcraze/crazyflie-lib-python/issues/389 + yaw_rate_deg = -1*yaw_rate * 180 / 3.1415 + + motion_commander.start_linear_motion( + velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg) + + # if spacebar is pressed, stop the demo + if keyboard.is_pressed('space'): + keep_flying = False diff --git a/examples/multiranger/wall_following/wall_following_multiranger.py b/examples/multiranger/wall_following/wall_following_multiranger.py index 29d18f3d6..43b23972b 100644 --- a/examples/multiranger/wall_following/wall_following_multiranger.py +++ b/examples/multiranger/wall_following/wall_following_multiranger.py @@ -1,7 +1,35 @@ -from enum import Enum -import math +#!/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 . + +""" +Helper class for the wall following demo +""" +from enum import Enum +import math class WallFollowingMultiranger(): class StateWF(Enum): FORWARD = 1 @@ -134,6 +162,9 @@ def wall_follower(self, front_range, side_range, current_heading, # Handle state transitions match self.state_wf: case self.StateWF.FORWARD: + print('front', front_range) + print('ref_distance_from_wall', self.ref_distance_from_wall) + print('buffer', self.ranger_value_buffer) if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) case self.StateWF.HOVER: @@ -157,6 +188,7 @@ def wall_follower(self, front_range, side_range, current_heading, case self.StateWF.TURN_TO_ALIGN_TO_WALL: align_wall_check = self.logic_is_close_to( self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer) + print(self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle) if align_wall_check: self.state_wf = self.transition(self.StateWF.FORWARD_ALONG_WALL) case self.StateWF.FORWARD_ALONG_WALL: From 2656ff2a707f331243a722c47d2a669285d68d44 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 Mar 2023 13:34:00 +0200 Subject: [PATCH 549/861] flake8 --- examples/multiranger/multiranger_wall_following.py | 11 ++++++----- .../wall_following/wall_following_multiranger.py | 5 +++-- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index 507b04282..db3fad845 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -61,8 +61,8 @@ logging.basicConfig(level=logging.ERROR) wall_following = WallFollowingMultiranger( - angle_value_buffer= 0.1, ref_distance_from_wall = 0.3, - max_forward_speed = 0.3, init_state = WallFollowingMultiranger.StateWF.FORWARD) + angle_value_buffer=0.1, ref_distance_from_wall=0.3, + max_forward_speed=0.3, init_state=WallFollowingMultiranger.StateWF.FORWARD) if __name__ == '__main__': @@ -101,7 +101,7 @@ # direction = -1 turning right and follow wall with left-range # direction = 1 turning left and follow wall with right-range direction = -1 - side_range = multiranger.left # Get range in milimeters + side_range = multiranger.left # Get range in milimeters if front_range is None: front_range = 999 @@ -112,9 +112,10 @@ # 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,-1,time.time()) + front_range, side_range, actual_yaw_rad, -1, time.time()) - print('velocity_x', velocity_x, 'velocity_y', velocity_y, 'yaw_rate', yaw_rate, 'state_wf', state_wf) + print('velocity_x', velocity_x, 'velocity_y', velocity_y, + 'yaw_rate', yaw_rate, 'state_wf', state_wf) # convert yaw_rate from rad to deg # the negative sign is because of this ticket: https://github.com/bitcraze/crazyflie-lib-python/issues/389 diff --git a/examples/multiranger/wall_following/wall_following_multiranger.py b/examples/multiranger/wall_following/wall_following_multiranger.py index 43b23972b..b708c808e 100644 --- a/examples/multiranger/wall_following/wall_following_multiranger.py +++ b/examples/multiranger/wall_following/wall_following_multiranger.py @@ -30,6 +30,8 @@ from enum import Enum import math + + class WallFollowingMultiranger(): class StateWF(Enum): FORWARD = 1 @@ -205,7 +207,6 @@ def wall_follower(self, front_range, side_range, current_heading, if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) - case self.StateWF.ROTATE_IN_CORNER: check_heading_corner = self.logic_is_close_to(math.fabs(self.wrap_to_pi(current_heading-self.prev_heading)), self.in_corner_angle, self.angle_value_buffer) @@ -266,7 +267,7 @@ def wall_follower(self, front_range, side_range, current_heading, self.around_corner_back_track = False cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = \ self.command_turn_around_corner_and_adjust( - self.ref_distance_from_wall, side_range) + self.ref_distance_from_wall, side_range) case self.StateWF.ROTATE_IN_CORNER: cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) cmd_vel_y_temp = 0.0 From f83624089eead8e6505cb0216955fe55e18ac8b2 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 Mar 2023 13:35:07 +0200 Subject: [PATCH 550/861] changed name wall following script --- examples/multiranger/multiranger_wall_following.py | 2 +- .../{wall_following_multiranger.py => wall_following.py} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename examples/multiranger/wall_following/{wall_following_multiranger.py => wall_following.py} (100%) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index db3fad845..e6c2a28e2 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -50,7 +50,7 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncLogger import SyncLogger -from wall_following.wall_following_multiranger import WallFollowingMultiranger +from wall_following.wall_following import WallFollowingMultiranger URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') diff --git a/examples/multiranger/wall_following/wall_following_multiranger.py b/examples/multiranger/wall_following/wall_following.py similarity index 100% rename from examples/multiranger/wall_following/wall_following_multiranger.py rename to examples/multiranger/wall_following/wall_following.py From bcbb1a413afb461f073a6773124f93cc6b18cb25 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 Mar 2023 13:36:24 +0200 Subject: [PATCH 551/861] changed class name wallfollowing --- examples/multiranger/multiranger_wall_following.py | 4 ++-- examples/multiranger/wall_following/wall_following.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index e6c2a28e2..19c427946 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -50,7 +50,7 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncLogger import SyncLogger -from wall_following.wall_following import WallFollowingMultiranger +from wall_following.wall_following import WallFollowing URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -86,7 +86,7 @@ velocity_x = 0.0 velocity_y = 0.0 yaw_rate = 0.0 - state_wf = WallFollowingMultiranger.StateWF.HOVER + state_wf = WallFollowing.StateWF.HOVER # Get Yaw log_entry = logger.next() diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index b708c808e..b8e57767e 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -32,7 +32,7 @@ import math -class WallFollowingMultiranger(): +class WallFollowing(): class StateWF(Enum): FORWARD = 1 HOVER = 2 From 5b5fd9cf682c0dd81461e38cf4d671baea79711e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 Mar 2023 13:56:30 +0200 Subject: [PATCH 552/861] fix flake8 --- .../multiranger/multiranger_wall_following.py | 20 +++++++++---------- .../wall_following/wall_following.py | 5 +++-- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index 19c427946..a08b8c941 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -60,9 +60,9 @@ # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) -wall_following = WallFollowingMultiranger( +wall_following = WallFollowing( angle_value_buffer=0.1, ref_distance_from_wall=0.3, - max_forward_speed=0.3, init_state=WallFollowingMultiranger.StateWF.FORWARD) + max_forward_speed=0.3, init_state=WallFollowing.StateWF.FORWARD) if __name__ == '__main__': @@ -94,21 +94,18 @@ actual_yaw = data['stabilizer.yaw'] actual_yaw_rad = actual_yaw * 3.1415 / 180 - # get from range in milimieters + # get front range in milimeters front_range = multiranger.front + if front_range is None: + front_range = 999 # choose here the direction that you want the wall following to turn to # direction = -1 turning right and follow wall with left-range # direction = 1 turning left and follow wall with right-range direction = -1 side_range = multiranger.left # Get range in milimeters - - if front_range is None: - front_range = 999 - if right_range is None: - right_range = 999 - if left_range is None: - left_range = 999 + if side_range is None: + side_range = 999 # get velocity commands and current state from wall following state machine velocity_x, velocity_y, yaw_rate, state_wf = wall_following.wall_follower( @@ -118,7 +115,8 @@ 'yaw_rate', yaw_rate, 'state_wf', state_wf) # convert yaw_rate from rad to deg - # the negative sign is because of this ticket: https://github.com/bitcraze/crazyflie-lib-python/issues/389 + # the negative sign is because of this ticket: + # https://github.com/bitcraze/crazyflie-lib-python/issues/389 yaw_rate_deg = -1*yaw_rate * 180 / 3.1415 motion_commander.start_linear_motion( diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index b8e57767e..3dcef0dce 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -208,8 +208,9 @@ def wall_follower(self, front_range, side_range, current_heading, self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) case self.StateWF.ROTATE_IN_CORNER: - check_heading_corner = self.logic_is_close_to(math.fabs(self.wrap_to_pi(current_heading-self.prev_heading)), - self.in_corner_angle, self.angle_value_buffer) + check_heading_corner = self.logic_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_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) case self.StateWF.FIND_CORNER: From 8a97c0d629b9b99cc57d9e04c9231739a4824321 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 Mar 2023 15:48:14 +0200 Subject: [PATCH 553/861] add top-range stop instead of keyboard --- examples/multiranger/multiranger_wall_following.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index a08b8c941..a984cf5f8 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -39,7 +39,6 @@ import logging import sys import time -import keyboard import cflib.crtp from cflib.crazyflie import Crazyflie @@ -99,6 +98,11 @@ if front_range is None: front_range = 999 + # get top range in milimeters + top_range = multiranger.up + if top_range is None: + top_range = 999 + # choose here the direction that you want the wall following to turn to # direction = -1 turning right and follow wall with left-range # direction = 1 turning left and follow wall with right-range @@ -123,5 +127,5 @@ velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg) # if spacebar is pressed, stop the demo - if keyboard.is_pressed('space'): + if top_range < 0.2: keep_flying = False From 7c9316803efbf3f9cd3be2d14303316bfcadb25e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 Mar 2023 15:50:54 +0200 Subject: [PATCH 554/861] fixed CI build issues --- examples/multiranger/multiranger_wall_following.py | 11 +++++------ examples/multiranger/wall_following/wall_following.py | 5 +---- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index a984cf5f8..3d6e419d9 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -22,7 +22,6 @@ # 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 @@ -40,16 +39,16 @@ import sys import time +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 -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncLogger import SyncLogger - -from wall_following.wall_following import WallFollowing URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -102,7 +101,7 @@ top_range = multiranger.up if top_range is None: top_range = 999 - + # choose here the direction that you want the wall following to turn to # direction = -1 turning right and follow wall with left-range # direction = 1 turning left and follow wall with right-range diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 3dcef0dce..124c488c1 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -22,14 +22,11 @@ # 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 . - - """ Helper class for the wall following demo """ - -from enum import Enum import math +from enum import Enum class WallFollowing(): From 5ceededade9e357111a5abfc0d552d11a014125f Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 29 Mar 2023 12:41:31 +0200 Subject: [PATCH 555/861] Update pre-commit-config to new format This is required for CI to work with new builder images --- .pre-commit-config.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b4c4462ea..b34f05004 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,3 +1,4 @@ +repos: - repo: https://github.com/pre-commit/pre-commit-hooks rev: 7192665e31cea6ace58a71e086c7248d7e5610c2 hooks: From db3f5ab90e215a6355abbe7780906100c0822a86 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 30 Mar 2023 16:01:53 +0200 Subject: [PATCH 556/861] add doc and remove debug prints --- examples/multiranger/multiranger_wall_following.py | 5 ++++- examples/multiranger/wall_following/wall_following.py | 9 +++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index 3d6e419d9..cbcd2c338 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -27,7 +27,10 @@ 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 +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 diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 124c488c1..28bd26695 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -24,6 +24,11 @@ # along with this program. If not, see . """ Helper 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 + """ import math from enum import Enum @@ -161,9 +166,6 @@ def wall_follower(self, front_range, side_range, current_heading, # Handle state transitions match self.state_wf: case self.StateWF.FORWARD: - print('front', front_range) - print('ref_distance_from_wall', self.ref_distance_from_wall) - print('buffer', self.ranger_value_buffer) if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) case self.StateWF.HOVER: @@ -187,7 +189,6 @@ def wall_follower(self, front_range, side_range, current_heading, case self.StateWF.TURN_TO_ALIGN_TO_WALL: align_wall_check = self.logic_is_close_to( self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer) - print(self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle) if align_wall_check: self.state_wf = self.transition(self.StateWF.FORWARD_ALONG_WALL) case self.StateWF.FORWARD_ALONG_WALL: From 2c1a557079b1fb871430bb02a6920040848bfbe7 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Wed, 19 Apr 2023 11:41:31 +0200 Subject: [PATCH 557/861] Added multiranger and paa3905 memory types. The multiranger (VL53L5) and paa3905 memory types will allow their data to be read over the memory interface. --- cflib/crazyflie/mem/__init__.py | 12 ++++ cflib/crazyflie/mem/memory_element.py | 2 + cflib/crazyflie/mem/multiranger_memory.py | 69 +++++++++++++++++++++++ cflib/crazyflie/mem/paa3905_memory.py | 69 +++++++++++++++++++++++ 4 files changed, 152 insertions(+) create mode 100644 cflib/crazyflie/mem/multiranger_memory.py create mode 100644 cflib/crazyflie/mem/paa3905_memory.py diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index b3767676b..6ce4c519a 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -31,6 +31,8 @@ import struct from threading import Lock +from .multiranger_memory import MultirangerMemory +from .paa3905_memory import PAA3905Memory from .deck_memory import DeckMemoryManager from .i2c_element import I2CElement from .led_driver_memory import LEDDriverMemory @@ -521,6 +523,16 @@ def _handle_cmd_info_details(self, payload): self.mem_read_failed_cb.add_callback(mem._new_data_failed) self.mem_write_cb.add_callback(mem._write_done) self.mem_write_failed_cb.add_callback(mem._write_failed) + elif mem_type == MemoryElement.TYPE_DECK_MULTIRANGER: + mem = MultirangerMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_read_failed_cb.add_callback(mem.read_failed) + elif mem_type == MemoryElement.TYPE_DECK_PAA3905: + mem = PAA3905Memory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) + self.mem_read_failed_cb.add_callback(mem.read_failed) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) diff --git a/cflib/crazyflie/mem/memory_element.py b/cflib/crazyflie/mem/memory_element.py index d75011217..895b2a5d5 100644 --- a/cflib/crazyflie/mem/memory_element.py +++ b/cflib/crazyflie/mem/memory_element.py @@ -38,6 +38,8 @@ class MemoryElement(object): TYPE_DRIVER_LEDTIMING = 0x17 TYPE_APP = 0x18 TYPE_DECK_MEMORY = 0x19 + TYPE_DECK_MULTIRANGER = 0x1A + TYPE_DECK_PAA3905 = 0x1B def __init__(self, id, type, size, mem_handler): """Initialize the element with default values""" diff --git a/cflib/crazyflie/mem/multiranger_memory.py b/cflib/crazyflie/mem/multiranger_memory.py new file mode 100644 index 000000000..3cebec860 --- /dev/null +++ b/cflib/crazyflie/mem/multiranger_memory.py @@ -0,0 +1,69 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2019 - 2020 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 logging +import struct + +from .memory_element import MemoryElement +from cflib.utils.callbacks import Syncer + +logger = logging.getLogger(__name__) + + +class MultirangerMemory(MemoryElement): + """Memory interface for reading the multiranger values""" + + def __init__(self, id, type, size, mem_handler): + super(MultirangerMemory, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + self._read_finished_cb = None + + def new_data(self, mem, addr, data): + """Callback for when new memory data has been fetched""" + if mem.id == self.id and self._read_finished_cb: + unpacked_data = struct.unpack('<'+'H'*int(len(data) / 2), data) + zone_matrix = [] + for i in range(8): + zone_matrix.append(unpacked_data[i*8:i*8+8]) + + self._read_finished_cb(addr, zone_matrix) + + def read_data(self, read_finished_cb): + """Write the saved LED-ring data to the Crazyflie""" + self._read_finished_cb = read_finished_cb + self.mem_handler.read(self, 0, 128) + + def read_data_sync(self): + """Write the saved LED-ring data to the Crazyflie""" + syncer = Syncer() + self.read_data(syncer.success_cb) + syncer.wait() + if syncer.is_success: + return syncer.success_args[1] + else: + return None + + def read_failed(self, mem, addr): + if mem.id == self.id: + logger.debug('Read failed') + + def disconnect(self): + self._read_finished_cb = None diff --git a/cflib/crazyflie/mem/paa3905_memory.py b/cflib/crazyflie/mem/paa3905_memory.py new file mode 100644 index 000000000..696fc9f91 --- /dev/null +++ b/cflib/crazyflie/mem/paa3905_memory.py @@ -0,0 +1,69 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2019 - 2020 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 logging +import struct + +from .memory_element import MemoryElement +from cflib.utils.callbacks import Syncer + +logger = logging.getLogger(__name__) + + +class PAA3905Memory(MemoryElement): + """Memory interface for reading the multiranger values""" + + def __init__(self, id, type, size, mem_handler): + super(PAA3905Memory, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + self._read_finished_cb = None + + def new_data(self, mem, addr, data): + """Callback for when new memory data has been fetched""" + if mem.id == self.id and self._read_finished_cb: + print(len(data)) + image_matrix = [] + for i in range(35): + image_matrix.append(data[i*35:i*35+35]) + + self._read_finished_cb(addr, image_matrix) + + def read_data(self, read_finished_cb): + """Read image data from PAA3905""" + self._read_finished_cb = read_finished_cb + self.mem_handler.read(self, 0, 1225) + + def read_data_sync(self): + """Write the saved LED-ring data to the Crazyflie""" + syncer = Syncer() + self.read_data(syncer.success_cb) + syncer.wait() + if syncer.is_success: + return syncer.success_args[1] + else: + return None + + def read_failed(self, mem, addr): + if mem.id == self.id: + logger.debug('Read failed') + + def disconnect(self): + self._read_finished_cb = None From 448d1d3e27a59b138824b06936d2c395bf22569c Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Wed, 19 Apr 2023 12:59:39 +0200 Subject: [PATCH 558/861] Added vl53l5 and paa3905 read examples --- examples/memory/read-l5.py | 76 ++++++++++++++++++++++++++++++++ examples/memory/read-paa3905.py | 78 +++++++++++++++++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100644 examples/memory/read-l5.py create mode 100644 examples/memory/read-paa3905.py diff --git a/examples/memory/read-l5.py b/examples/memory/read-l5.py new file mode 100644 index 000000000..c5a0a9301 --- /dev/null +++ b/examples/memory/read-l5.py @@ -0,0 +1,76 @@ +# -*- 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 +from threading import Event +import time +import matplotlib.pyplot as plt +import numpy as np +import random + +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-paa3905.py b/examples/memory/read-paa3905.py new file mode 100644 index 000000000..4b2133d0a --- /dev/null +++ b/examples/memory/read-paa3905.py @@ -0,0 +1,78 @@ +# -*- 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 +from threading import Event +import time +import matplotlib.pyplot as plt +import numpy as np +import random + +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) From 01704a7d57f36867c0654cb4671480008b2ac6ba Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Wed, 19 Apr 2023 13:21:27 +0200 Subject: [PATCH 559/861] Fixed flake8 errors --- cflib/crazyflie/mem/__init__.py | 4 ++-- cflib/crazyflie/mem/multiranger_memory.py | 8 ++++---- cflib/crazyflie/mem/paa3905_memory.py | 9 ++++----- examples/memory/read-l5.py | 17 ++++++++--------- examples/memory/read-paa3905.py | 19 +++++++++---------- 5 files changed, 27 insertions(+), 30 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 6ce4c519a..00d07da11 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -31,8 +31,6 @@ import struct from threading import Lock -from .multiranger_memory import MultirangerMemory -from .paa3905_memory import PAA3905Memory from .deck_memory import DeckMemoryManager from .i2c_element import I2CElement from .led_driver_memory import LEDDriverMemory @@ -45,7 +43,9 @@ from .loco_memory_2 import LocoMemory2 from .memory_element import MemoryElement from .memory_tester import MemoryTester +from .multiranger_memory import MultirangerMemory from .ow_element import OWElement +from .paa3905_memory import PAA3905Memory from .trajectory_memory import CompressedSegment from .trajectory_memory import CompressedStart from .trajectory_memory import Poly4D diff --git a/cflib/crazyflie/mem/multiranger_memory.py b/cflib/crazyflie/mem/multiranger_memory.py index 3cebec860..b0cf16eeb 100644 --- a/cflib/crazyflie/mem/multiranger_memory.py +++ b/cflib/crazyflie/mem/multiranger_memory.py @@ -33,7 +33,7 @@ class MultirangerMemory(MemoryElement): def __init__(self, id, type, size, mem_handler): super(MultirangerMemory, self).__init__(id=id, type=type, size=size, - mem_handler=mem_handler) + mem_handler=mem_handler) self._read_finished_cb = None def new_data(self, mem, addr, data): @@ -43,9 +43,9 @@ def new_data(self, mem, addr, data): zone_matrix = [] for i in range(8): zone_matrix.append(unpacked_data[i*8:i*8+8]) - + self._read_finished_cb(addr, zone_matrix) - + def read_data(self, read_finished_cb): """Write the saved LED-ring data to the Crazyflie""" self._read_finished_cb = read_finished_cb @@ -60,7 +60,7 @@ def read_data_sync(self): return syncer.success_args[1] else: return None - + def read_failed(self, mem, addr): if mem.id == self.id: logger.debug('Read failed') diff --git a/cflib/crazyflie/mem/paa3905_memory.py b/cflib/crazyflie/mem/paa3905_memory.py index 696fc9f91..3133ceab7 100644 --- a/cflib/crazyflie/mem/paa3905_memory.py +++ b/cflib/crazyflie/mem/paa3905_memory.py @@ -20,7 +20,6 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import logging -import struct from .memory_element import MemoryElement from cflib.utils.callbacks import Syncer @@ -33,7 +32,7 @@ class PAA3905Memory(MemoryElement): def __init__(self, id, type, size, mem_handler): super(PAA3905Memory, self).__init__(id=id, type=type, size=size, - mem_handler=mem_handler) + mem_handler=mem_handler) self._read_finished_cb = None def new_data(self, mem, addr, data): @@ -43,9 +42,9 @@ def new_data(self, mem, addr, data): image_matrix = [] for i in range(35): image_matrix.append(data[i*35:i*35+35]) - + self._read_finished_cb(addr, image_matrix) - + def read_data(self, read_finished_cb): """Read image data from PAA3905""" self._read_finished_cb = read_finished_cb @@ -60,7 +59,7 @@ def read_data_sync(self): return syncer.success_args[1] else: return None - + def read_failed(self, mem, addr): if mem.id == self.id: logger.debug('Read failed') diff --git a/examples/memory/read-l5.py b/examples/memory/read-l5.py index c5a0a9301..723145455 100644 --- a/examples/memory/read-l5.py +++ b/examples/memory/read-l5.py @@ -23,11 +23,10 @@ Example of how to read the memory from the multiranger """ import logging -from threading import Event import time +from threading import Event + import matplotlib.pyplot as plt -import numpy as np -import random import cflib.crtp # noqa from cflib.crazyflie import Crazyflie @@ -52,20 +51,20 @@ def __init__(self, uri): 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))) - + 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') diff --git a/examples/memory/read-paa3905.py b/examples/memory/read-paa3905.py index 4b2133d0a..96fab14e5 100644 --- a/examples/memory/read-paa3905.py +++ b/examples/memory/read-paa3905.py @@ -23,11 +23,10 @@ Example of how to read the memory from the multiranger """ import logging -from threading import Event import time +from threading import Event + import matplotlib.pyplot as plt -import numpy as np -import random import cflib.crtp # noqa from cflib.crazyflie import Crazyflie @@ -52,24 +51,24 @@ def __init__(self, uri): 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))) + 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='radio://0/80/2M/E7E7E7E7E7') uri = uri_helper.uri_from_env(default='usb://0') # Initialize the low-level drivers From 772cce3494ae1a0b5d562146579bfc251136942b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 28 Apr 2023 12:32:36 +0200 Subject: [PATCH 560/861] read/write file from lh estimator --- .../multi_bs_geometry_estimation.py | 37 ++++++++++++++++++- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py index 0de9c1b40..0b483f89f 100644 --- a/examples/lighthouse/multi_bs_geometry_estimation.py +++ b/examples/lighthouse/multi_bs_geometry_estimation.py @@ -44,6 +44,7 @@ from __future__ import annotations import logging +import pickle import time from threading import Event @@ -206,6 +207,21 @@ def visualize(cf_poses: list[Pose], bs_poses: list[Pose]): 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], @@ -282,7 +298,12 @@ def data_written(_): event.wait() -def connect_and_estimate(uri: str): +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 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: @@ -343,6 +364,10 @@ def connect_and_estimate(uri: str): 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('Wrote data to file {file_name}') + print('Step 6. Estimating geometry...') bs_poses = estimate_geometry(origin, x_axis, xy_plane, samples) print(' Geometry estimated') @@ -361,4 +386,12 @@ def connect_and_estimate(uri: str): cflib.crtp.init_drivers() uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - connect_and_estimate(uri) + + # 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) From c30c83c0ccc46c479f0f43b327b34868aec44255 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 28 Apr 2023 14:30:50 +0200 Subject: [PATCH 561/861] Fixed print --- examples/lighthouse/multi_bs_geometry_estimation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py index 0b483f89f..955c06d4f 100644 --- a/examples/lighthouse/multi_bs_geometry_estimation.py +++ b/examples/lighthouse/multi_bs_geometry_estimation.py @@ -366,7 +366,7 @@ def connect_and_estimate(uri: str, file_name: str | None = None): if file_name: write_to_file(file_name, origin, x_axis, xy_plane, samples) - print('Wrote data to file {file_name}') + print(f'Wrote data to file {file_name}') print('Step 6. Estimating geometry...') bs_poses = estimate_geometry(origin, x_axis, xy_plane, samples) From 2b2428b224ea6c23f0a90d447b94365d21942baa Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 2 May 2023 15:11:32 +0200 Subject: [PATCH 562/861] cleaned up wall_following --- .../wall_following/wall_following.py | 344 +++++++++++------- 1 file changed, 215 insertions(+), 129 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 28bd26695..4491d2bbe 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -35,7 +35,7 @@ class WallFollowing(): - class StateWF(Enum): + class StateWallFollowing(Enum): FORWARD = 1 HOVER = 2 TURN_TO_FIND_WALL = 3 @@ -44,11 +44,15 @@ class StateWF(Enum): ROTATE_AROUND_WALL = 6 ROTATE_IN_CORNER = 7 FIND_CORNER = 8 + + class WallFollowingDirection(Enum): + LEFT = 1 + RIGHT = -1 - def __init__(self, ref_distance_from_wall=0.0, + def __init__(self, reference_distance_from_wall=0.0, max_forward_speed=0.2, max_turn_rate=0.5, - direction=1.0, + wall_following_direction=WallFollowingDirection.LEFT, first_run=False, prev_heading=0.0, wall_angle=0.0, @@ -56,15 +60,44 @@ def __init__(self, ref_distance_from_wall=0.0, state_start_time=0.0, ranger_value_buffer=0.2, angle_value_buffer=0.1, - ranger_threshold_lost=0.3, + range_lost_threshold=0.3, in_corner_angle=0.8, wait_for_measurement_seconds=1.0, - init_state=StateWF.FORWARD): + init_state=StateWallFollowing.FORWARD): + + """ + __init__ function for the WallFollowing class - self.ref_distance_from_wall = ref_distance_from_wall + 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.direction = direction + self.wall_following_direction = wall_following_direction self.first_run = first_run self.prev_heading = prev_heading self.wall_angle = wall_angle @@ -72,15 +105,16 @@ def __init__(self, ref_distance_from_wall=0.0, self.state_start_time = state_start_time self.ranger_value_buffer = ranger_value_buffer self.angle_value_buffer = angle_value_buffer - self.ranger_threshold_lost = ranger_threshold_lost + 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_wf = init_state + self.state = init_state self.time_now = 0.0 - def logic_is_close_to(self, real_value, checked_value, margin): + # 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: @@ -93,69 +127,123 @@ def wrap_to_pi(self, number): 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 * reference_rate + return velocity_x, rate_yaw - def command_turn(self, ref_rate): - cmd_vel_x = 0.0 - cmd_ang_w = self.direction * ref_rate - return cmd_vel_x, cmd_ang_w + 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 - def command_align_corner(self, ref_rate, range, wanted_distance_from_corner): - if range > wanted_distance_from_corner + 0.3: - cmd_ang_w = self.direction * ref_rate - cmd_vel_y = 0 + 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 + 0.3: + rate_yaw = self.wall_following_direction * reference_rate + velocity_y = 0.0 else: - if range > wanted_distance_from_corner: - cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 3.0) + if side_range > wanted_distance_from_corner: + velocity_y = self.wall_following_direction * (-1.0 * self.max_forward_speed / 3.0) else: - cmd_vel_y = self.direction * (self.max_forward_speed / 3.0) - cmd_ang_w = 0 - return cmd_vel_y, cmd_ang_w + velocity_y = self.wall_following_direction * (self.max_forward_speed / 3.0) + rate_yaw = 0.0 + return velocity_y, rate_yaw def command_hover(self): - cmd_vel_x = 0.0 - cmd_vel_y = 0.0 - cmd_ang_w = 0.0 - return cmd_vel_x, cmd_vel_y, cmd_ang_w + """ + 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, range): - cmd_vel_x = self.max_forward_speed - cmd_vel_y = 0 - check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) + 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, 0.1) if not check_distance_wall: - if range > self.ref_distance_from_wall: - cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 2.0) + if side_range > self.reference_distance_from_wall: + velocity_y = self.wall_following_direction * (-1.0 * self.max_forward_speed / 2.0) else: - cmd_vel_y = self.direction * (self.max_forward_speed / 2.0) - return cmd_vel_x, cmd_vel_y + velocity_y = self.wall_following_direction * (self.max_forward_speed / 2.0) + return velocity_x, velocity_y - def command_turn_around_corner_and_adjust(self, radius, range): - cmd_vel_x = self.max_forward_speed - cmd_ang_w = self.direction * (-1 * cmd_vel_x / radius) - cmd_vel_y = 0.0 - check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) + 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 * (-1 * velocity_x / radius) + velocity_y = 0.0 + check_distance_wall = self.value_is_close_to(self.reference_distance_from_wall, side_range, 0.1) if not check_distance_wall: - if range > self.ref_distance_from_wall: - cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 3.0) + if side_range > self.reference_distance_from_wall: + velocity_y = self.wall_following_direction * (-1.0 * self.max_forward_speed / 3.0) else: - cmd_vel_y = self.direction * (self.max_forward_speed / 3.0) - return cmd_vel_x, cmd_vel_y, cmd_ang_w + velocity_y = self.wall_following_direction * (self.max_forward_speed / 3.0) + return velocity_x, velocity_y, rate_yaw - def command_turn_and_adjust(self, rate, range): - cmd_ang_w = self.direction * rate - cmd_vel_y = 0.0 - return cmd_vel_y, cmd_ang_w + # state machine helper functions + def state_transition(self, new_state): + """ + Transition to a new state and reset the state timer - def transition(self, new_state): + new_state is defined in the StateWallFollowing enum + """ self.state_start_time = self.time_now return new_state - def adjust_distance_wall(self, distance_wall_new): - self.ref_distance_from_wall = distance_wall_new + 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, - direction_turn, time_outer_loop): + 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.direction = direction_turn + self.wall_following_direction = wall_following_direction self.time_now = time_outer_loop if self.first_run: @@ -163,90 +251,88 @@ def wall_follower(self, front_range, side_range, current_heading, self.around_corner_back_track = False self.first_run = False - # Handle state transitions - match self.state_wf: - case self.StateWF.FORWARD: - if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: - self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) - case self.StateWF.HOVER: + # -------------- Handle state transitions ---------------- # + match self.state: + case 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) + case self.StateWallFollowing.HOVER: print('hover') - case self.StateWF.TURN_TO_FIND_WALL: - side_range_check = side_range < (self.ref_distance_from_wall / + case self.StateWallFollowing.TURN_TO_FIND_WALL: + side_range_check = side_range < (self.reference_distance_from_wall / math.cos(0.78) + self.ranger_value_buffer) - front_range_check = front_range < (self.ref_distance_from_wall / + front_range_check = front_range < (self.reference_distance_from_wall / math.cos(0.78) + self.ranger_value_buffer) if side_range_check and front_range_check: self.prev_heading = current_heading - self.wall_angle = self.direction * \ + self.wall_angle = self.wall_following_direction * \ (1.57 - math.atan(front_range / side_range) + self.angle_value_buffer) - self.state_wf = self.transition(self.StateWF.TURN_TO_ALIGN_TO_WALL) + self.state = self.state_transition(self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL) # If went too far in heading - if side_range < self.ref_distance_from_wall + self.ranger_value_buffer and \ - front_range > self.ref_distance_from_wall + self.ranger_threshold_lost: + 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_wf = self.transition(self.StateWF.FIND_CORNER) - case self.StateWF.TURN_TO_ALIGN_TO_WALL: - align_wall_check = self.logic_is_close_to( + self.state = self.state_transition(self.StateWallFollowing.FIND_CORNER) + case 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_wf = self.transition(self.StateWF.FORWARD_ALONG_WALL) - case self.StateWF.FORWARD_ALONG_WALL: + self.state = self.state_transition(self.StateWallFollowing.FORWARD_ALONG_WALL) + case self.StateWallFollowing.FORWARD_ALONG_WALL: # If side range is out of reach, # end of the wall is reached - if side_range > self.ref_distance_from_wall + self.ranger_threshold_lost: - self.state_wf = self.transition(self.StateWF.FIND_CORNER) + 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.ref_distance_from_wall + self.ranger_value_buffer: + if front_range < self.reference_distance_from_wall + self.ranger_value_buffer: self.prev_heading = current_heading - self.state_wf = self.transition(self.StateWF.ROTATE_IN_CORNER) - case self.StateWF.ROTATE_AROUND_WALL: - if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: - self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) - - case self.StateWF.ROTATE_IN_CORNER: - check_heading_corner = self.logic_is_close_to( + self.state = self.state_transition(self.StateWallFollowing.ROTATE_IN_CORNER) + case 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) + case 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_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) - case self.StateWF.FIND_CORNER: - if side_range <= self.ref_distance_from_wall: - self.state_wf = self.transition(self.StateWF.ROTATE_AROUND_WALL) + self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL) + case self.StateWallFollowing.FIND_CORNER: + if side_range <= self.reference_distance_from_wall: + self.state = self.state_transition(self.StateWallFollowing.ROTATE_AROUND_WALL) case _: - self.state_wf = self.transition(self.StateWF.HOVER) + self.state = self.state_transition(self.StateWallFollowing.HOVER) - # Handle state actions - cmd_vel_x_temp = 0.0 - cmd_vel_y_temp = 0.0 - cmd_ang_w_temp = 0.0 + # -------------- Handle state actions ---------------- # + command_velocity_x_temporary = 0.0 + command_velocity_y_temporary = 0.0 + command_angle_rate_temporary = 0.0 - match self.state_wf: - case self.StateWF.FORWARD: - cmd_vel_x_temp = self.max_forward_speed - cmd_vel_y_temp = 0.0 - cmd_ang_w_temp = 0.0 - case self.StateWF.HOVER: - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() - case self.StateWF.TURN_TO_FIND_WALL: - cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) - cmd_vel_y_temp = 0.0 - case self.StateWF.TURN_TO_ALIGN_TO_WALL: + match self.state: + case self.StateWallFollowing.FORWARD: + command_velocity_x_temporary = self.max_forward_speed + command_velocity_y_temporary = 0.0 + command_angle_rate_temporary = 0.0 + case self.StateWallFollowing.HOVER: + command_velocity_x_temporary, command_velocity_y_temporary, command_angle_rate_temporary = self.command_hover() + case self.StateWallFollowing.TURN_TO_FIND_WALL: + command_velocity_x_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) + command_velocity_y_temporary = 0.0 + case self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL: if self.time_now - self.state_start_time < self.wait_for_measurement_seconds: - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() + command_velocity_x_temporary, command_velocity_y_temporary, command_angle_rate_temporary = self.command_hover() else: - cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) - cmd_vel_y_temp = 0.0 - case self.StateWF.FORWARD_ALONG_WALL: - cmd_vel_x_temp, cmd_vel_y_temp = self.command_forward_along_wall(side_range) - cmd_ang_w_temp = 0.0 - case self.StateWF.ROTATE_AROUND_WALL: + command_velocity_x_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) + command_velocity_y_temporary = 0.0 + case self.StateWallFollowing.FORWARD_ALONG_WALL: + command_velocity_x_temporary, command_velocity_y_temporary = self.command_forward_along_wall(side_range) + command_angle_rate_temporary = 0.0 + case 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.ref_distance_from_wall + self.ranger_threshold_lost: + 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: @@ -254,32 +340,32 @@ def wall_follower(self, front_range, side_range, current_heading, # turn and adjust distance to corner from that point if self.around_corner_back_track: # rotate back if it already went into one direction - cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust( - -1 * self.max_turn_rate, side_range) - cmd_vel_x_temp = 0.0 + command_velocity_y_temporary, command_angle_rate_temporary = self.command_turn( + -1 * self.max_turn_rate) + command_velocity_x_temporary = 0.0 else: - cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust(self.max_turn_rate, side_range) - cmd_vel_x_temp = 0.0 + command_velocity_y_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) + command_velocity_x_temporary = 0.0 else: # continue to turn around corner self.prev_heading = current_heading self.around_corner_back_track = False - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = \ + command_velocity_x_temporary, command_velocity_y_temporary, command_angle_rate_temporary = \ self.command_turn_around_corner_and_adjust( - self.ref_distance_from_wall, side_range) - case self.StateWF.ROTATE_IN_CORNER: - cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) - cmd_vel_y_temp = 0.0 - case self.StateWF.FIND_CORNER: - cmd_vel_y_temp, cmd_ang_w_temp = self.command_align_corner( - -1 * self.max_turn_rate, side_range, self.ref_distance_from_wall) - cmd_vel_x_temp = 0.0 + self.reference_distance_from_wall, side_range) + case self.StateWallFollowing.ROTATE_IN_CORNER: + command_velocity_x_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) + command_velocity_y_temporary = 0.0 + case self.StateWallFollowing.FIND_CORNER: + command_velocity_y_temporary, command_angle_rate_temporary = self.command_align_corner( + -1 * self.max_turn_rate, side_range, self.reference_distance_from_wall) + command_velocity_x_temporary = 0.0 case _: # state does not exist, so hover! - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() + command_velocity_x_temporary, command_velocity_y_temporary, command_angle_rate_temporary = self.command_hover() - cmd_vel_x = cmd_vel_x_temp - cmd_vel_y = cmd_vel_y_temp - cmd_ang_w = cmd_ang_w_temp + command_velocity_x = command_velocity_x_temporary + command_velocity_y = command_velocity_y_temporary + command_yaw_rate = command_angle_rate_temporary - return cmd_vel_x, cmd_vel_y, cmd_ang_w, self.state_wf + return command_velocity_x, command_velocity_y, command_yaw_rate, self.state From 76b23f8c423f466ead9f04f345379979881e1af6 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 2 May 2023 15:20:18 +0200 Subject: [PATCH 563/861] update wf example --- .../multiranger/multiranger_wall_following.py | 58 +++++++++---------- 1 file changed, 27 insertions(+), 31 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index cbcd2c338..eec2bd8ea 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -38,8 +38,8 @@ * Flow deck * Multiranger deck """ + import logging -import sys import time from wall_following.wall_following import WallFollowing @@ -53,23 +53,28 @@ 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] +from math import degrees -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - -wall_following = WallFollowing( - angle_value_buffer=0.1, ref_distance_from_wall=0.3, - max_forward_speed=0.3, init_state=WallFollowing.StateWF.FORWARD) +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') @@ -80,14 +85,13 @@ with Multiranger(scf) as multiranger: with SyncLogger(scf, lg_stab) as logger: - keep_flying = True while keep_flying: # initialize variables velocity_x = 0.0 velocity_y = 0.0 yaw_rate = 0.0 - state_wf = WallFollowing.StateWF.HOVER + state_wf = WallFollowing.StateWallFollowing.HOVER # Get Yaw log_entry = logger.next() @@ -95,27 +99,19 @@ actual_yaw = data['stabilizer.yaw'] actual_yaw_rad = actual_yaw * 3.1415 / 180 - # get front range in milimeters - front_range = multiranger.front - if front_range is None: - front_range = 999 - - # get top range in milimeters - top_range = multiranger.up - if top_range is None: - top_range = 999 + # 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 - # direction = -1 turning right and follow wall with left-range - # direction = 1 turning left and follow wall with right-range - direction = -1 - side_range = multiranger.left # Get range in milimeters - if side_range is None: - side_range = 999 + 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, -1, time.time()) + 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) @@ -123,11 +119,11 @@ # convert yaw_rate from rad to deg # the negative sign is because of this ticket: # https://github.com/bitcraze/crazyflie-lib-python/issues/389 - yaw_rate_deg = -1*yaw_rate * 180 / 3.1415 + yaw_rate_deg = -1 * degrees(yaw_rate) motion_commander.start_linear_motion( velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg) - # if spacebar is pressed, stop the demo + # if top_range is activated, stop the demo if top_range < 0.2: keep_flying = False From 493ba0fa0963f5dfaeeaa338adedcd8ea67ab014 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 2 May 2023 15:24:14 +0200 Subject: [PATCH 564/861] replaced hardcoded values --- .../wall_following/wall_following.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 4491d2bbe..9aff7d995 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -112,6 +112,8 @@ def __init__(self, reference_distance_from_wall=0.0, 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): @@ -154,9 +156,9 @@ def command_align_corner(self, reference_rate, side_range, wanted_distance_from_ velocity_y = 0.0 else: if side_range > wanted_distance_from_corner: - velocity_y = self.wall_following_direction * (-1.0 * self.max_forward_speed / 3.0) + velocity_y = self.wall_following_direction * (-1.0 * self.max_forward_speed / self.speed_redux_corner ) else: - velocity_y = self.wall_following_direction * (self.max_forward_speed / 3.0) + velocity_y = self.wall_following_direction * (self.max_forward_speed / self.speed_redux_corner ) rate_yaw = 0.0 return velocity_y, rate_yaw @@ -179,12 +181,12 @@ def command_forward_along_wall(self, side_range): """ 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, 0.1) + 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 * (-1.0 * self.max_forward_speed / 2.0) + velocity_y = self.wall_following_direction * (-1.0 * self.max_forward_speed / self.speed_redux_straight ) else: - velocity_y = self.wall_following_direction * (self.max_forward_speed / 2.0) + velocity_y = self.wall_following_direction * (self.max_forward_speed / self.speed_redux_straight ) return velocity_x, velocity_y def command_turn_around_corner_and_adjust(self, radius, side_range): @@ -199,12 +201,12 @@ def command_turn_around_corner_and_adjust(self, radius, side_range): velocity_x = self.max_forward_speed rate_yaw = self.wall_following_direction * (-1 * velocity_x / radius) velocity_y = 0.0 - check_distance_wall = self.value_is_close_to(self.reference_distance_from_wall, side_range, 0.1) + 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 * (-1.0 * self.max_forward_speed / 3.0) + velocity_y = self.wall_following_direction * (-1.0 * self.max_forward_speed / self.speed_redux_corner ) else: - velocity_y = self.wall_following_direction * (self.max_forward_speed / 3.0) + velocity_y = self.wall_following_direction * (self.max_forward_speed / self.speed_redux_corner) return velocity_x, velocity_y, rate_yaw # state machine helper functions From 19282816293168f02ce294b5b5c71280a709f5bd Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 2 May 2023 15:45:05 +0200 Subject: [PATCH 565/861] fixed wall following ref distance --- .../wall_following/wall_following.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 9aff7d995..76efd6c74 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -97,7 +97,7 @@ def __init__(self, reference_distance_from_wall=0.0, 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 = wall_following_direction + 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 @@ -139,7 +139,7 @@ def command_turn(self, reference_rate): velocity_x is defined in m/s """ velocity_x = 0.0 - rate_yaw = self.wall_following_direction * reference_rate + 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): @@ -152,13 +152,13 @@ def command_align_corner(self, reference_rate, side_range, wanted_distance_from_ velocity_x is defined in m/s """ if side_range > wanted_distance_from_corner + 0.3: - rate_yaw = self.wall_following_direction * reference_rate + 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 * (-1.0 * self.max_forward_speed / self.speed_redux_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 * (self.max_forward_speed / self.speed_redux_corner ) + velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_corner ) rate_yaw = 0.0 return velocity_y, rate_yaw @@ -184,9 +184,9 @@ def command_forward_along_wall(self, side_range): 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 * (-1.0 * self.max_forward_speed / self.speed_redux_straight ) + velocity_y = self.wall_following_direction_value * (-1.0 * self.max_forward_speed / self.speed_redux_straight ) else: - velocity_y = self.wall_following_direction * (self.max_forward_speed / self.speed_redux_straight ) + 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): @@ -199,14 +199,14 @@ def command_turn_around_corner_and_adjust(self, radius, side_range): velocity_x and velocity_y is defined in m/s """ velocity_x = self.max_forward_speed - rate_yaw = self.wall_following_direction * (-1 * velocity_x / radius) + 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 * (-1.0 * self.max_forward_speed / self.speed_redux_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 * (self.max_forward_speed / self.speed_redux_corner) + 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 @@ -245,7 +245,7 @@ def wall_follower(self, front_range, side_range, current_heading, self.state is defined as StateWallFollowing enum """ - self.wall_following_direction = wall_following_direction + self.wall_following_direction_value = float(wall_following_direction.value) self.time_now = time_outer_loop if self.first_run: @@ -267,7 +267,7 @@ def wall_follower(self, front_range, side_range, current_heading, math.cos(0.78) + self.ranger_value_buffer) if side_range_check and front_range_check: self.prev_heading = current_heading - self.wall_angle = self.wall_following_direction * \ + self.wall_angle = self.wall_following_direction_value * \ (1.57 - 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 From 0f759b2ae629f08458e1beef107b2073f484be15 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 2 May 2023 16:07:28 +0200 Subject: [PATCH 566/861] flake8 and autopep8 --- .../multiranger/multiranger_wall_following.py | 8 +- .../wall_following/wall_following.py | 93 ++++++++++--------- 2 files changed, 53 insertions(+), 48 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index eec2bd8ea..d4a7688ef 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -38,9 +38,9 @@ * Flow deck * Multiranger deck """ - import logging import time +from math import degrees from wall_following.wall_following import WallFollowing @@ -53,15 +53,15 @@ from cflib.utils import uri_helper from cflib.utils.multiranger import Multiranger -from math import degrees +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E704') -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() @@ -107,7 +107,7 @@ def handle_range_measurement(range): # choose here the direction that you want the wall following to turn to wall_following_direction = WallFollowing.WallFollowingDirection.RIGHT - side_range = left_range + 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( diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 76efd6c74..1a3ddab5d 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -44,7 +44,7 @@ class StateWallFollowing(Enum): ROTATE_AROUND_WALL = 6 ROTATE_IN_CORNER = 7 FIND_CORNER = 8 - + class WallFollowingDirection(Enum): LEFT = 1 RIGHT = -1 @@ -64,7 +64,6 @@ def __init__(self, reference_distance_from_wall=0.0, in_corner_angle=0.8, wait_for_measurement_seconds=1.0, init_state=StateWallFollowing.FORWARD): - """ __init__ function for the WallFollowing class @@ -72,14 +71,14 @@ def __init__(self, reference_distance_from_wall=0.0, 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 + 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 + 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) @@ -89,9 +88,9 @@ def __init__(self, reference_distance_from_wall=0.0, 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 + 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.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 @@ -129,8 +128,8 @@ def wrap_to_pi(self, number): return number + 2 * math.pi else: return number - - # Command functions + + # Command functions def command_turn(self, reference_rate): """ Command the Crazyflie to turn around its yaw axis @@ -156,9 +155,10 @@ def command_align_corner(self, reference_rate, side_range, wanted_distance_from_ 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 ) + 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 ) + velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_corner) rate_yaw = 0.0 return velocity_y, rate_yaw @@ -175,25 +175,27 @@ 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) + 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 ) + 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 ) + 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 @@ -201,10 +203,12 @@ def command_turn_around_corner_and_adjust(self, radius, side_range): 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) + 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 ) + 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 @@ -307,29 +311,29 @@ def wall_follower(self, front_range, side_range, current_heading, self.state = self.state_transition(self.StateWallFollowing.HOVER) # -------------- Handle state actions ---------------- # - command_velocity_x_temporary = 0.0 - command_velocity_y_temporary = 0.0 - command_angle_rate_temporary = 0.0 + command_velocity_x_temp = 0.0 + command_velocity_y_temp = 0.0 + command_angle_rate_temp = 0.0 match self.state: case self.StateWallFollowing.FORWARD: - command_velocity_x_temporary = self.max_forward_speed - command_velocity_y_temporary = 0.0 - command_angle_rate_temporary = 0.0 + command_velocity_x_temp = self.max_forward_speed + command_velocity_y_temp = 0.0 + command_angle_rate_temp = 0.0 case self.StateWallFollowing.HOVER: - command_velocity_x_temporary, command_velocity_y_temporary, command_angle_rate_temporary = self.command_hover() + command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() case self.StateWallFollowing.TURN_TO_FIND_WALL: - command_velocity_x_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) - command_velocity_y_temporary = 0.0 + command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) + command_velocity_y_temp = 0.0 case self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL: if self.time_now - self.state_start_time < self.wait_for_measurement_seconds: - command_velocity_x_temporary, command_velocity_y_temporary, command_angle_rate_temporary = self.command_hover() + command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() else: - command_velocity_x_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) - command_velocity_y_temporary = 0.0 + command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) + command_velocity_y_temp = 0.0 case self.StateWallFollowing.FORWARD_ALONG_WALL: - command_velocity_x_temporary, command_velocity_y_temporary = self.command_forward_along_wall(side_range) - command_angle_rate_temporary = 0.0 + command_velocity_x_temp, command_velocity_y_temp = self.command_forward_along_wall(side_range) + command_angle_rate_temp = 0.0 case self.StateWallFollowing.ROTATE_AROUND_WALL: # If first time around corner # first try to find the wall again @@ -342,32 +346,33 @@ def wall_follower(self, front_range, side_range, current_heading, # 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_temporary, command_angle_rate_temporary = self.command_turn( + command_velocity_y_temp, command_angle_rate_temp = self.command_turn( -1 * self.max_turn_rate) - command_velocity_x_temporary = 0.0 + command_velocity_x_temp = 0.0 else: - command_velocity_y_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) - command_velocity_x_temporary = 0.0 + 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_temporary, command_velocity_y_temporary, command_angle_rate_temporary = \ + 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) case self.StateWallFollowing.ROTATE_IN_CORNER: - command_velocity_x_temporary, command_angle_rate_temporary = self.command_turn(self.max_turn_rate) - command_velocity_y_temporary = 0.0 + command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) + command_velocity_y_temp = 0.0 case self.StateWallFollowing.FIND_CORNER: - command_velocity_y_temporary, command_angle_rate_temporary = self.command_align_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_temporary = 0.0 + command_velocity_x_temp = 0.0 case _: # state does not exist, so hover! - command_velocity_x_temporary, command_velocity_y_temporary, command_angle_rate_temporary = self.command_hover() + command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() - command_velocity_x = command_velocity_x_temporary - command_velocity_y = command_velocity_y_temporary - command_yaw_rate = command_angle_rate_temporary + 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 From 5e7ed59cf094e69e7307370c34e3894c2b4fa559 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 2 May 2023 16:09:57 +0200 Subject: [PATCH 567/861] address back to default --- examples/multiranger/multiranger_wall_following.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index d4a7688ef..4f76b2b8d 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -53,7 +53,7 @@ from cflib.utils import uri_helper from cflib.utils.multiranger import Multiranger -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E704') +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') def handle_range_measurement(range): From 60523321f82d86fd5fef5bf15c345ce4ba6129a3 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 3 May 2023 09:56:44 +0200 Subject: [PATCH 568/861] Correct nr of deck mem info sections --- cflib/crazyflie/mem/deck_memory.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem/deck_memory.py b/cflib/crazyflie/mem/deck_memory.py index 0e99569a8..f8da789fc 100644 --- a/cflib/crazyflie/mem/deck_memory.py +++ b/cflib/crazyflie/mem/deck_memory.py @@ -182,9 +182,10 @@ class DeckMemoryManager(MemoryElement): """ MAX_NR_OF_DECKS = 4 + MAX_NR_OF_DECK_MEM_INFOS = MAX_NR_OF_DECKS * 2 SIZE_OF_DECK_MEM_INFO = 0x20 SIZE_OF_VERSION = 1 - SIZE_OF_INFO_SECTION = SIZE_OF_VERSION + MAX_NR_OF_DECKS * SIZE_OF_DECK_MEM_INFO + SIZE_OF_INFO_SECTION = SIZE_OF_VERSION + MAX_NR_OF_DECK_MEM_INFOS * SIZE_OF_DECK_MEM_INFO INFO_SECTION_ADDRESS = 0 COMMAND_SECTION_ADDRESS = 0x1000 SIZE_OF_COMMAND_SECTION = 0x20 @@ -276,7 +277,7 @@ def _parse_info_section(self, data): if version != self.SUPPORTED_VERSION: raise RuntimeError(f'Deck memory version {version} not supported') else: - for i in range(self.MAX_NR_OF_DECKS): + for i in range(self.MAX_NR_OF_DECK_MEM_INFOS): deck_memory = DeckMemory(self, self.COMMAND_SECTION_ADDRESS + i * self.SIZE_OF_COMMAND_SECTION) start = self.SIZE_OF_VERSION + self.SIZE_OF_DECK_MEM_INFO * i end = start + self.SIZE_OF_DECK_MEM_INFO From 47d5552bcb869f09f2c212ed8f78f672c2fcef57 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 3 May 2023 09:57:23 +0200 Subject: [PATCH 569/861] Correct indexing when flashing decks --- cflib/bootloader/__init__.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 663d73809..85ac5c996 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -424,7 +424,12 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis time.sleep(2) raise RuntimeError(message) - for (deck_index, deck) in decks.items(): + # The decks dictionary uses the deck index as the key. Note that all indexes might not be present as some + # decks do not support memory operations. decks.keys() might return the set {2, 4}. + + for deck_index in sorted(decks.keys()): + deck = decks[deck_index] + if self.terminate_flashing_cb and self.terminate_flashing_cb(): raise Exception('Flashing terminated') @@ -508,8 +513,6 @@ def progress_cb(msg: str, percent: int): # We flashed a deck, return for re-boot next_index = deck_index + 1 - if next_index >= len(decks): - next_index = -1 return next_index # We have flashed the last deck From e12a5d31ad85267dad8493bd46c678ec68c230a8 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 3 May 2023 11:03:10 +0200 Subject: [PATCH 570/861] changes suggested by kristoffer --- examples/multiranger/multiranger_wall_following.py | 7 +++++-- .../multiranger/wall_following/wall_following.py | 13 ++++++++----- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index 4f76b2b8d..e07f6dc7a 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -40,7 +40,6 @@ """ import logging import time -from math import degrees from wall_following.wall_following import WallFollowing @@ -53,6 +52,10 @@ from cflib.utils import uri_helper from cflib.utils.multiranger import Multiranger +from wall_following.wall_following import WallFollowing + +from math import degrees, radians + URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -97,7 +100,7 @@ def handle_range_measurement(range): log_entry = logger.next() data = log_entry[1] actual_yaw = data['stabilizer.yaw'] - actual_yaw_rad = actual_yaw * 3.1415 / 180 + actual_yaw_rad = radians(actual_yaw) # get front range in meters front_range = handle_range_measurement(multiranger.front) diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 1a3ddab5d..fd91d2e39 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -150,7 +150,7 @@ def command_align_corner(self, reference_rate, side_range, wanted_distance_from_ reference_rate and rate_yaw is defined in rad/s velocity_x is defined in m/s """ - if side_range > wanted_distance_from_corner + 0.3: + 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: @@ -265,16 +265,19 @@ def wall_follower(self, front_range, side_range, current_heading, case self.StateWallFollowing.HOVER: print('hover') case 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(0.78) + self.ranger_value_buffer) + math.cos(math.pi/4) + self.ranger_value_buffer) front_range_check = front_range < (self.reference_distance_from_wall / - math.cos(0.78) + self.ranger_value_buffer) + 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 * \ - (1.57 - math.atan(front_range / side_range) + self.angle_value_buffer) + (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 + # 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 From 0af42bb943544aca947f26910a88a62476c196be Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 3 May 2023 11:04:01 +0200 Subject: [PATCH 571/861] format --- examples/multiranger/multiranger_wall_following.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index e07f6dc7a..710f9b6c1 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -40,6 +40,8 @@ """ import logging import time +from math import degrees +from math import radians from wall_following.wall_following import WallFollowing @@ -52,10 +54,6 @@ from cflib.utils import uri_helper from cflib.utils.multiranger import Multiranger -from wall_following.wall_following import WallFollowing - -from math import degrees, radians - URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') From c8f2be74d486be79d8fecbf5c7ff00b55eca5739 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 5 May 2023 13:07:41 +0200 Subject: [PATCH 572/861] remove switch case and update explanation --- .../wall_following/wall_following.py | 228 +++++++++--------- 1 file changed, 116 insertions(+), 112 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index fd91d2e39..bdc22cbbc 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -1,11 +1,12 @@ -#!/usr/bin/env python3 # -*- coding: utf-8 -*- # -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# ........... ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,..´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +....... /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# GNU general public license v3.0 # # Copyright (C) 2023 Bitcraze AB # @@ -23,13 +24,18 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . """ -Helper class for the wall following demo +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 @@ -258,121 +264,119 @@ def wall_follower(self, front_range, side_range, current_heading, self.first_run = False # -------------- Handle state transitions ---------------- # - match self.state: - case 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) - case self.StateWallFollowing.HOVER: - print('hover') - case 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) - case 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) - case 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) - case 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) - case 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) - case self.StateWallFollowing.FIND_CORNER: - if side_range <= self.reference_distance_from_wall: - self.state = self.state_transition(self.StateWallFollowing.ROTATE_AROUND_WALL) - case _: - self.state = self.state_transition(self.StateWallFollowing.HOVER) + 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 - match self.state: - case self.StateWallFollowing.FORWARD: - command_velocity_x_temp = self.max_forward_speed - command_velocity_y_temp = 0.0 - command_angle_rate_temp = 0.0 - case self.StateWallFollowing.HOVER: + 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() - case self.StateWallFollowing.TURN_TO_FIND_WALL: + else: command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) command_velocity_y_temp = 0.0 - case 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() + 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_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) - command_velocity_y_temp = 0.0 - case 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 - case 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) - case 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 - case 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 - case _: - # state does not exist, so hover! - command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() + 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 From f4817680469889ae33178a4a8c228bf0c8175011 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 5 May 2023 13:14:22 +0200 Subject: [PATCH 573/861] import order --- examples/multiranger/wall_following/wall_following.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index bdc22cbbc..deabf629f 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -34,8 +34,6 @@ Author: Kimberly McGuire (Bitcraze AB) """ - - import math from enum import Enum From 3998da08ab50a50025bc189cb56fdfd31f0e9ad3 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Mon, 8 May 2023 14:22:04 +0200 Subject: [PATCH 574/861] Delete PID_controller_tuner.py --- examples/tuning/PID_controller_tuner.py | 398 ------------------------ 1 file changed, 398 deletions(-) delete mode 100644 examples/tuning/PID_controller_tuner.py diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py deleted file mode 100644 index 21a160d23..000000000 --- a/examples/tuning/PID_controller_tuner.py +++ /dev/null @@ -1,398 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2011-2013 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 . -""" -Gui une the PID controller of the crazyflie - -""" -import logging -import sys -import time - -import matplotlib.pyplot as plt -import tkinter as tk -from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg - -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.utils import uri_helper - -# URI to the Crazyflie to connect to -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -STANDARD_HEIGHT = 0.8 -STEP_RESPONSE_TIME = 3.0 -STEP_SIZE = -0.2 # meters - -if len(sys.argv) > 1: - URI = sys.argv[1] -elif len(sys.argv) > 2: - STANDARD_HEIGHT = sys.argv[2] - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class TunerGUI: - def __init__(self, master): - self.master = master - self.master.title('PID tuner Crazyflie') - - self.figplot = plt.Figure(figsize=(2, 1), dpi=100) - self.ax2 = self.figplot.add_subplot(111) - self.line2 = FigureCanvasTkAgg(self.figplot, self.master) - self.line2.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=1) - - self.scale_Kp = tk.Scale(master, label='scale_Kp', from_=0, to=100, - length=1200, tickinterval=5, resolution=0.1, - orient=tk.HORIZONTAL) - self.scale_Ki = tk.Scale(master, label='scale_Ki', from_=0, to=50, - length=1200, tickinterval=3, resolution=0.1, - orient=tk.HORIZONTAL) - self.scale_Kd = tk.Scale(master, label='scale_Kd', from_=0, to=50, - length=1200, tickinterval=3, resolution=0.1, - orient=tk.HORIZONTAL) - self.scale_vMax = tk.Scale(master, label='vMax', from_=0, to=5, - length=1200, tickinterval=5, resolution=0.1, - orient=tk.HORIZONTAL) - - self.scale_Kp.pack() - self.scale_Ki.pack() - self.scale_Kd.pack() - self.scale_vMax.pack() - - self.pos_array_prev = [] - self.sp_array_prev = [] - self.time_array_prev = [] - - def draw_plot(self, time_array, pos_array, sp_array): - - self.ax2.clear() - self.ax2.plot(self.time_array_prev, self.pos_array_prev, - label='pos_z', color='red', alpha=0.5) - - self.ax2.plot(time_array, pos_array, label='pos', color='red') - self.ax2.plot(time_array, sp_array, label='sp', color='blue') - self.line2.draw() - self.pos_array_prev = pos_array - self.sp_array_prev = sp_array - self.time_array_prev = time_array - - def clear_plot(self): - self.ax2.clear() - self.line2.draw() - self.time_array_prev = [] - self.pos_array_prev = [] - self.sp_array_prev = [] - - -class TunerControlCF: - def __init__(self, pid_gui, scf): - self.cf = scf - self.pid_gui = pid_gui - - self.label = tk.Label(self.pid_gui.master, text='Choose an axis!') - self.label.pack() - variable = tk.StringVar(self.pid_gui.master) - variable.set('z') - self.dropdown = tk.OptionMenu( - self.pid_gui.master, variable, 'x', 'y', 'z', - command=self.change_param_axis_callback) - self.dropdown.pack() - self.axis_choice = 'z' - - self.label = tk.Label(self.pid_gui.master, - text='Choose velocity or position!') - self.label.pack() - variable_pos = tk.StringVar(self.pid_gui.master) - variable_pos.set('pos') - self.dropdown = tk.OptionMenu( - self.pid_gui.master, variable_pos, 'pos', 'vel', - command=self.change_param_unit_callback) - self.dropdown.pack() - self.unit_choice = 'pos' - - self.button_send = tk.Button( - self.pid_gui.master, text='SEND PID GAINS', - command=self.send_pid_gains).pack() - self.button_step = tk.Button( - self.pid_gui.master, text='DO STEP', - command=self.do_step).pack() - self.button_quit = tk.Button( - self.pid_gui.master, text='STOP', - command=self.stop_gui).pack() - - self.cf.param.add_update_callback( - group='posCtlPid', name='zKp', cb=self.param_updated_callback_Kp) - self.cf.param.add_update_callback( - group='posCtlPid', name='zKi', cb=self.param_updated_callback_Ki) - self.cf.param.add_update_callback( - group='posCtlPid', name='zKd', cb=self.param_updated_callback_Kd) - self.cf.param.add_update_callback( - group='posCtlPid', name='xVelMax', - cb=self.param_updated_callback_vMax) - self.cf.param.add_update_callback( - group='posCtlPid', name='yVelMax', - cb=self.param_updated_callback_vMax) - - self.current_value_kp = 0 - self.current_value_kd = 0 - self.current_value_ki = 0 - self.current_value_vmax = 0 - - self.cf.param.request_param_update('posCtlPid.zKp') - self.cf.param.request_param_update('posCtlPid.zKi') - self.cf.param.request_param_update('posCtlPid.zKd') - self.cf.param.request_param_update('posCtlPid.xVelMax') - self.cf.param.request_param_update('posCtlPid.yVelMax') - - time.sleep(0.1) - - self.update_scale_info() - - self.commander = cf.high_level_commander - self.take_off(STANDARD_HEIGHT) - - def update_scale_info(self): - print('update info') - self.pid_gui.scale_Kp.set(self.current_value_kp) - self.pid_gui.scale_Kd.set(self.current_value_kd) - self.pid_gui.scale_Ki.set(self.current_value_ki) - self.pid_gui.scale_vMax.set(self.current_value_vmax) - - # Buttons - - def send_pid_gains(self): - print('Sending to the ' + self.axis_choice + - 'position PID controller: Kp: ' + - str(self.pid_gui.scale_Kp.get()) + - ', Ki: ' + str(self.pid_gui.scale_Ki.get()) + - ', Kd: ' + str(self.pid_gui.scale_Kd.get())) - cf.param.set_value(self.unit_choice + 'CtlPid.' + self.axis_choice + - 'Kp', self.pid_gui.scale_Kp.get()) - cf.param.set_value(self.unit_choice + 'CtlPid.' + self.axis_choice + - 'Ki', self.pid_gui.scale_Ki.get()) - cf.param.set_value(self.unit_choice + 'CtlPid.' + self.axis_choice + - 'Kd', self.pid_gui.scale_Kd.get()) - cf.param.set_value('posCtlPid.xVelMax', self.pid_gui.scale_vMax.get()) - cf.param.set_value('posCtlPid.yVelMax', self.pid_gui.scale_vMax.get()) - - time.sleep(0.1) - - self.update_scale_info() - - def do_step(self): - print('do step') - log_config = LogConfig(name='Position setpoint', period_in_ms=10) - log_config.add_variable('stateEstimate.' + self.axis_choice, 'float') - log_config.add_variable('ctrltarget.' + self.axis_choice, 'float') - - if self.axis_choice == 'z': - self.commander.go_to(0, 0, STEP_SIZE, 0, 0.6, relative=True) - elif self.axis_choice == 'x': - self.commander.go_to(STEP_SIZE, 0, 0, 0, 0.6, relative=True) - elif self.axis_choice == 'y': - self.commander.go_to(0, STEP_SIZE, 0, 0, 0.6, relative=True) - else: - print('WRONG CHOICE?!?!') - self.stop_gui() - - current_time = time.time() - - pos_history = [] - sp_history = [] - time_history = [] - with SyncLogger(self.cf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - pos_history.append(data['stateEstimate.' + self.axis_choice]) - sp_history.append(data['ctrltarget.' + self.axis_choice]) - time_history.append(time.time() - current_time) - - if ((time.time() - current_time) > STEP_RESPONSE_TIME): - break - # print(pos_history) - # print(sp_history) - self.pid_gui.draw_plot(time_history, pos_history, sp_history) - if self.axis_choice == 'z': - self.commander.go_to(0, 0, -1 * STEP_SIZE, 0, 1.0, relative=True) - elif self.axis_choice == 'x': - self.commander.go_to(-1 * STEP_SIZE, 0, 0, 0, 1.0, relative=True) - elif self.axis_choice == 'y': - self.commander.go_to(0, -1 * STEP_SIZE, 0, 0, 1.0, relative=True) - else: - print('WRONG CHOICE?!?!') - self.stop_gui() - - def stop_gui(self): - self.pid_gui.master.quit() - self.land_and_stop() - - # parameter update - def change_param_axis_callback(self, value_axis): - # - print(self.unit_choice + 'CtlPid.' + value_axis) - - groupname = self.unit_choice + 'CtlPid' - self.cf.param.remove_update_callback( - group=groupname, name=self.axis_choice + 'Kp') - self.cf.param.remove_update_callback( - group=groupname, name=self.axis_choice + 'Ki') - self.cf.param.remove_update_callback( - group=groupname, name=self.axis_choice + 'Kd') - - time.sleep(0.1) - self.cf.param.add_update_callback( - group=groupname, name=value_axis + - 'Kp', cb=self.param_updated_callback_Kp) - self.cf.param.add_update_callback( - group=groupname, name=value_axis + - 'Ki', cb=self.param_updated_callback_Ki) - self.cf.param.add_update_callback( - group=groupname, name=value_axis + - 'Kd', cb=self.param_updated_callback_Kd) - - self.cf.param.request_param_update(groupname + '.' + value_axis + 'Kp') - self.cf.param.request_param_update(groupname + '.' + value_axis + 'Ki') - self.cf.param.request_param_update(groupname + '.' + value_axis + 'Kd') - time.sleep(0.1) - - self.update_scale_info() - self.pid_gui.clear_plot() - self.axis_choice = value_axis - - # parameter update - def change_param_unit_callback(self, value_unit): - # - print(value_unit + 'CtlPid.' + self.axis_choice) - - groupname_old = self.unit_choice + 'CtlPid' - self.cf.param.remove_update_callback( - group=groupname_old, name=self.axis_choice + 'Kp') - self.cf.param.remove_update_callback( - group=groupname_old, name=self.axis_choice + 'Ki') - self.cf.param.remove_update_callback( - group=groupname_old, name=self.axis_choice + 'Kd') - - time.sleep(0.1) - groupname_new = value_unit + 'CtlPid' - self.cf.param.add_update_callback( - group=groupname_new, name=self.axis_choice + - 'Kp', cb=self.param_updated_callback_Kp) - self.cf.param.add_update_callback( - group=groupname_new, name=self.axis_choice + - 'Ki', cb=self.param_updated_callback_Ki) - self.cf.param.add_update_callback( - group=groupname_new, name=self.axis_choice + - 'Kd', cb=self.param_updated_callback_Kd) - - print(groupname_new + '.' + self.axis_choice + 'Kp') - self.cf.param.request_param_update( - groupname_new + '.' + self.axis_choice + 'Kp') - self.cf.param.request_param_update( - groupname_new + '.' + self.axis_choice + 'Ki') - self.cf.param.request_param_update( - groupname_new + '.' + self.axis_choice + 'Kd') - time.sleep(0.1) - - self.update_scale_info() - - self.unit_choice = value_unit - - def param_updated_callback_Kp(self, name, value): - self.current_value_kp = float(value) - - def param_updated_callback_Ki(self, name, value): - self.current_value_ki = float(value) - - def param_updated_callback_Kd(self, name, value): - self.current_value_kd = float(value) - - def param_updated_callback_vMax(self, name, value): - self.current_value_vmax = float(value) - - def take_off(self, height): - self.commander.takeoff(height, 2.0) - - def land_and_stop(self): - self.commander.land(0.0, 2.0) - time.sleep(2) - self.commander.stop() - - -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -if __name__ == '__main__': - - root = tk.Tk() - pid_gui = TunerGUI(root) - - cflib.crtp.init_drivers() - cf = Crazyflie(rw_cache='./cache') - with SyncCrazyflie(URI, cf) as scf: - wait_for_position_estimator(scf) - cf_ctrl = TunerControlCF(pid_gui, cf) - tk.mainloop() From 15fee7a5adb4041686296dfe9d7e070c2c0dda92 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 16 May 2023 15:00:41 +0200 Subject: [PATCH 575/861] Added arming --- cflib/crazyflie/platformservice.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index e1c8d398c..8b96c214a 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -40,6 +40,7 @@ APP_CHANNEL = 2 PLATFORM_SET_CONT_WAVE = 0 +PLATFORM_REQUEST_ARMING = 1 VERSION_GET_PROTOCOL = 0 VERSION_GET_FIRMWARE = 1 @@ -82,7 +83,20 @@ def set_continous_wave(self, enabled): """ pk = CRTPPacket() pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND) - pk.data = (0, enabled) + + pk.data = (PLATFORM_SET_CONT_WAVE, enabled) + self._cf.send_packet(pk) + + 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 + """ + pk = CRTPPacket() + pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND) + pk.data = (PLATFORM_REQUEST_ARMING, do_arm) self._cf.send_packet(pk) def get_protocol_version(self): From e9d5972811b7f6f627454ab66f509f7edfc2b636 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 30 May 2023 14:12:06 +0200 Subject: [PATCH 576/861] Graceful handling of missing bcdDevice --- cflib/drivers/cfusb.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/cflib/drivers/cfusb.py b/cflib/drivers/cfusb.py index 124e27061..80d12a321 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -102,9 +102,10 @@ def __init__(self, device=None, devid=0): self.dev.set_configuration() self.handle = self.dev - self.version = float( - '{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, - self.dev.bcdDevice & 0x0FF)) + if self.dev: + self.version = float('{0:x}.{1:x}'.format(self.dev.bcdDevice >> 8, self.dev.bcdDevice & 0x0FF)) + else: + self.version = 0.0 def get_serial(self): # The signature for get_string has changed between versions to 1.0.0b1, From 1f2c36105b87e1cf329113d61a2ca1dbdb7143c7 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 30 May 2023 16:10:14 +0200 Subject: [PATCH 577/861] Update version to 0.1.23 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 118ad7a63..dfa9aca71 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.22', + version='0.1.23', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From 57038713892c39b899368732c15fc88da14cceb1 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 26 Jun 2023 15:09:58 +0200 Subject: [PATCH 578/861] Add reset method to the log sub system --- cflib/crazyflie/log.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 47e146a7a..728bb0086 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -508,6 +508,13 @@ def add_config(self, logconf): 'The log configuration is too large or has an invalid ' 'parameter') + def reset(self): + """ + Reset the log system and remove all log blocks + """ + self.log_blocks = [] + self._send_reset_packet() + def refresh_toc(self, refresh_done_callback, toc_cache): """Start refreshing the table of loggale variables""" @@ -517,6 +524,9 @@ def refresh_toc(self, refresh_done_callback, toc_cache): self._refresh_callback = refresh_done_callback self.toc = None + self._send_reset_packet() + + def _send_reset_packet(self): pk = CRTPPacket() pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS) pk.data = (CMD_RESET_LOGGING,) From 76beea929132dc7e5bd6e25035611f29e0225b81 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Jul 2023 13:53:15 +0200 Subject: [PATCH 579/861] Update sbs motion commander --- docs/user-guides/sbs_motion_commander.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index ab8f72b42..edee19cd0 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -3,7 +3,7 @@ title: "Step-by-Step: Motion Commander" page_id: sbs_motion_commander --- -Here we will go through step-by-step how to make your crazyflie move based on a motion script. For the first part of this tutorial, you just need the crazyflie and the flowdeck. For the second part, it would be handy to have the multiranger present. +Here we will go through step-by-step how to make your crazyflie move based on a motion script. For the first part of this tutorial, you just need the crazyflie and the flow deck. For the second part, it would be handy to have the multiranger present. ## Prerequisites @@ -46,11 +46,11 @@ This probably all looks pretty familiar, except for one thing line, namely: `from cflib.positioning.motion_commander import MotionCommander` -This imports the motion commander, which is pretty much a wrapper around the position setpoint frame work of the crazyflie. You probably have unknowingly experienced this a when trying out the assist modes in this [tutorial with the flowdeck in the cfclient](https://www.bitcraze.io/documentation/tutorials/getting-started-with-flow-deck/) +This imports the motion commander, which is pretty much a wrapper around the position setpoint frame work of the crazyflie. You probably have unknowingly experienced this a when trying out the assist modes in this [tutorial with the flow deck in the cfclient](https://www.bitcraze.io/documentation/tutorials/getting-started-with-flow-deck/) ## Step 1: Security before flying -Since this tutorial won't be a table top tutorial like last time, but an actual flying one, we need to put some securities in place. The flowdeck or any other positioning deck that you are using, should be correctly attached to the crazyflie. If it is not, it will try to fly anyway without a good position estimate and for sure is going to crash. +Since this tutorial won't be a table top tutorial like last time, but an actual flying one, we need to put some securities in place. The flow deck that you are using, should be correctly attached to the crazyflie. If it is not, it will try to fly anyway without a good position estimate and for sure is going to crash. We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...SyncCrazyflie(...)` in `__main__` ```python @@ -82,7 +82,7 @@ URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') deck_attached_event = Event() ``` -Try to run the script now, and try to see if it is able to detect that the flowdeck (or any other positioning deck), is correctly attached. Also try to remove it to see if it can detect it missing as well. +Try to run the script now, and try to see if it is able to detect that the flow deck, is correctly attached. Also try to remove it to see if it can detect it missing as well. This is the full script as we are: ```python @@ -564,7 +564,7 @@ Let's take it up a notch! Replace the content in the while loop with the followi This will now start a linear motion into a certain direction, and makes the Crazyflie bounce around in a virtual box of which the size is indicated by 'BOX_LIMIT'. So before you fly make sure that you pick a box_limit small enough so that it able to fit in your flying area. -**Note**: if you are using the flowdeck, it might be that the orientation of this box will seem to change. This is due to that the flowdeck is not able to provide an absolute heading estimate, which will be only based on gyroscope measurements. This will drift over time, which is accelerated if you incorporate many turns in your application. There are also reports that happens quickly when the crazyflie is still on the ground. This should not happen with MoCap or the lighthouse deck. +**Note**: if you are using the flow deck, it might be that the orientation of this box will seem to change. This is due to that the flow deck is not able to provide an absolute heading estimate, which will be only based on gyroscope measurements. This will drift over time, which is accelerated if you incorporate many turns in your application. There are also reports that happens quickly when the crazyflie is still on the ground. This should not happen with MoCap or the lighthouse deck. Check out if your code still matches the full code and run the script! From d9e737689b52d20a53c9be23b7d26e7d8e08dcd0 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Wed, 26 Jul 2023 14:02:35 +0200 Subject: [PATCH 580/861] Update sbs_motion_commander.md --- docs/user-guides/sbs_motion_commander.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index edee19cd0..f117801e9 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -84,6 +84,8 @@ deck_attached_event = Event() Try to run the script now, and try to see if it is able to detect that the flow deck, is correctly attached. Also try to remove it to see if it can detect it missing as well. +> Note: You can use a different positioning system and deck if you'd like, but you've got to look at a different parameter than "deck.bcFlow2". Check out other options in the [parameter list](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/params/#deck). + This is the full script as we are: ```python import logging From 207d123126529a0d7e047820f1183fa5e7e709b6 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 15 Aug 2023 10:37:07 +0200 Subject: [PATCH 581/861] Fix flake8 issues --- cflib/crtp/crtpstack.py | 8 ++++---- cflib/crtp/radiodriver.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/crtp/crtpstack.py b/cflib/crtp/crtpstack.py index 929ee58a2..2cb9118e7 100644 --- a/cflib/crtp/crtpstack.py +++ b/cflib/crtp/crtpstack.py @@ -119,13 +119,13 @@ def _get_data(self): def _set_data(self, data): """Set the packet data""" - if type(data) == bytearray: + if isinstance(data, bytearray): self._data = data - elif type(data) == str: + elif isinstance(data, str): self._data = bytearray(data.encode('ISO-8859-1')) - elif type(data) == list or type(data) == tuple: + elif isinstance(data, list) or isinstance(data, tuple): self._data = bytearray(data) - elif type(data) == bytes: + elif isinstance(data, bytes): self._data = bytearray(data) else: raise Exception('Data must be bytearray, string, list or tuple,' diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index f0bf50eb7..7d22b6c52 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -662,7 +662,7 @@ def run(self): if outPacket: dataOut.append(outPacket.header) for X in outPacket.data: - if type(X) == int: + if isinstance(X, int): dataOut.append(X) else: dataOut.append(ord(X)) From 6cfb9b96cdf973fb1f81e854c7f81a7cffb3bc32 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 15 Aug 2023 10:44:05 +0200 Subject: [PATCH 582/861] More flake8 fixes --- cflib/crazyflie/param.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 4f77c9457..bc339eb9e 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -109,7 +109,7 @@ def __init__(self, ident=0, data=None): self.name = strs[1] metadata = data[0] - if type(metadata) == str: + if isinstance(metadata, str): metadata = ord(metadata) # If the fouth byte (1 << 4) (0x10) is set we have extended From 8d2769237dae4513c4e69e58f3b5789239b24d13 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 12:39:45 +0200 Subject: [PATCH 583/861] Removed old fp16 file --- cflib/utils/fp16.py | 53 --------------------------------------------- 1 file changed, 53 deletions(-) delete mode 100644 cflib/utils/fp16.py diff --git a/cflib/utils/fp16.py b/cflib/utils/fp16.py deleted file mode 100644 index 5eb47ee5b..000000000 --- a/cflib/utils/fp16.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2020 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 . -import struct - - -# Code from davidejones at https://gamedev.stackexchange.com/a/28756 -def fp16_to_float(float16): - s = int((float16 >> 15) & 0x00000001) # sign - e = int((float16 >> 10) & 0x0000001f) # exponent - f = int(float16 & 0x000003ff) # fraction - - if e == 0: - if f == 0: - return int(s << 31) - else: - while not (f & 0x00000400): - f <<= 1 - e -= 1 - e += 1 - f &= ~0x00000400 - # print(s,e,f) - elif e == 31: - if f == 0: - return int((s << 31) | 0x7f800000) - else: - return int((s << 31) | 0x7f800000 | (f << 13)) - - e += 127 - 15 - f <<= 13 - result = int((s << 31) | (e << 23) | f) - return struct.unpack('f', struct.pack('I', result))[0] From f7963c4460fbf57acb0829b722e97ddd7294d629 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 12:52:57 +0200 Subject: [PATCH 584/861] Corrections --- cflib/crazyflie/commander.py | 33 ++++--- cflib/crazyflie/localization.py | 2 +- cflib/utils/encoding.py | 48 +++++++--- examples/autonomy/full_state_setpoint_demo.py | 93 +++++++++---------- test/utils/test_encoding.py | 66 +++++++++++++ 5 files changed, 166 insertions(+), 76 deletions(-) create mode 100644 test/utils/test_encoding.py diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index f4c8f4a23..df5a37752 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -7,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2011-2013 Bitcraze AB +# Copyright (C) 2011-2023 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -138,25 +138,34 @@ def send_hover_setpoint(self, vx, vy, yawrate, zdistance): vx, vy, yawrate, zdistance) self._cf.send_packet(pk) - def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, rollrate, pitchrate, yawrate): + def send_full_state_setpoint(self, pos, vel, acc, orientation, rollrate, pitchrate, yawrate): """ - Control mode where the position, velocity, acceleration, orientation, angular + Control mode where the position, velocity, acceleration, orientation and angular velocity are sent as absolute (world) values. - x, y, z are in m - vx, vy, vz are in m/s - ax, ay, az are in m/s^2 - qx, qy, qz, qw are the quaternion components of the orientation + position [x, y, z] are in m + velocity [vx, vy, vz] are in m/s + acceleration [ax, ay, az] are in m/s^2 + orientation [qx, qy, qz, qw] are the quaternion components of the orientation rollrate, pitchrate, yawrate are in degrees/s """ + def vector_to_mm_16bit(vec): + return int(vec[0] * 1000), int(vec[1] * 1000), int(vec[2] * 1000) + + x, y, z = vector_to_mm_16bit(pos) + vx, vy, vz = vector_to_mm_16bit(vel) + ax, ay, az = vector_to_mm_16bit(acc) + rr, pr, yr = vector_to_mm_16bit([rollrate, pitchrate, yawrate]) + orient_comp = compress_quaternion(orientation) + pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('. import struct -from math import sqrt import numpy as np + # Code from davidejones at https://gamedev.stackexchange.com/a/28756 def fp16_to_float(float16): s = int((float16 >> 15) & 0x00000001) # sign @@ -54,9 +52,18 @@ def fp16_to_float(float16): result = int((s << 31) | (e << 23) | f) return struct.unpack('f', struct.pack('I', result))[0] -# decompress a quaternion, see quatcompress.h in firmware -# input: 32-bit number, output q = [x,y,z,w] + def decompress_quaternion(comp): + """Decompress a quaternion + + see quatcompress.h in the firmware for definitions + + Args: + comp int: A 32-bit number + + Returns: + np array: q = [x, y, z, w] + """ q = np.zeros(4) mask = (1 << 9) - 1 i_largest = comp >> 30 @@ -73,23 +80,34 @@ def decompress_quaternion(comp): q[i_largest] = np.sqrt(1.0 - sum_squares) return q -# compress a quaternion, see quatcompress.h in firmware -# input: 32-bit number, output q = [x,y,z,w] -def compress_quaternion(qx, qy, qz, qw): - q = [qx, qy, qz, qw] + +def compress_quaternion(quat): + """Compress a quaternion. + assumes input quaternion is normalized. will fail if not. + + see quatcompress.h in firmware the for definitions + + Args: + quat : An array of floats representing a quaternion [x, y, z, w] + + Returns: 32-bit integer + """ + # Normalize the quaternion + quat_n = np.array(quat) / np.linalg.norm(quat) + i_largest = 0 for i in range(1, 4): - if abs(q[i]) > abs(q[i_largest]): + if abs(quat_n[i]) > abs(quat_n[i_largest]): i_largest = i - negate = q[i_largest] < 0 + negate = quat_n[i_largest] < 0 M_SQRT1_2 = 1.0 / np.sqrt(2) comp = i_largest for i in range(4): if i != i_largest: - negbit = (q[i] < 0) ^ negate - mag = int(((1 << 9) - 1) * (abs(q[i]) / M_SQRT1_2) + 0.5) + negbit = int((quat_n[i] < 0) ^ negate) + mag = int(((1 << 9) - 1) * (abs(quat_n[i]) / M_SQRT1_2) + 0.5) comp = (comp << 10) | (negbit << 9) | mag - return comp \ No newline at end of file + return comp diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index a25bbdc0d..f1920863d 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -6,9 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2016 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client +# 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 @@ -22,10 +20,11 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . """ -Used for sending full state control setpoints to the Crazyflie +Shows how to send full state control setpoints to the Crazyflie """ import time -import logging + +from scipy.spatial.transform import Rotation import cflib.crtp from cflib.crazyflie import Crazyflie @@ -33,30 +32,24 @@ from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper -import math -from cflib.crazyflie.log import LogConfig - # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/65/2M/E7E7E7E7F2') -def quaternion_from_euler(roll, pitch, yaw): - cy = math.cos(yaw * 0.5) - sy = math.sin(yaw * 0.5) - cp = math.cos(pitch * 0.5) - sp = math.sin(pitch * 0.5) - cr = math.cos(roll * 0.5) - sr = math.sin(roll * 0.5) +def quaternion_from_euler(roll: float, pitch: float, yaw: float): + """Convert Euler angles to quaternion - q = [0] * 4 - q[0] = sr * cp * cy - cr * sp * sy - q[1] = cr * sp * cy + sr * cp * sy - q[2] = cr * cp * sy - sr * sp * cy - q[3] = cr * cp * cy + sr * sp * sy + Args: + roll (float): roll, in radians + pitch (float): pitch, in radians + yaw (float): yaw, in radians - return q + Returns: + array: the quaternion [x, y, z, w] + """ + return Rotation.from_euler(seq='xyz', angles=(roll, pitch, yaw), degrees=False).as_quat() def wait_for_position_estimator(scf): @@ -127,46 +120,48 @@ def start_position_printing(scf): 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') - # quaternion from roll pitch yaw - roll = 0.0 - pitch = 0.0 - yaw = 0.0 #20.0*math.pi/180.0 - q = quaternion_from_euler(roll, pitch, yaw) print('takeoff') - cf.commander.send_full_state_setpoint(0.0,0.0,1.0, - 0.0,0.0,0.0, - 0.0,0.0,0.0, - q[0],q[1],q[2],q[3], - 0.0,0.0,0.0) - - time.sleep(2.0) - cf.commander.send_full_state_setpoint(0.0,0.0,1.0, - 0.0,0.0,0.0, - 0.0,0.0,0.0, - q[0],q[1],q[2],q[3], - 0.0,0.0,0.0) - time.sleep(2.0) + send_setpoint(cf, 4.0, + [0.0, -1.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, -1.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) print('land') - cf.commander.send_full_state_setpoint(0.0,0.0,0.2, - 0.0,0.0,0.0, - 0.0,0.0,0.0, - q[0],q[1],q[2],q[3], - 0.0,0.0,0.0) - - time.sleep(2.0) - + send_setpoint(cf, 2.0, + [0.0, -1.0, 0.2], + [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() # 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'], @@ -174,6 +169,8 @@ def _stab_log_data(timestamp, data, logconf): print('ctrltarget.x: {}, ctrltarget.y: {}, ctrltarget.z: {}'.format(data['ctrltarget.x'], data['ctrltarget.y'], data['ctrltarget.z'])) + + if __name__ == '__main__': cflib.crtp.init_drivers() @@ -190,5 +187,5 @@ def _stab_log_data(timestamp, data, logconf): _lg_stab.data_received_cb.add_callback(_stab_log_data) _lg_stab.start() - #reset_estimator(scf) + reset_estimator(scf) run_sequence(scf) diff --git a/test/utils/test_encoding.py b/test/utils/test_encoding.py new file mode 100644 index 000000000..1d1904eda --- /dev/null +++ b/test/utils/test_encoding.py @@ -0,0 +1,66 @@ +# -*- 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 . +import unittest +import numpy as np +from cflib.utils.encoding import compress_quaternion, decompress_quaternion + + +class EncodingTest(unittest.TestCase): + + def test_compress_decompress(self): + # Fixture + expected = self._normalize_quat([1, 2, 3, 4]) + + # Test + compressed = compress_quaternion(expected) + actual = decompress_quaternion(compressed) + + # Assert + np.testing.assert_allclose(actual, expected, 0.001) + + def test_compress_decompress_not_normalized(self): + # Fixture + quat = [1, 2, 3, 4] + expected = self._normalize_quat(quat) + + # Test + compressed = compress_quaternion(quat) + actual = decompress_quaternion(compressed) + + # Assert + np.testing.assert_allclose(actual, expected, 0.001) + + def test_other_largest_component(self): + # Fixture + quat = [5, 10, 3, 4] + expected = self._normalize_quat(quat) + + # Test + compressed = compress_quaternion(quat) + actual = decompress_quaternion(compressed) + + # Assert + np.testing.assert_allclose(actual, expected, 0.001) + + def _normalize_quat(self, quat): + quat = np.array(quat) + return quat / np.linalg.norm(quat) From 5e94b13b7467f9bbf6e46f9f2e0464b3c6f8a134 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 13:00:15 +0200 Subject: [PATCH 585/861] Reorder imports --- test/utils/test_encoding.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/test/utils/test_encoding.py b/test/utils/test_encoding.py index 1d1904eda..57d97e72a 100644 --- a/test/utils/test_encoding.py +++ b/test/utils/test_encoding.py @@ -20,8 +20,11 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import unittest + import numpy as np -from cflib.utils.encoding import compress_quaternion, decompress_quaternion + +from cflib.utils.encoding import compress_quaternion +from cflib.utils.encoding import decompress_quaternion class EncodingTest(unittest.TestCase): From 1920ee7f338cda64ce062e9b57da2adc29acb129 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 13:17:08 +0200 Subject: [PATCH 586/861] Updated demo --- examples/autonomy/full_state_setpoint_demo.py | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index f1920863d..9b6ea00c6 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -136,27 +136,30 @@ def run_sequence(scf): print('takeoff') send_setpoint(cf, 4.0, - [0.0, -1.0, 1.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, -1.0, 1.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), + quaternion_from_euler(0.0, 0.0, 0.7), 0.0, 0.0, 0.0) print('land') send_setpoint(cf, 2.0, - [0.0, -1.0, 0.2], + [0.0, 0.0, 0.2], [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) @@ -171,21 +174,24 @@ def _stab_log_data(timestamp, data, logconf): 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: - _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() - + # set_up_logging(scf) reset_estimator(scf) run_sequence(scf) From 4457306438750d702adb462727980e990694fee2 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 13:35:34 +0200 Subject: [PATCH 587/861] Call send_notify_setpoint_stop() --- examples/autonomy/autonomousSequence.py | 3 +++ examples/positioning/flowsequenceSync.py | 2 ++ examples/positioning/initial_position.py | 3 +++ examples/swarm/swarmSequence.py | 3 +++ examples/swarm/swarmSequenceCircle.py | 2 ++ 5 files changed, 13 insertions(+) diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index e7353a8c0..48f3047c6 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -137,6 +137,9 @@ def run_sequence(scf, sequence): 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) diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py index 1c7abd661..42d7a691f 100644 --- a/examples/positioning/flowsequenceSync.py +++ b/examples/positioning/flowsequenceSync.py @@ -79,3 +79,5 @@ 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 index 6ab9a4f1a..d2119c635 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -130,6 +130,9 @@ def run_sequence(scf, sequence, base_x, base_y, base_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) diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py index 5c715d610..dea78169c 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -197,6 +197,9 @@ def land(cf, position): 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) diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index 31475d77a..b80e176ad 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -129,6 +129,8 @@ def run_sequence(scf, params): 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__': From ec94e43e66d85feb61dd6f5090707d99b34141d4 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 15:40:03 +0200 Subject: [PATCH 588/861] Add motioncapture lib example --- examples/mocap/mocap_hl_commander.py | 237 ++++++++++++++++++ .../qualisys_hl_commander.py | 0 2 files changed, 237 insertions(+) create mode 100644 examples/mocap/mocap_hl_commander.py rename examples/{qualisys => mocap}/qualisys_hl_commander.py (100%) diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py new file mode 100644 index 000000000..3568eecd0 --- /dev/null +++ b/examples/mocap/mocap_hl_commander.py @@ -0,0 +1,237 @@ +# -*- 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 motioncapture +import time +from threading import Thread + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.mem import Poly4D +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.syncLogger import SyncLogger +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') + +# 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 wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +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 reset_estimator(cf): + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + # time.sleep(1) + wait_for_position_estimator(cf) + + +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) + run_sequence(cf, trajectory_id, duration) + + mocap_wrapper.close() diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py similarity index 100% rename from examples/qualisys/qualisys_hl_commander.py rename to examples/mocap/qualisys_hl_commander.py From 3e680f30222fbd56694c8a062c454cea0a07eafc Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 15:43:04 +0200 Subject: [PATCH 589/861] Styling --- examples/mocap/mocap_hl_commander.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py index 3568eecd0..2a73fb455 100644 --- a/examples/mocap/mocap_hl_commander.py +++ b/examples/mocap/mocap_hl_commander.py @@ -28,10 +28,11 @@ Set the uri to the radio settings of the Crazyflie and modify the mocap setting matching your system. """ -import motioncapture import time from threading import Thread +import motioncapture + import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig @@ -45,11 +46,11 @@ 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" +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" +# 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' @@ -94,7 +95,7 @@ def close(self): self._stay_open = False def run(self): - mc = motioncapture.connect(mocap_system_type, {"hostname": host_name}) + mc = motioncapture.connect(mocap_system_type, {'hostname': host_name}) while self._stay_open: mc.waitForNextFrame() for name, obj in mc.rigidBodies.items(): From 8d391ff434d20e9d72bba0a1e93a949c4dee2544 Mon Sep 17 00:00:00 2001 From: PiggyGaGa <30955474+PiggyGaGa@users.noreply.github.com> Date: Sun, 8 Oct 2023 11:48:50 +0800 Subject: [PATCH 590/861] Update sbs_connect_log_param.py change line 73 `with SyncLogger(scf, lg_stab) as logger: ` as `with SyncLogger(scf, logconf) as logger: ` --- examples/step-by-step/sbs_connect_log_param.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/step-by-step/sbs_connect_log_param.py b/examples/step-by-step/sbs_connect_log_param.py index c46b0ddc6..98cb4a4dd 100644 --- a/examples/step-by-step/sbs_connect_log_param.py +++ b/examples/step-by-step/sbs_connect_log_param.py @@ -70,7 +70,7 @@ def simple_log_async(scf, logconf): def simple_log(scf, logconf): - with SyncLogger(scf, lg_stab) as logger: + with SyncLogger(scf, logconf) as logger: for log_entry in logger: From fd9cc572927ead003b1b6eb0be8d8b05e226f69f Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 10 Oct 2023 10:43:24 +0200 Subject: [PATCH 591/861] Update sbs_connect_log_param.md fix sbs typo --- docs/user-guides/sbs_connect_log_param.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index 3fc365e2f..a708744bc 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -250,7 +250,7 @@ logging.basicConfig(level=logging.ERROR) def simple_log(scf, logconf): - with SyncLogger(scf, lg_stab) as logger: + with SyncLogger(scf, logconf) as logger: for log_entry in logger: From 08162a54981347c40f4dacf979473210a21d484f Mon Sep 17 00:00:00 2001 From: Jack Date: Tue, 10 Oct 2023 16:26:01 -0700 Subject: [PATCH 592/861] Update usage info in radio-test.py --- examples/radio/radio-test.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/radio/radio-test.py b/examples/radio/radio-test.py index e94755e28..2140f6875 100644 --- a/examples/radio/radio-test.py +++ b/examples/radio/radio-test.py @@ -9,9 +9,10 @@ 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 + ack packet enabled to get RSSI feedback. To configure the Crazyflie in this mode build the crazyflie2-nrf-firmware with ```make BLE=0 CONFIG=-DRSSI_ACK_PACKET```. + Additionally, the Crazyflie must be using the default address 0xE7E7E7E7E7. See https://github.com/bitcraze/crazyflie-lib-python/issues/131 for more informations. ''' From 73f07d5e502e3cd3f2a5de96400e608ea4877cce Mon Sep 17 00:00:00 2001 From: Jack Date: Tue, 10 Oct 2023 16:28:04 -0700 Subject: [PATCH 593/861] Update radio-test.py --- examples/radio/radio-test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/radio/radio-test.py b/examples/radio/radio-test.py index 2140f6875..d7129977e 100644 --- a/examples/radio/radio-test.py +++ b/examples/radio/radio-test.py @@ -9,7 +9,7 @@ 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 + ack packet enabled to get RSSI feedback. To configure the Crazyflie in this mode build the crazyflie2-nrf-firmware with ```make BLE=0 CONFIG=-DRSSI_ACK_PACKET```. Additionally, the Crazyflie must be using the default address 0xE7E7E7E7E7. From d932a4693c6f3796823b87427c7826deafc708cd Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 16 Oct 2023 11:28:02 +0200 Subject: [PATCH 594/861] Enable HL commander again --- cflib/positioning/motion_commander.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index 40bc92f04..35debdd0f 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -117,6 +117,9 @@ def land(self, velocity=VELOCITY): self._thread = None self._cf.commander.send_stop_setpoint() + # Stop using low level setpoints and hand responsibility over to the high level commander to + # avoid time out when no setpoints are received any more + self._cf.commander.send_notify_setpoint_stop() self._is_flying = False def __enter__(self): From 7434215409d8bfcea1ac2ce0d2a3a8052ff04e44 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 18 Oct 2023 14:46:36 +0200 Subject: [PATCH 595/861] Implement remote param updates --- cflib/crazyflie/param.py | 68 +++++++++++++++++++++++++--------------- 1 file changed, 42 insertions(+), 26 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index bc339eb9e..77d474a39 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -62,15 +62,16 @@ MISC_CHANNEL = 3 MISC_SETBYNAME = 0 +MISC_VALUE_UPDATED = 1 MISC_GET_EXTENDED_TYPE = 2 - -PersistentParamState = namedtuple('PersistentParamState', 'is_stored default_value stored_value') - MISC_PERSISTENT_STORE = 3 MISC_PERSISTENT_GET_STATE = 4 MISC_PERSISTENT_CLEAR = 5 MISC_GET_DEFAULT_VALUE = 6 +PersistentParamState = namedtuple('PersistentParamState', 'is_stored default_value stored_value') + + # One element entry in the TOC @@ -186,32 +187,39 @@ def _check_if_all_updated(self): def _param_updated(self, pk): """Callback with data for an updated parameter""" + + # This method handles both param value packets as well as misc param updated packets + # The Misc packets have a command byte first and the variable id is shifted one byte + # Misc packets are not supported for V1 + if pk.channel == MISC_CHANNEL: + id_index = 1 + else: + id_index = 0 + if self._useV2: - var_id = struct.unpack(' Date: Tue, 24 Oct 2023 13:41:59 +0200 Subject: [PATCH 596/861] update version to 0.1.24 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index dfa9aca71..26e2db617 100644 --- a/setup.py +++ b/setup.py @@ -9,7 +9,7 @@ setup( name='cflib', - version='0.1.23', + version='0.1.24', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From 461f9e10fdfd9aacce16f118828094e1fbc9519c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 1 Nov 2023 08:52:31 +0100 Subject: [PATCH 597/861] Update dependency_check.yml disable python latest temporary --- .github/workflows/dependency_check.yml | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index ee23767e7..d3903dbbd 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -6,10 +6,24 @@ on: - cron: '0 2 * * *' jobs: - python-latest: +# python-latest: +# runs-on: ubuntu-latest +# container: +# image: python:latest +# +# steps: +# - uses: actions/checkout@v3 +# +# - name: python version +# run: python --version +# +# - name: install lib +# run: pip install -e . + + python-python311: runs-on: ubuntu-latest container: - image: python:latest + image: python:3.11 steps: - uses: actions/checkout@v3 From a95589609498481a17491a802665f697cca4ccdd Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 16 Nov 2023 10:29:11 +0100 Subject: [PATCH 598/861] Update install.md --- docs/installation/install.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 30933d905..778b75b80 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -5,7 +5,10 @@ page_id: install ## Requirements -This project requires Python 3.7+. +This project requires Python 3.7 - 3.11. + +> Python 3.12 is not supported as it has issues with missing packages (see [this ticket](https://github.com/bitcraze/crazyflie-lib-python/issues/425)) + See below sections for more platform-specific requirements. ## Install from Source ### Clone the repository From fcee91255a87a4ab3d6696f0f9d1039bf8d3cf91 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20=C3=81ngel?= <66631229+gelanchez@users.noreply.github.com> Date: Mon, 18 Dec 2023 19:28:50 +0100 Subject: [PATCH 599/861] Fix typos in sbs_motion_commander.md --- docs/user-guides/sbs_motion_commander.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index f117801e9..79876107e 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -470,7 +470,7 @@ Add `BOX_LIMIT = 0.5` underneath the definition of the `DEFAULT_HEIGHT = 0.5`. Run the script and you will see that the crazyflie will start moving back and forth until you hit ctrl+c. It changes its command based on the logging input, which is the state estimate x and y position. Once it indicates that it reached the border of the 'virtual' limit, it will change it's direction. -You probably also noticed that we are using `mc.start_back()` and `mc.start_forward()` instead of the `mc.forward(0.5)` and `mc.back(0.5)` used in the previous steps. The main difference is that the *mc.forward* and *mc.back* are **blocking** functions that won't continue the code until the distance has been reached. The *mc.start_...()* will start the crazyflie in a direction and will not stop until the `mc.stop()` is given, which is given automatically when the motion commander instance is exited. That is why this is nice functions to use in reactive scenarios like these. +You probably also noticed that we are using `mc.start_back()` and `mc.start_forward()` instead of the `mc.forward(0.5)` and `mc.back(0.5)` used in the previous steps. The main difference is that the *mc.forward* and *mc.back* are **blocking** functions that won't continue the code until the distance has been reached. The *mc.start_...()* will start the crazyflie in a direction and will not stop until the `mc.stop()` is given, which is done automatically when the motion commander instance is exited. That is why these are nice functions to use in reactive scenarios like this. ```python import logging From 2aaa3dab95dc2ee0ff35f8eec63326bd37a20fad Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Fri, 29 Dec 2023 17:46:05 +0200 Subject: [PATCH 600/861] Proofreading sbs_swarm_interface.md --- docs/user-guides/sbs_swarm_interface.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index 6cd098f37..9ec4f3d56 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -3,7 +3,7 @@ title: "Step-by-Step: Swarm Interface" page_id: sbs_swarm_interface --- -Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib.For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a global positioning system (Lighthouse, Loco or MoCap) that is able to provide data for the position estimation of the crazyflies. You can also use the Flowdeck but keep in mind that you should command relative movements of each Crazyflie and due to its nature it may lead to accumulative errors and unexpected behavior over time. +Here we will go through step-by-step how to interface with a swarm of crazyflies and make all the copters of the swarm hover and fly simultaneously in a square shape using the `Swarm()` class of the cflib. For this tutorial you will need a swarm (2 or more) of crazyflies with the latest firmware version installed and a positioning system (Lighthouse, Loco or MoCap) that is able to provide data for the position estimation of the crazyflies. You can also use the Flowdeck but keep in mind that you should command relative movements of each Crazyflie and due to its nature it may lead to accumulative errors and unexpected behavior over time. ## Prerequisites @@ -16,7 +16,7 @@ We will assume that you already know this before you start with the tutorial: ## Get the script started -Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up a new python script called `swarm_rectangle.py`. First you will start by adding the following import to the script: +Since you should have installed cflib in the previous step by step tutorial, you all ready to go now. Open up a new python script called `swarm_rectangle.py`. First you will start by adding the following import to the script: ```python import time @@ -39,9 +39,9 @@ if __name__ == '__main__': with Swarm(uris, factory=factory) as swarm: ``` -This will import all the necessary modules and open the necessary links for communication with all the Crazyflies of the swarms. `Swarm` is a wrapper class which facilitates the execution of functions given by the user for each copter and can execute them in parallel or sequentially. Each Crazyflie is treated as a `SyncCrazyflie` instance and as the first argument in swarm wide actions. There is no need to worry about threads since they are handled internally. To reduce connection time,the factory is chosen to be instance of the `CachedCfFactory` class that will cache the Crazyflie objects in the `./cache` directory. +This will import all the necessary modules and open the necessary links for communication with all the Crazyflies of the swarm. `Swarm` is a wrapper class which facilitates the execution of functions given by the user for each copter and can execute them in parallel or sequentially. Each Crazyflie is treated as a `SyncCrazyflie` instance and as the first argument in swarm wide actions. There is no need to worry about threads since they are handled internally. To reduce connection time, the factory is chosen to be instance of the `CachedCfFactory` class that will cache the Crazyflie objects in the `./cache` directory. -The radio addresses of the copters are defined in the `uris` list and you can add more if you want to. +The radio addresses of the copters are defined in the `uris` list and you can add more if you want. ## Step 1: Light Check @@ -117,7 +117,7 @@ with Swarm(uris, factory=factory) as swarm: ``` ## Step 3: Taking off and Landing Sequentially -Now we are going to execute the fist take off and landing using the high level commander. The high level commander (more information [here](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/sensor-to-control/commanders_setpoints/#high-level-commander)) is a class that handles all the high level commands like takeoff, landing, hover, go to position and others. The high level commander is usually preferred since it needs less communication and provides more autonomy on the Crazyflie. It is always on, but just in a lower priority so you just need to execute the take off and land commands through the below functions: +Now we are going to execute the fist take off and landing using the high level commander. The high level commander (more information [here](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/sensor-to-control/commanders_setpoints/#high-level-commander)) is a class that handles all the high level commands like takeoff, landing, hover, go to position and others. The high level commander is usually preferred since it needs less communication and provides more autonomy for the Crazyflie. It is always on, but just in a lower priority so you just need to execute the take off and land commands through the below functions: ```python def take_off(scf): commander= scf.cf.high_level_commander @@ -138,7 +138,7 @@ def hover_sequence(scf): land(scf) ``` -Initially , we want only one copter at a time executing the hover_sequence so it is going to be called through the `sequential()` method of the `Swarm` in the following way: +Initially , we want only one copter at a time executing the `hover_sequence` so it is going to be called through the `sequential()` method of the `Swarm` in the following way: ```python swarm.sequential(hover_sequence) @@ -213,7 +213,7 @@ If you want to take off and land in sync, you can use the `parallel_safe()` meth Now the same action is happening but for all the copters in parallel. ## Step 5: Performing a square shape flight -To make the swarm fly in a square shape, we will use the go_to method of the high level commander. Each copter executes 4 relative movements to its current position covering a square shape. +To make the swarm fly in a square shape, we will use the `go_to` method of the high level commander. Each copter executes 4 relative movements to its current position covering a square shape. ```python def run_square_sequence(scf): From cfb90d26b7164f38628fdd2c0dd4ccfaca7bb997 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Fri, 29 Dec 2023 19:12:29 +0200 Subject: [PATCH 601/861] Proofreading python_api.md --- docs/user-guides/python_api.md | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 8136f4417..7063192d9 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -74,10 +74,9 @@ There\'s are few limitations that need to be taken into account: ### Parameters The library supports reading and writing parameters at run-time to the -firmware. This is intended to be used for data that is not continuously -being changed by the firmware, like setting regulation parameters and +firmware. This is intended to be used for data that is not continuously changed by the firmware, like setting regulation parameters and reading out if the power-on self-tests passed. Parameters should only be -change in the firmware when being set from the host (cfclient or a cflib script) or during start-up. +changed in the firmware when being set from the host (cfclient or a cflib script) or during start-up. The library doesn\'t continuously update the parameter values, this should only be done once after connecting. After each write to a @@ -144,7 +143,7 @@ This will search for available drivers and instantiate them. ### Serial driver The serial driver is disabled by default and has to be enabled to -be used. Enable it in the call to `init_drivers()` +be used. Enable it in the call to `init_drivers()`. ``` python init_drivers(enable_serial_driver=True) @@ -453,7 +452,7 @@ exiting it, for instance a connection or take off/landing of a Crazyflie. ### SyncCrazyflie -The SyncCrazyflie class wraps a Crazyflie instance and mainly simplifies connect/disconnect. +The `SyncCrazyflie` class wraps a Crazyflie instance and mainly simplifies connect/disconnect. Basic usage ``` python @@ -469,7 +468,7 @@ Basic usage ``` If some special properties are required for the underlying Crazyflie object, -a Crazyflie instance can be passed in to the SyncCrazyflie instance. +a Crazyflie instance can be passed in to the `SyncCrazyflie` instance. ``` python my_cf = Crazyflie(rw_cache='./cache') @@ -481,8 +480,8 @@ a Crazyflie instance can be passed in to the SyncCrazyflie instance. ### SyncLogger -The SyncLogger class wraps setting up, as well as starting/stopping logging. It works both for Crazyflie -and SyncCrazyflie instances. To get the log values, iterate the instance. +The `SyncLogger` class wraps setting up, as well as starting/stopping logging. It works both for Crazyflie +and `SyncCrazyflie` instances. To get the log values, iterate the instance. ``` python # Connect to a Crazyflie @@ -508,11 +507,11 @@ and SyncCrazyflie instances. To get the log values, iterate the instance. ### MotionCommander -The MotionCommander class is intended to simplify basic autonomous flight, where the motion control is done from the host computer. The Crazyflie takes off and makes +The `MotionCommander` class is intended to simplify basic autonomous flight, where the motion control is done from the host computer. The Crazyflie takes off and makes when entering the "with" section, and lands when exiting. It has functions for basic movements that are blocking until the motion is finished. -The MotionCommander is using velocity set points and does not have a global coordinate +The `MotionCommander` is using velocity set points and does not have a global coordinate system, all positions are relative. It is mainly intended to be used with a Flow deck. ``` python @@ -529,11 +528,11 @@ system, all positions are relative. It is mainly intended to be used with a Flow ### PositionHlCommander -The PositionHlCommander is intended to simplify basic autonomous flight, where all the high level commands exist inside the Crazyflie firmware. The Crazyflie takes off +The `PositionHlCommander` is intended to simplify basic autonomous flight, where all the high level commands exist inside the Crazyflie firmware. The Crazyflie takes off when entering the "with" section, and lands when exiting. It has functions for basic movements that are blocking until the motion is finished. -The PositionHlCommander uses the high level commander in the Crazyflie and is +The `PositionHlCommander` uses the high level commander in the Crazyflie and is based on a global coordinate system and absolute positions. It is intended to be used with a positioning system such as Loco, the lighthouse or a mocap system. From 26cd8c755911291b19273ed5c6c8f4f5b97e38b2 Mon Sep 17 00:00:00 2001 From: Marcus Date: Thu, 25 Jan 2024 19:52:42 +0100 Subject: [PATCH 602/861] Bootloading s110<>s130 cold and warm working --- cflib/bootloader/__init__.py | 86 ++- resources/binaries/nrf51-s110-and-bl.bin | 787 +++++++++++++++++++++++ setup.py | 10 + 3 files changed, 880 insertions(+), 3 deletions(-) create mode 100644 resources/binaries/nrf51-s110-and-bl.bin diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 85ac5c996..95661bd21 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -27,6 +27,7 @@ """ import json import logging +import os import sys import time import zipfile @@ -51,7 +52,7 @@ __author__ = 'Bitcraze AB' __all__ = ['Bootloader'] -Target = namedtuple('Target', ['platform', 'target', 'type']) +Target = namedtuple('Target', ['platform', 'target', 'type', 'provides', 'requires']) FlashArtifact = namedtuple('FlashArtifact', ['content', 'target']) @@ -150,6 +151,60 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo flash_artifacts = [a for a in artifacts if a.target.platform == platform] deck_artifacts = [a for a in artifacts if a.target.platform == 'deck'] + # Detect what soft device we are running on the nRF51 and what we need and make sure they match + nrf_softdevice = [] + if self._cload.targets[TargetTypes.NRF51].start_page == 88: + nrf_softdevice.append('sd-s110') + elif self._cload.targets[TargetTypes.NRF51].start_page == 108: + nrf_softdevice.append('sd-s130') + else: + raise Exception('Unknown soft device running on nRF51') + + # Check if there is a nrf51 firmware in the manifest and skip the soft device + # flashing if we already have the correct one + nrf51_artifacts = [x for x in flash_artifacts if x.target.target == 'nrf51'] + if len(nrf51_artifacts) > 0: + nrf51_fw = [x for x in nrf51_artifacts if x.target.type == 'fw'] + if len(nrf51_fw) == 1: + if 'sd-s110' in nrf51_fw[0].target.requires and 'sd-s110' in nrf_softdevice: + print("nRF51 s110 already flashed on device, skipping") + flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] + elif 'sd-s130' in nrf51_fw[0].target.requires and 'sd-s130' in nrf_softdevice: + print("nRF51 s130 already flashed on device, skipping") + flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] + else: + raise Exception('Manifest should contain at most one nrf51 firmware file') + # TODO: Bootloader without firmware should be supported + # TODO: Bootloader without soft device should be supported + + + nrf51_artifacts = [x for x in flash_artifacts if x.target.target == 'nrf51'] + + # First flash the nRF51 soft device and bootloader if needed + rf51_sdbl = [x for x in nrf51_artifacts if x.target.type == 'bootloader+softdevice'] + + print("nRF flash page is {}".format(self._cload.targets[TargetTypes.NRF51].start_page)) + + if len(rf51_sdbl) > 0: + print("Flashing nRF51 soft device and bootloader") + self._flash_flash(rf51_sdbl, flash_targets) + # Now remove the soft device and bootloader from the list of artifacts to flash + # and then reboot into the new bootloader + flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] + self._cload.reset_to_bootloader(TargetTypes.NRF51) + uri = self._cload.link.uri + self._cload.close() + print("Closing bootloader link and reconnecting to new bootloader (" + uri + ")") + self._cload = Cloader(uri, + info_cb=None, + in_boot_cb=None) + self._cload.open_bootloader_uri(uri) + print("Reconnected to new bootloader") + started = self._cload.check_link_and_get_info() + self._cload.request_info_update(TargetTypes.NRF51) + print("Flashed sd+bl, we are now at {}".format(self._cload.targets[TargetTypes.NRF51].start_page)) + + # Flash the MCU flash if len(targets) == 0 or len(flash_targets) > 0: self._flash_flash(flash_artifacts, flash_targets) @@ -232,13 +287,35 @@ def _get_flash_artifacts_from_zip(self, filename): manifest = zf.read('manifest.json').decode('utf8') manifest = json.loads(manifest) - if manifest['version'] != 1: + if manifest['version'] > 2: raise Exception('Wrong manifest version') + + print("Found manifest version: {}".format(manifest['version'])) flash_artifacts = [] + add_legacy_nRF51_s110 = False for (file, metadata) in manifest['files'].items(): content = zf.read(file) - target = Target(metadata['platform'], metadata['target'], metadata['type']) + + # Handle version 1 of manifest where prerequisites for nRF soft-devices are not specified + requires = [] if 'requires' not in metadata else metadata['requires'] + provides = [] if 'provides' not in metadata else metadata['provides'] + if len(requires) == 0 and metadata['target'] == 'nrf51': + requires.append('sd-s110') + # If there is no requires for the nRF51 target then we also need the legacy s110 + # so add this to the file list afterwards + add_legacy_nRF51_s110 = True + print("Legacy format detected for manifest, adding s110 requirement") + + target = Target(metadata['platform'], metadata['target'], metadata['type'], + provides, requires) + flash_artifacts.append(FlashArtifact(content, target)) + + if add_legacy_nRF51_s110: + print("Legacy format detected for manifest, adding s110+bl binary from distro") + from importlib.resources import files + content = files('resources.binaries').joinpath('nrf51-s110-and-bl.bin').read_bytes() + target = Target('cf2', 'nrf51', 'bootloader+softdevice', ['sd-s110'], ['sd-s130']) flash_artifacts.append(FlashArtifact(content, target)) return flash_artifacts @@ -383,6 +460,9 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ print('\nError during flash operation (code %d). Maybe' ' wrong radio link?' % self._cload.error_code) raise Exception() + + sys.stdout.write('\n') + sys.stdout.flush() def _get_platform_id(self): """Get platform identifier used in the zip manifest for curr copter""" diff --git a/resources/binaries/nrf51-s110-and-bl.bin b/resources/binaries/nrf51-s110-and-bl.bin new file mode 100644 index 000000000..d6f8a70d8 --- /dev/null +++ b/resources/binaries/nrf51-s110-and-bl.bin @@ -0,0 +1,787 @@ +@ 1moqsuwwwwwwwwwwwwww)wwͬwwwwL#x+K+H##p$% K+IHF(% K+KF@"F!FFHJ,K+ИGK+ИG ! H( H )F@ $% , rpGypG}pGepG$% ;x3B + B?;x;ST7shY""((xB aBaA;xIR*,њ:p3۲T:x!("x$;x;p5 "(+"9 #px +&Cx +K'#9;(@3x+; "+sx+Л"+";۲Y)pjp"x2#px+#x +!'8V(#xZ"p"T#xZ"p"T#x:p +Zp#x3#p+вh* "x) Q3!pT#xZ"p"T x(h"x#ph*#x+Yx!p!T#xRY!p +!T#xY!pT3+P3+ 0+"!0T(3+ 0+"! 0G(Ѳj*>j/(;y"x )Q3!pT#xZ"p"T#x:p +Zp x0 p* +йh)ш("x#p343x+L z0+"!0(o x+q)e7O6KFcE[S+WgER/OBлBЙBJC#p#+T#xY!p!T#xp +Yp#x3۲#pRp +Zp#x3#p3k+*#xB '"x_C3kQ;y!p3T#xZ"p"T#x:p +Zp x0 p* +йh)(1"x#p3۲F}𵓰kFjF# 3p;pBЃ+ h+x[2{(- +Ы+: +(n( +;x1x* upGvpGzpG{pGLK BУx"|3۲pB +! 1(i+ИG#p#}+;!(i+ИGi+"pF@% Lc"B bBӣi+ #04#p#03x+Уi+"p x+ ѡh"`k(i+ИGh"@% pL#bbbbh%`#"h03p 5B2(k(~( # ![B40J#pBp(pF@% KXk@% +&++0P+LbBË+0L 1(0"#'#pc+l# +L[B##p`kj(i+ИGH"(0 @% sL% 5"(( c"BbBӣi+ (v#!&03 1 pp"pGF@% xpKxCp pGp e` ppGpG𵑰)Jm!8U-Jn!(N "!0"q +# 3p"x +pbxpJp"!c`K"1%'ax% p TMTK*p Q"*Q"* ++ +3#h<3JH(M"* ++3#ai<3DH(?"* ++3#i<3>H(1"* ++"3#aj<38Hu(#"* ++*3#j<32Hg(#k+ak)(ѣk+20p#ah<3(HQ(hZh +qh#H +Kq q+23q#"<39(+:3 +y# h<3H,(x +pZB5Jp +pHp + +qۈ"Kq +q#<3Fx% +$*%*'*&*(*)*#***P*&pGQpGpGpGapGNM.x.7kh\B\Ah**()(Ji!4,/i8 (( +J!#,hG8khG,4+4,F% p )ЌL(!aIca(#0#ppF% ZAJ۲p% #(KX`#pG% #(K`#pG% K[i+ИG(Й!JIpF% 0DxxbB"x#B3\CCh +`0K [h ;Z۲0@@*"@; J[h@ @۲ +;CA0pG2KX/#FFFFFFFFFFFFFF;+pG" Kpg#"% L[P@:Pe``%L%`%qp@% %!K"L]ddde]``h{1KJX#h66 +$#_ChKhB"ijii*G(d!J1{"}c}B3۲+ #u"#}R KaZ`G#F@% @*Kh}Z}Bї2Ҳ*ѐZuZ}iR +`"h3J;۲Sj+vhh*ZA۲#[BFKhh#;۲+M+Щ`hB+DSY$#SChL[hJX+h +3+y+р#:[:P@:P!"'KIZP`'`/qB# +PX +3 +Bـ"|KZ` "l"v]+xkxBr"ZCkh3۲xB#fKxhh)) !Qh$"JC}*$'WCB:jP#u$ CCuj\(NJ`2ZCUhB)HI`U`<@IiT`uj-`   +`aFFb;Z)"8J`"!RBF$& cF3$&UxxB XCTh3 x۲pBpx+Dh#3cC}/хh`h`i@iaa`F$#CCdFjFhOE-- +hBح]` &``8ubK$%eC]$#KChh_hB88X`)b1( +B6^` X`F% @6jw 3$&^CwhB?w`h`$ CC.bb !v B $"SCj +J`3$!LC;j4zh#bXKCshr` % %@M+K%K`a(2p$"WC*B8!Zw! 1"pC\ba`"pbpp4 B!$& IB`Zuu1Jd1TPYC!CQ!` L`P>Y H@! CQ JX`("p"u$4%% @Kh ,8*.%(3xB $4#$;x+$Appa5`5F% Lh  +2"(2x8B,)*$"rCx0*#[x+'dj$  +" +Ip +J^`X ``Za"aRcpKZ`F% @sNh#*!3(3x#B3CC\#*tj$ x#( Ѐ";pE`cpKRZ`#vF% L#h*ndj$ U#( +Ѐ";p;C`RcpKZ`#F% JKX` pG@@ +` pGpG-pGpG$Kd h#hG`𵍰D^L#x+ Ѣ"!XR"Pi##pcx+f(4#cpf*x ++x;(w_"Nxax*CEH@EHB ) +#cp+x;w)(@cx+ѧ4.%#+p6298np((##(w( +x+mx*8=w!L-cx+)/ 021(3+p1#4wKZl*"ZdK2`+X! #JXCP<+' 5$7*1&0vc8w& @`@sMH " IKX@) X[԰#[h3CY[ ձ#?L"P4?I #q< "`!SY CSQ#8J_9P"! P4K"!(1J{!B #,I-JP,K-J$P#"%+HP*HP*N*LfQFFF(N"`fQFFF +`&I $ZP!\P$Q@$P!LQ$P$Q$\PLQ$PHP XP P"ZP" P\PF & L`& [@HD4 `@ TtHh! +C`IJK;XPHGHG$@ $%  KJ`pG $pL#h۲+HhpI# +h&@2B!JhB KJ`"K`#h۲+h" h@@+ KhBр"#!P0-@l@" !KP!X!CPKpD) aLchB["#!z``X)ћ##rL`##rDh,#3BK #[r(#"7I(3p8" # #pp`'ea"&H"$KR'K"((1H0x@") I # V" #c``##sK 'taA("!Kڊ@ J +Cڂ"vF$) YIz f1 pGpGpGp+P+ +т(Kp"&KRB&Lˆ#B #z+ + )02 1u3#s##r#B`#~IP@(ڋp#) @2"d!1D0*"XxB# p)"lW0:!1J#z+ !%l0D1*?#%s#rF ) #𵇰Sp8Kp2 d"0)! 0K1L3xCCp.K";##p5%ch 2!(Ka'bi?"0#Ka;#p5edMJ85+%H5)hgcL""0! xJCCp#;#!2p2 KbdPd3d1 +Jggc(( ) P KzpGF) H 0pGF) "KrpG) 𵋰#i} +C |{*C%#B+,HB()%+i)} + C%h ki ++~CB#%+h h*{ +C#B۲#qZBSAp### p#!p#qpw!pip!qpw+  p(M.Kx**/ph]P`xx +Ciy.y 1CB ^("!KapixXx C``pK "RE("Ka2hxPh^yQy`"Cy`$4CB K "(Kx**hhJh+ KphLJ`* ^h LB"Xh2`p$XH`DT*"`F+ @ @+ "ISXH0@SP$#!%QHSPKP%HP%HP QPP" ZPI :ZPI@XP !P H`` P` +Ks0t  @l$+ K{+KJPKj*"bpG+  @ @K{+4 x(+ Kp{+EBp x4F+ K{(h[hCApGF+ K{ Zhh BHQY`\)"Z`pG+ + #2L"xCp*#pc`&'(#x+ x8CBXA x( "5<;ZBSA#p"+""p+p#rcr#"zazҲ"rRbr"`b`B#bh*ܩp!z[az۲#r[crh2b`B#"zB'p#bzBh&pkp##pc`p, 7HJ0x1px4x-uxedrxB(!  xx7d#$'[BB"$!$ B%@(C2[ *C%"]>B9@ +Cv=S@"[-IK@:Ҳ*4FqFII \IDpGFK+!Khj+ИG AFp& M LdB &3 +M +LdBpXG6XG6% % % % #B\T3BpGp3#\3*XpGFFFpGFFpGnrf51_sdk/nrf51822/Source/ble/ble_services/ble_dis.cnrf51_sdk/nrf51822/Source/sd_common/softdevice_handler.cnrf51_sdk/nrf51822/Source/app_common/app_timer.csrc/main.c + +C{OCrazyflie LoaderBitcraze$ h$   @BB$  P=OP==eQkQ======qQ==wQ=}QQQ========QQ====== 08$-FgFTF]FB~F>̶F&3BFF3CGTAA:xxR00h `pG#$%&:xR0 `pGFF#iIaHDpGa/F@FIFRF[F0HGFFFFFpG@pGpF FF>..>.6!x)pdmvp"@ @( 8(HiCCapG$HhCC`pGF! ! ! ! ! ! ! !! ! F c!pG:HpG5pGpGpG0F7M (pY 4H:F )F/Hd, +0!hFpq( (F0pG#NF0x' F( F`8 ( F+`,H` Fl8( F Fp8( F7 F8( F F8( F# F8( F(`/``,(h(7p 30pGpGpGpGpGpGpGpG(Bp8 "%,38=AEIPU\dlt{JhxFh' y {hh!F/hmJhFj +yFx@hhh( FPFJhF yhF yhFKhhFJhFSr 8g $,6?FNhFJhFJhFGJhF}KhhFFKhFJhFHF (б(в(  KhhF8  (2w‡| p v ~(((&v&tix)МH0 0† tnp ( |(ШxFFhM@=(xN(/0~( 0," Fw p (pM ={(/0~(((( s0 p{p(|`p p sq! F (0|(R9F F|iH 8hH +8)x)/VIp&fp p (u",tG\H! +8px(/>0~(C P%ep QI p +" p*9F F ($Ѩ|(/HH +8x~(0|( /0~(:BH&fp + ;I p" v 6H~)|(! C(  $ +CCVF!CFFFF(hF(, !H00F9B' =1F)Fl(H @0(H$@0A|)Dt + HĀsrc\ll_ctrl.s0.c  ,  ?y)) I0 B!B! FFFFF (H|(H >(IH$hF?(0p( (sd$$ 9F ((HI0_(HI0 8FiF( `qhFx(( I%0 8 q![pLF@< x(H~(нIH{] q `q=!p pppLF@< x(ѳH~(дHI 8`(x(x q `q9!p pppMF@=(x(ѤH~(ХHI%8BN!B ( (q hq8!p(pp pvHx 0Atxt L@< x( яH~( ( (ыHIe0I 1q q `q:!p ppLF@< x( ~H~( ( (e vIuM 5y( y"h01F-y@q  q `q;!p pphMF@=(x(eH~(fHaI_8aO!B ( ">drxRB[JQCPC@!1BFB Fq@yvpC@ (q hqH*B6HH8~*sxAv!v7v s4H@0@|(  s't s(OF8x=(8F@0~((H#I801F '$(p(H"`01FFq|q p8X;>QTB(,04FJN $k[_cgo~{wsdU`\XTPMJFB>Z:<740-{)T%!o& +z\  I!HpJ$FM@1"!HEH!H8wAw8 (H I0p tUep I0F <DU[۲+0  R`8 + +&*16;hsFh'F+KhxhF6KhhhFJh_JhF{FF  pG ?I@`>I`>I +h#CC +`8I9`pGF 8I[y+ +F@+3C 2 P"YJqq" +q*J!qpGF(&R #H  y(!S `yy@B`q I@1`pHh(i NL% xy*ByyBByqpXG x@ p( p(Fm(p, src\soc_signalling.c< rHAxIAp@x(K,bpLx( +%pD(  pHeqE`prL`x(¡ `x@`p`x(-bpHx)!Aqp FIx@ ` FL&q !%'K  + &!#%(%q" !q q'q &(&!qq 'q @ q ~0-( B0F($ `J!Q`JaK"Z``HiiC['=CaM oh/+i#CaxH`i`y( F{J-Т````.F(hI pJxRJp FpiOh%(ѽ`Exh(}`8h&\L(=``y( 5eq x( W(YH8cap8i(,=ah Fay(A)hhA)ڊ)``)(BH8cap&ph(975x( 30m`p%p(1H8c(Lx( ax( 'Lh(( ` IC`P iFjFA(H'`I ``$ N(H8c,ap4p\HG` I0ycaI ala@ @src\soc_clock.c@@F F Q`cH IHppG   0IFHhh#JFH$Hd,I 1@ 1( "!H +H"!0 ]H"4!t0 +6HL!(0PIjFt1FL8d1{1Ad!r R(ˡ wH"$!8 +H",!\8 +I ( ankF"!Ġ(з O(Fy(г F!HIJ \ @p(ӨI H`pppNFpj F@ѰjC 0j(ppjj q +`q+ iFp hF +( #0 + (p" + Ff(x(`y!C`qpLF h F7F@7(`h@Ѡh@h@ h1F`h1h9Fh1F 1%+ iFp hF +( sL0" +0F$+ iFp hF +( iS0" +8F i.F@6(`i@Ѡi@i@ i)Fp`i)li1Fhi)F 1c+$hFp +o( Oq0" +(FhFp +]( Fx0m"90F!F ! ! ! ! ! ALx ( 10C x`!HC0!pAx"@Ap'J!pp%Lx(1H%xIɲp )pxIppppLF`h( 0e`ppN!MqhF)`" p`(y(vH\8 +``(H`h +(3 Fp`x!4 src\host_core.cQ  #0 +  nRF51822 @`p(y@(q&Hah)Hah +ohy(+HL8 +_``(IH`h +( F`x `phy@hq`HahL8 +G}HahL8 +@pFx' F>F(HI"0u(z((Н Ik,'f ' &,Ш{'&,(i` ({ rpF!FF5# ##"F)F0F p"F)F0Fp"F)F0Fy(=(;(9 (7 %01y(0 +(. +0(y('(% (# /0y( +( ( 40y ( +( 90 y ( +(( =0(h0qp B0I" hR\Ѡ1BшpG pGpF p!Map)h1z))) k0 x ((h}`pp 000 p0|I}LBКBBBЕBғB؃}$BBrMF +<-B҈BЊB[ZCB 0 0" (ЙB\)  \,Ѕox /L; +B**0022::B\+?RҲB/1/5/,2//',ѫx+#p[ + $xC, %#-[+0Fx +$#C#p\RҲBݩ &vB۟x@* pG +FI* pGp&M!(hC0$w!F ! !  (h0qrcp0L"h2y* * +(#jFp);) 0 qF  h0y))) R(F T src\gap_core.c{ 0! vM!(hjFr!p! 0 k)h@"{CshF ( )h ) +,Ћ"! ( 0 p%F(! CL! hr! 0 ; h{s h( +.Ѓ"! (%(Fp%𵛰IHpr pah& +x%R'HU ||~|||||Hh0y(нHI 0P!F ! `xJ(C`ph%F1Kz+CKrchx<-MF2wch0ۉ chKch[ahkyuahk10" H#hF1r~@v"XzrFHz@@Hr(F(ДHI$0(F(ЏHI'0(F(ЊHI)0> RFh0y(((( ( + +( (({HzI50`x(C`p8h0Az)CAry ( +( ( ( 0Fz0F0F | h0y(((( ( + +( ((^H\IY0`hy(ax)CapahȀFh0y(((( ( + +( ((KHJIl0l7`x(C`p`h8h0ah AahI! 8h0qgFh0y(((( ( +( ( (1H/I070h0y((0h|@:`h! 0(3`h!(,1h @Xz @\(HH@"CHJ CJ-"hFp BZhF"1^ hF (= IDbh0h! +Rah" 0It0h0 T B&qFh0y)))) ) +) ) ) I0h0y((`hy)Ѐx(# I@`x!(C`p0h Xz)Qс| N~I!!0r+ iFp hF u(IHhF1h"tH"RX2Ba +F`2B`yp +F2aR`2`:a2abG:Bb2b2bG:cRdP1Bdd 3((HI)0M |I~I!!0r! 0hA +@"CA"CAIHh0y(бHIW0'!F ! `xJ(C`ph%F1Kz+CKrchx<-#F2wch0ۉ chKch[ahkyuahk10"X \H#h +F2r~@vNPz@@Pr(F(ЊHIr0(F(ЅHIu0(F(ЀHIw0`x(C`p) Ј}Hh0ʀ +BJpGFx F' =F)trHh0)o) +)ljImH qh +xR Q/XXGeox<*'%,}Јph phph`ph@qhF1y `su'%,!qq'%,Љqh"yqAz@Ar3' %,Јphy rለ'%!F}'%,Ј;D\'%,Hphyqx<*'%,Јph phph`ph@qh`{I}@@C`sqhI}IIC`sqhF1y8Hh0Az"s I_0z(HI8X'%p!F,px@@( +Hh0Az"UI h1,Њ2z B T =HSo' %,"Jhx)!z&II!r#@hx[[C!r#@hx[C!r#@h[xC!rbrh"xrzC"@#C' %,h"h|'%,1{qz"r'%,0kF1} |!C!@ tHh0z"C'8%,2! q z!C r!@I h +~RC r @XzWРz|@@C"@r|"@Cr|&0@C#@rz@|Cr|0@Cr F"g10bI h}RCw +~@R@CwF@0M 4M0H((#ЌH! iF(h!(hF( hFAA0Fh,  p FF ((rHp!F(FpF0(҈F0 +0 ( aH1"hAphFpq  iFYHhjF! iF ) 8!jFRIB 8MIF h! FiF ) 8>F iFEHB BBHB > F( iF`ȀH7HhjF!iF ) >F iF.HB B,HB 'H Fh!9(iF) iF !iFIaiFiFɈFIFFB"HBӆB / LB2ӕBӅB. h@&((xiFHq h@(iFx!CT ` 0iFpIy !CiFp hiF@(iF h3F@!AiF B 8 FI(ЈBӌB 8!jF() IF hM!(FiF !8 80 FIB!BIB 0M)h1 +zRԊyR Iz  ((-H ( (h0y( + !hFpyxChFq @xChFq! (   𵇰F F~((иHKhF1 +zЊy* *-P JB"BӧJB xR* x+ +"yB*"* F2k'rk#y3sksz;@x;Csrk#kxz;Cr';@xk;CrkxSzd#CSr "kFp-"-Ё-Ђ-Ѓ--Ј-Љ-Њ- "q"\wL*Ш0""""""" " +" y(( +( ( HhF ( S h"0zIICr IpZN F1h1 zF  l((SHd-PHB BKHB V- -, M   iFq iFphF ( :0h"0z@r 2pFFw((4Hp1M(h0z , 2( .-!hFp,h [hF" ~!@|IC v|!@u`1C`.!hFp(h0hF hF%)h#1 +z@ +r F'(( H +HB BHB  F!  T `0F((dHcHh0y))8 iFp qq hF( F((QHQHh0y) ) 8 iFp qq hF FHh0y( pG pGF(@hHyjF \p"X iFphF FU!p0j~p-LFh h),JB"B*JB p"w0 h!0kr h{) р{ ( + (IHk p pFhL(IB p h! X z) Fw1)0 +{ h!0kr!h"(hw1 h p0T ` BFxF67( (@(}H̉ + `C01# +*Hh Ѐ {Ͳ0qh8Hx +xCxxC8 (F p-( 8qx6 +7 Fd( p̉ + `C02# +*Hh%Հ{@Bqh2x:yRRC#@[C#@CC#@C#@C#@[C #@CC:q xqxKxC8:F (нp=6 +7 Fd(pc{̉(F0`C04# +*HhF Ѐ7Ձ 08x`*Fqh67@ Fd(kFx F +< 3AOm {F((ЅHT09 0 {F(~Hxq0+ 1 0/D`h`z008 7 {F(pHj0 2 {F(iHc0 3 {F(bH\0!4 0/ `h`z008 p {F((RHL0!5 0/`h`z008 8 {F(BH<0 6 (F-5:HpFx F&L44$8 + + + + + + + + + + + +Lhx(.H',0, $@`x0C`p ,} x)? p!F@x0C`p7HG0HL0^, +`xՓ p FX")F0D pHP0 L IFRR@psrc\gattc_core.cZX I p pGI ppG!HC!p;F'hFhh FF(z(.IH)z F0 = F\m4x(I iF00 ,"hh' y  +tʁRӉK@B"F)F0F"F)F0Fx( I"0hh'!)i+x( I<0iF05 ,x( IV0iF06 ,hh' F0)iiF08" +,yhh'(z( !s!)i!"F)F0F{x(ЋIH? iF7 ,mhh'g' s>x (ЀH~I0$(iF07 ,Qhh!'s!)a F*0ii8$x +(mHkI30 iF7 ,,hh'!s'gp "F)F0F iF9" +, UH''s QIhF,hF hF` FF + (x( @H iF&-!hFq!F(F +3(  !hFq!CA!s xq(hF!s!CA 2(khF"!Q(pp FC  +(x( p H!hF!r)i!t I +2(.X he<c0(hF"!0F(!pp F   (x(  H!hF!r)i!tI +2E(hF"!0F(!pp F   (x( v tHr!hFr)i! +2 (^hF"!0F(!pP𵑰FF  i(x(  H!hF!r)ixt1 +2(lhF"!8FR(!pFFC + +(x( H'-hFrŁ iF +2(2!hFrhF"!(pF   (x(  H'hFr! +2W(hF"!(p F)!x ++ ∀*Уh+ЂB  )Р(  x '((($(5 THhFqaAh&0F}()LH !hFqaAhx( !hFqaAሁh&0F[( !hFq!{r!hF 2(jhF"1FP(!x)))) !p +!p}0 FF3( (H0H!hFq! 2()hF"!(F IB I B +HpG pG(Ёx 0(X (((( pG pG pG\HpG xFFF(WI 0(0 08{,&xh &*FAi2 +=( (tvh@B pFxFF F( 7I0!HC083!,hh F F"QCJ ׉ρ|t[ +BppFF"7",Hh&Hz00ȉ ȉfx( ( +(IHZpp{ (HI 8P { ( +H I8F {(HI< s0heF J@(CZBpG0FJ :ySy +UyB@Iɲ)!B0H8Ay +IͲF-%H8yB  F( ۡ0 (/ 0` `q q F( +H8CyTEq H ɡ0  𵍰F%hFqFF (!x'()hFvaxv!Ńph #"!(г H8x*BxcxB))aF)p! +4hFqphhF!C`y"yChFaF)hFy(1Fp! +hFq`y"yChF!CAH!8pJBApqAq + IFC@R(FH8D pG!hFpI!fFx' F>F( rd0 !(z( nz0 ,'f p'F +0, hh F +0)i bChFyy !C\zz%\5C$By+--&-@-FF(F( + p r&a`x!C`p F()F F0;L<"x*&#jFscxs# SKӂɈ #"!( "#F + p`x`p + #L'<F`y!yBч  y y@](( y@ q( q0Fv(8FF@( $(1 !HI)JZ*)IDR  src\l2cap_core.c0 @( (!;J@RF 𵋰F F.a@)  (( F(%*F1F0 ^b+F!8F FFFF%0F( iF ( iF3 +9F0F( ((I (FhF+`h"!(I `x%!C`p ds(Ёx)9 pGJB3JB. )+ '(%! @(((88("2B(( ( +())) pG pG +(! P + $$$("$&( ( ( (((Ђ(Є( Ѕ(   H    p) ;$FͲ+(( p 0 0B3 F88(99)p pp p   +     x** +  ((( pGp pG pG(((( pG  ! 1 p pG8 FOiF(" !CiF x C iFh(! ChFx C!x@ ** 8CC!!Cx C  +Щx@@ C  @ @!C 8pF F`h(!BahYNxqB1F ,) !1B p000(`hx1F 000) hxGHs hFI 9H pxx[[#Cp$#@xd#Cp$#@xd#Cp$#@x$#Cp$#@x$#Cp$#@xd#Cpx$@#@Cp)xр YICp0 +#C#ѣЛ#C# +CЀ C +0󵑰 (!(F s $&F7Fw)(0    hF|( hF  +( +( ( ,, $$hF$hF$$$hFB?,t.: "!0F( hF*F (Ѯ/$ "!8F( +hF1* ( hF~B~C,hF|rhF(  (()) pGIɍI@1ɈB pG pG0L%` .!i)+ {("hFqr"Br"! #2(0 {!C siF "!F !p !r4 0(FLF!i ) !F"1FT a !{"@!sp FF! F v --H p  ppjFx& F7F  /F/FHlhh +8)гH0 7F@x!C`pLx,ѻ u(F(l`x!C`p&fiF,ѠH/0 _(x( ДH!q&M, JH{ i( <&`x!C`p6)0(8&" F',уH~d0 &{H!{@F`xC`p.(y(hy( F?F , o rHlz0 , `x /ф p FX")F00Fz FF F 00'>F +1YH +**jB*R'&Q'&,}jhOH"qy@(wHHyIɉ!y@@h( Q'"0,UШ(Whh q  AP' 0,@Ш(Chh(z( + r( Fii 0 ii) "Q'"1!q,_( q  r(L ii {@Q'"&P' &,9hhP/ q!F({1t: src\gatts_core.c({` G#੉BT'&, hhS'&,hh +U'&, qIH +,'f H!r 8AAq{#RR@sapGpL < `qiF &FCChF i(HI30 {HI40 -(FCp H 8{"Cs!Aq3L C < {@@ spMF =({HIK0 }({(IB(i( p,Р( p h(aMp F( I X FMF 5 hF()F@9iFHzF(((( IB_ BӚJBXӘIBU BӕHBN`h(БIBH!BӏIBA`!B"RB  0F( i;( i( `x(hFڀnjhFՀ`h(tIB ( pIB!BnIB `i (`i(hF)?р<i(hF2hF.ԡi))'р$h(x((OJa2 F h! C!AhF !tJIEJ20h ( G?I²jq 122F2 +('5H 8 3H3802iF9 +s 0h})H0p +p&18jy1F (| (hF !t)! ({ax@@C(s!@ax"IC(sH*0#J +e<`h(; i( !`xiF}"  ` (! CiF iFtH I bhH +F#(~} xԀ.hF`i(  hF!C!AhFiFH!CiF tH@I J#c x.hFi(  hF C  hFhF@ ChF!tIH!J#1h(2hF !tIh +xpIxAphAhFAhyqh"Aq +qHA"# + !/r8x(h $FF((hF!thFIt/! C!AhF8F(  Mx.=(s8/F ?xjF} )F12F22 +6(u.0,r pF%puIhF" F0: x$(?!hF whFx ChFppfIhF @hFw FhFw FhFAw!ą +  \ hF5FX-(FpMHiF N8iFAIH.8r FN!.>1z)) @IN9ʍB B ;H!v@ #FCjF5Iфw +jF K$%F'F G(Ђ({,|-zE$ux(rzhF}/(bhFH@BhF@"!(w(([hFBF (BhF!BIB9HN8hF@B'): (*',- BF"!E(5 ((5(hF@{  G*K( >}#²u(!1r+'> F@!F]S"! FSFLhF(IH r F >NF0zF((  FH#80"iF9iF +c h(/H 08 0r(F8 F"iF(8 8iF Fi(ѠxiF(F > iF(> >! F hF2( hFy phF&( >hFy`px @hF  +C! +@A +Cp! +@  @iFC zP@Cp >FFFF"+F1F8FFhF (FQ F!FhF F,h(Рh( kFx((  (@x(B6HiF!B hF|%(,hFIB'%%B-/m--iF |)iF|Kʉ[ByH( )99% &A(Ђ((-0Fh( jF h F-hFhx]HhF "!F-(FhF( rx(|Ѐ i (EH@_% % iFv!hF!#2 (GhF")F8F=pFF FF*( +*F!F0FF FppF FF"F)F0FFF FppFF ,ah)!}( h( +m2F!F(FFFi Fp ppF #!a( h( Q)F FFN Fp}>  0((4 FHFFFB,ЄB8FG( +,iF8F%(hF #F*F1F8FqFF ( hFH#J9FF-.  "3F9FFhF (F)K +[BHpGC#lFK,[BґBH kFZ!!piF!𵑰 F iFqF-(xL(((  穈(Ѱ4hFy( Ѩy} j)h}iFquiFy( 7穈e(4 B!hFrsyAs աhF i 砈iF/F iF ++F2Fhx:/8Fh {!C s iF "!@&F F),OӧpH +W)9{{CBBѼ z8v-AЩxx6CB+iFKx x CB"#xx +CHx xC!(>xxiFCB xxC., FT@(hF#"FQ( iF +!(Y8!C8(~(Mi)!"hFvw$Dw!#F2 >( iF("!(l $FF* hL xEDhF#JFF</. 4 0.#hF) 7B0iF 9p +ypiFp +piF;!F / hF (Ђ(.)B "F1F9H +01p +Hp, iFP( +hF| ////1 hFx(hF!@(hFy(hF!I@(hFz(hF) ((( IH 8J!Qy$hlF$% 8PdՑq 8IɲB 8}3  I@bpGpM$p(Flp 0Dsv,b,ulc `,ahb(F<0 +( @0 Hc @bpp%I&`IcIjK JhI@2,i+LbjKbkbSkbk ck#CJcILj(+(- Сo0 LHc 0`c%cI ` aI @1H`HIAcI9cI cI9c I@9H`I H`H`pH`aHH0`aHCaJa`pGI ppGI ppGpGpGI@1(h"C@@`pGp FMF" \lrpH 0@{pGJpPppGp FF) 0 "H cX  `S bRҲB kI@1`p #CIB2" +* **0( ( ( 0  +     `wH 0~pG~Lxab [a%(&('(A +( !!P!ɲpJ`oI@1HapGdIbpGpbL u( }@jM(h^oH h }ՀkHY ^ } xaN((Х R PcHpa`j0a YI0@9H` }(h[HXHF0 8 }@(h!!C(`pp!=L%aw(((KH9E8 IH!x))Ћ 2 p`pE`p p j(э ) !j`k@9I`0H9I@0`5I 1A`,I@9H`H 0vp iFp(L`h&`+H!0a)K"a kFxRpjFx*i*ahFx(H18 -f`I bpGH src\hal_rcs.c@@@[@D@@@@%@@@@L@ DI +cI `I `I`I 9`IpH +(IH _H +IH`I<9K"aZ`IkIH`H!<8aJ!Q`@kI0H` I@`I <9aIH`zI <9HcuHI<8@k0`nI `jHh"C`I aaI aHh"C`XI <9wI <9wIi* BJ@2hRRRBp a 7L i(!H<8x(@!h*" +C( "ah)!CJ(!h+h+# C(#h)h)"CM("ih)!C( !`h(hh(&&HCh  ai)!O?9vhРi( xvHh(?( v!`!aa`aaai`0F{I<9HwyN'>~%(&{L h%`u `yH`aE`EaasI`4F ` h`8` Hx)!J`IJh!*JhBpApp Hx)H@h( HhpG@xpGa @@@@src\hal_ccm.c@6H!pAp!ppGpFF F  /I/Jpa`U`p )I p)H!`A``(IH`"L x( #I`.!( `pHhap p!p Hx)( @xHx) +(Hh xHh)Ah)Ѐh( pG pGc @@@@(! @xpG@xpG( pGpFx + C(p   $ $"$$X hx  ChppxpG" 1 F"F )" 0 # FF" 1F x#@I +CppGx@pGxRR +CppGx pGp FF F" 0 + x@@ p)xIC pppFFF*ѡ F*F1F 0 +`x C`pppF@xF,á g,$)F"F 10F + FppFFF*ٸ P F*F1F 0 +`x C`pppF@xF-Ҫ 5-٦ -!F*F 10F +(Fp"0 + FF"1F +z"0 +t FF"1F +kAv +vpG~B~CpGv +wpG~CpGAw +wpGBCpGu +vpG~}CpGupG}pG 0yR R +CqpG 0ypG 0yI +CqpG 0y@ pG"0 +# FF"1F +0A~ +F}D~}!CCgM}B** ؊B +ҋB؁BCB 0 0! +FBTI")pG@xpG@xpG" pGxCpBxR RBp))) E0 j! +CBpxCpBxR RBp))) 70 N! +CBpxpGx#@I +CppGx@pGx#@  +CppGxpGx#@ +CppGxpGpFp  +    $$$$ $ $ $$E $ +hx@ @ ChppxpGpF@xF- H +`"1F src\ul_pdu.cz : +8ppF@xF-٠IH +a"0F +%pq +rpGzyCpGAr +rpGzBzCpGr +spG{zCpGAq +qpGyByCpGqpGypGAs +spG{B{CpG0Az +Fy!CSyy#C~MyB(( ؈B +ҋBzzCB 0 0" + F"F +Ar +rpGzBzCpGqpGypG" + F"F + +xsIxAspG{ +p@{HppG"0 + FF"1F +"0 + FF"1F +" +z F"F +r" 0 +l FF" 1F +cqpGAq +qpGq +rpGypGyByCpGzyCpGqpGypGqpGypG" +? F"F +7" +1 F"F +)pFFF*7 I +*F1F +`x@ @C`pppF@xF, I +"F0F + Fp@x( pG pG"" +Cz FFFF 0%F1`6@5//-/+K ` `htttvvss t`tt#+t(v胰phss w`wrt0rprr2tt (/R` +@"ACB@}  +@0//zr hs"$!\@2[q[q[q[q[q[vYrj{@Qis(/ tJ F0* )Ps + HC +灈pF@4f{F.h%.f)d> @]{HC%! +FsBR0 < `* (pH 0~( pG pGH 0~pGH40~pG!vv`AbH 0~(H40~(I ppG F~~BFF8L`hhIx)$!hFNhFxI `hhhRhQah`I`h41"B)Br!rq8}!hah`HaI!ArNF 6pj4hh`h0jj('F87 FH07(Fd0 8q))})Q)zЯ (8yr(ГH ;0j(ѕHr8 I9HC@aI苟9HCaJ:1jPC iJ ;"bKR];ZC00 b`b0j8!58B}IByHse8 PvJ:hPCmaqI苟9HCea0j(lHe]8 5X @[kJ(h PCi@b!ihHCNibJ@;!I]QCA00 biPCA `bj5PCi@1h8Ih@(F@0(0j(LHFE8 LIiHCbF +!ihRKCiYcFSC;"R]CKZCQ11!b!Aab1j@5(4 ](s8IHCi@HCb(F@0()(-H&8 -JiHC@00 bPCA `b(Сj8!#8B 8q((H&8 j8bFpij8!8B  src\ll_lm.s0.c'6*(I pjpi +`aj jB``Ih`( xq8p(4 ]( !H@ 4`u1:F0F I(JhQj hhɋKiYC O5"xhAj hhR\* +IbB@z(IH Fxh@i@yhe h.ZBRZ@(Xh6"\*B#BRB(@h@ F@0a@@ 4Ij yvKI}Z[#((ЯHI:0 yhhB+ t xhhAIApM (ph!b~ TTFTBjhhphQ`h 0@}(h h l(ܨhhJaB"QabQahIahhdhAahh bah ah +l` +lBal`f Fo({H|I8hAa(vHwI8 @.prHsI8nJ Rh $   --*-]~(~( cI@ ~(~(\I^H +[HWI0 󵁰QL%F&' 5 1<<1CC<J)h ))C.@"C@p(|(hy(пHT8I`iT9x0i Fixixy(Рy(  O p qH0L( `p `pHT8aH@|(`x(iOHT8HJ`iT:0i I"Hi2#vv"T@0|( +p! ! pL`y FAiF@0*"K F3~*ЂMRbz*= R~*O"z*P 6= 2ѐpsHH* (`1x*‹UŃ BZ**BSC B҂B" |*(ZH{(>  x`i~( B p!FpupLIMH{*+*)*'*IMH{))@iF@1ʊQ)!Q @iF@1*BɊQI@i )I) + I)) +9 ) F$H0('H"8Li!i?i"x@p)F{&'+]]]aa]M]a=8]`iq! \)!0A~i-KiFiCaii1Yaii1b8 src\ll_slave.ckx!ir# ixiV iiS iAiR`i`0|(iFx8CpiFihIH6*(HI- --`i^!R`0p`i@0FL ai@\( (4`1Hz() Ht &"1hFai"1hFailFgx7gF`0`F 7l#cCc 1vIHt +ai 1w H0(BL z(>`i%0~N( б{ (ѱ{ (`i'0@( `i\! R`0Et /{ (а{???????=? ???),??????/?`ij! \0Ew `i`0z9Cr (ut ![`i!0``t7t `i`0p pM{(4ѓL"`iAZIB,!z))F1‹BʈB +FB ЈJ0:VˈӁ F.1Q`!p"F! ai 1HwssppyL{(tIwH"tH!BisH`2rp08~ %#.}*EuuuVz.Qr"v "v"vwvspaMy#(z( ^L {(Ѩi'x&bbb.;O +bb REb !{(RHMI38s{(MHHI-8hi`0p{ +(GHBI&8hi`0r s{(Ы ;Ihi`0r {(9H5I8 {(4H0I8 kiiF1 +*`0zwH(Nw'H#In0#L {(H"zCr sL`iF1J* )!z"C!r`0zidai`1ri`ai`1i_ai"`1ȁzCr`i"F1 + +w   wj#\#Kw@0BH{ NL&(`i`0@z( z0C rai%^ ERb FT1iaii1ai"F10eH08~')})GuuuaiFAIi9Ii 9/vH!spM {L( z!C riB(i8ai"PRI(( p@pi ai1piai1Hiai1iai1Ȁiai1 spLyo z(lѴM {(Si'x"& w 5JJ8LGJ)DJ(ai"PTs^ GR`1p8k52{ ( z0C r`i^!R`0p + { ( zC r/t`i`0Fr %{( zC r`i`0p s(O I1`i#0kBkIZABcc}O(x|(8|(_ ({&%pL(`y s`i@0ts! {(( `y  s`i@0t`y s y(=8@8`y`ir! T@0@0Iy(ai`1|(@t`i`0|(=}`i`0t{ (Ѹ{(K`iF1 +*EI)B=I{)>`0|(:A(6(2aiȈ(. +F@2VtЊB @,KB#[BF(Ut*"R\*"JRB!IBFai @1MtaiJO`i"I 0( +H8y)aiF :q H0(ix( ((IH, `i@0tpL`y((7)P)~H M ,,,,,,,,,, ,gp !!p~p@i!gp!p@i +FI`9 0 gp !!p~p@iq/vIH,H8y*)gp!!paq z* )gp!!p1aqr vHx( pGrI 09puv 1qpGpnLF x F(Г kI &ar%b p(gHdII0p`L!x)!x) FF`a" 1XH3'@5tUM(5('(("((((M`i&(%HH0(`i0~(Щ{ ( { ( `iF@0`1Nt `i@0Ft4H08x(aiȍ B`i`iAI))){)n.siiF@1È+[À*`0t{(H|((B( `i*! \)iBӦppppp`iQ!T{))!B ZQ )F"1$0 "!`i`i0Fws`iA  9Ex(*`i 0z(n `i80`i40d!j V+  `i@n*`i@0{Sfqqq&rrfr&q p~ L x( x( a`i 0|( z( IrpGL x(IHH!x*"pqy#CqixCx+py!Cqxixy( `x( Ѡy(( z( L x(Yz(8L`i 0|( Ѡz(hFZ(ai1 @\(ѵJ #mF V(VBPs0:F 1s rz@r8pLF x(ЧHIU0/`y!CM`q.Gax&C(h|( iAxxyBfrGx( iAxx:iyx@B`y!C`qiyxB`y0C`q qyը{( z!C`qy@q( h|( xHT8@x)( [ x(z(IpgHx(gHeI0  /^I p\I3"IiPT]J svpGWL`i0((VH{ (`iF1 +t  `0zCw `i"`0zCr* +#d <T[۲B (6L!`i0(`i!0(0I2HDai"F10 |*N {]%$L( z(C riFi[hFx!hFp`i*`0t`0t`i^"R`0p s|@IB( pG pGpM {* L( z!C riai"PRI(( Hp 2 + baii1q spG8L8M`y@ թ{ ( z!C r$y ը{ sH%HyIz)"I{     @i"b! +T0~!!@i0AwHy z)J{#o @i`0p@|(  @i`0rps  ` R\`@#FB `F +h *Fh +`pGh +``pG(" +) *(:8  H      F( F( x( ( ((w  x }  `B0src\host_hci.c𵗰! %N'uut4|h@) ЅiFpMphiF!hF Fd(K`tIKJ B I|)!CpGFFx!J@R@JQKB+KB JCQ- (p"x'@l !7U]j9 ( !)pp +Ap !)pp +Apሁp +p !)pp +Apሁp +p!q +Aq!iq!)p!x)!)pp +Apሁp +p 0 Fj0(}iF0 y@S +!)pp +Ap + !)pp +Apሁp +p 0ࠈF@@0B[)Y (p hBZRSp +@p`E1IH|(s@Hth!y"E(h@8hG!hFq!Aq(hhF!3 .0)x"C)p)x@"C)p)x 1)pp +Aph0(xA@ r!hFq!+Aq1 @cM)|hFv!F0 00//P`<2YiF ~h*|Bҁ)'O.i|Iit`1 0DLH/)x 1)pp +Apሁp +p"hh @F(x 0(p yhp ~;H +'B'"*p"I2I A( 1M"(F 8 x(q 0'*HBCB8F F*#H$K@B #x]  #mF+pkpF#xaSKވ; ^k*q"iF +pI +yiF +qxx +CiFʀ"y qJpax )# ܉  RRRRR)))))@(9*@@$* jFpHx xC @*ax))''"iF +p"6Jx xC28FkFRB(iF p `Cxx3C xx3CKnF [**wѰt*r!hFp!cxqFxx6>CN`oF1vBoveQF +H jFpЀ@? +"hFp€6 jFpi-* !hFp!cxqFxx6>Cxx6>CNN`oF1vBَZ*X !hFp9RP*N#hFpxxChFR`%@=*; iFp5Kxx3C$N3*-/'#nF3p&K0CXHx xChF€ x( +(! "HA AhF(FS kFp%%"CIJ("hFp yq!  'LFF(i^( 7p( !8C'pi q!iiriHi s( !C(!'p%0x + 0pH0"  F,0m (-@p +p +BB(0pp +Ap@ BF!V0p((X >( ly +(,6hFɲ- IxB"B lx)#FHp lx)y +( ፁ((@B- (p +p 0p lx(  J&m*.@M@5#F3B"؋xOx;Cp +Cp yx:Cp +pq" +Bq"FI芤I m"pBp &0F0zK@3BwKm\xx$,C \yy$,Cdd  `0 0𵅰F&hFUAxx CAyyCJ=#"RF(FDhFAyy CCxxCB +J!R  +Cxx C# FHy +yC"Cq +HqHx xCiF#F,hF Hy +yC@@ q +Hqo(Ђ, F FF'q*H%m(/(, Fq'.)hFHx xC Xyyx +CxXx9Cx8CP(0(Ђ( F.He:8F𵗰! O$=F|@5lss({l8(  iF (p0h!hFFp"hFr) 'x@ՎHAB!Ab p!1H Ab ,nAxx +C +F "iF +r!"$Cxx C[`Fp"iF +r"R +((ЙkH '~iHm( Fp(M+F2F F$FE,@xd((` +!hFrs ;FhFAs!s0:F!hFr!/LHly(p KGI7/Ё p1 p( (p 0hFz(0F/+ 8"4H , "iF +r! >  @')HACAP@(`8F x( (pxhpp +px(q 0HACAF 1x @IJ@ IIB( #!'/p F%  xx CI F@ +B xCBӮB B hF{(*(B ! C C p( ( x Ԡh:F  !C MF/h !HC<Ix +F(F 1@1#FB؄ M@! ) `pp!)C%-C-)C%)C! )xq 1x sIbD +`!!s2x +FSfMd(h4F `C Z ԉ \\IC(@(dB 8p(@ pG pGNIˈBӂ  h !QCY x#CA`ysDÁhRa !pGF ChhFxhFphhhF!q(,BF,4(@(0ih ()9/HB`8hB'0xCiF0()$HB!0F( *Bhh( +d Fd, |yq080x xCyh8a FF7Hz (!(F(P "(x@ !hFp)D((x CxCB +7JBaB  B B"F((Шh pF F:( ), xx CHB  p p ppF F -%.Шx CI. p111pC pIʈB( " hPC 8pG pGP 𵅰FF iFppxI XO--s,r x ywr,k  x ^^^6^PT^0x(v hqhl" iFp(50x (⡁ hx(иhqhk"(h!kBhjk +xqh0x (ӡ u iFpqhHx xChJjQxx CH@iFFȡ ^hFx(hF' hailihhl๡ @h!FlJ PP---Я -x$))BhH!`pHl8fgAgg҉!L0$ F 0 Fɗ uI*x1L!x&Oh 0 p$&O6FpEpF(І0F8F Fd( + iFphF( {X0h&Fjx/ iFphFt( rk0hla ln0@x=P (phhi`i`dH`(FQ( `x0hjx@ iFphF?( W0}hl, R0r@x=P (phhi`i`IH@`(F( F0Zh)3iFq)y)2) >0J:H"0h7H06I 1H` x,!@ p iFp0H0hF F(,, +0$iFo!jFq$Jl:gH```!hI1` !C pEZVl@y ( 0hz)Ak@hh r iFphF( 0hl 0  src\sm.cA@x=P (phAki`l`(Fs I0)F(1"hFr"px(yChFp @(y@ChFpꈂJl( I0!uhFzh&@l iFphF4(IHshl"у Ihhl(HI\Hhl +HI 0P@x>P + 0p(z0qhhM`h@l0a 0sapa0FF(,, I@. F1jO(x!@)^!@(p*@@ p"@ p jFp qHlhFF(--ЧHI:0iF(F" x!@)!@ p uHxv( iFphF(ЗHIT0hl I@HhlыHIZ0@x>P 0pHh@hp`hh``0FuЀH~Ie01j F iFphFc(wHuIz0@xhlMQ nIhlBjHiI0>P)iA` 0phhp`h`cH0a0apa0F-(\HZI0k(uZJ 2p kFp(y(pRH# 0@h'pxPJ;@pKz'Cp2W` {pzrSz }[[#C F4`$#@~#CSr F3aGqqz;J 2h,єx,Лrx[ԋz[(Kz[[[xd۲<]_#@LzSydd#@Cqy'<@qC K\px[p#x'<@zh;S &;Liww+ iFs =(VHTIA0MTI &NH`NHih 0`hB` iFp"@p pDH0Ah`@h( qu "T+ iFsFbH 62yy8$x= h)"iF +p" +q +"Jq9i9y)}x*z *x 5M ! 8y(fBL< x(( py +) +%`p9H"G 0q p iFp3H8i@7)=.L<(`haxzB%w&H"I08& p E)"L<( pH"I 8 0qxi!pbhQhA`I 9`!F 1`a}SzIICupTha#@Sz[Cuiv)(g iFq@xHq8iQ, DR )L(}Ѡhk"I!hFp8i!hFq W )L(~Ѡhk"Ih"@kgHAjG(u!hFp8i!hFq F0AyII>A\)L(=Ѡhk"Ir!hFp8i!hFq~ p)ѾI ($Bxhl +p@l"U!hFp8i!hFq_H0Ay"Aq^0q)ѫI +(%hl"I2!hFp8i!hFq>H0Ay"@%j)x)~ћI(jhJ`xF-\!jFp9i!q$ pxRRpKy[[Kqy[[qъqJqF +y@xBjpH"0 ,p0qFhFK)x)(o"~Hih 0q )x)wO(wH"0ih 0qxhAh)!)p!)q@h`r (x((y(F-!hFp8i!hFq!pEp!hF!hFp!qEq[IhIh(H`ZHH` 0qxi"pJhB`TJ`RJ 2`a}KzRRCuha#@Kz[Cu ia0 )x )CI( p iFp8i q!p@"ihZ!hFl 0q)x )(4Ijh 9 @ \\B %0q( 0qxi!p)IIhIhA`(I`9(x +( iFp8i q!p@"I!hF1H!@xjh ɲ +hhH0x Ёy)@y( 0q)FhF iFp!hF_(x(Hih`i@xp  +   $R(x(yѨh8a(y8s (x(Lihh"j 0qxi"iphhC`j`~(x (Lihh#ii 0qxi!phhB` if(x (Lihhbx@j`xh! Rjɲ!hFp8i"hFq"ph@Ij!hF n(x(v!hFp8i !hFq M ph"i`nh"jh!hFzz u (x(OѨh8a(y8s 0qHhHix($xi"phA`I`I `0HhAi x) )x )+ѭLihj"/h Iip!hFp8i!hFq!pI@h"j!hF+ 0q !hFp)F#F(}Јo(x(l iFp8i q !pIh +xBpIh"!hFH 0q iFp (x(QѨh8a(y8s 0qxi!p}IhhB` i`{I(x (EwLihh"j iFp8i q +!ph@j"!hFiH0@y(eH!p + aIp$  R]I( 0qp + iFphFx(!hF$TH!p FRK$\rX`r"Iɲ  3T)JNFqz )q'rzsh\ 1Y\ dXjp!x   ',YJOT/p|!F 1 + y ))g`h !2"h5 +N)x)t!])F Fi!F 1 y ))d`h"h" h+)x)Q +!:)F FF)F FkA)F FVF 0 Fy!**:ahWI1!q iFp)FhFF)(x(&ih"hhh{JJh#2F"hhwh{BJh#2F (phh`h!`F>F&FF 61y ) )9x) 8p `h"h"h !(x 0q iFp9FhFF 3y",,,%!qh" +p@hH`H``F x ,"qh#pIhQ`h`FFx ( p"F$ xR[#Cp@ xIҲ *, Rp F-!(F"TI(F9RH!8DE` p ppLN$F>qh @{{B {{B ")F(d0B Cp Fpp F +-:L< a@B p p0F(@ + ah@"1F @ (p ppF ,!p((hx)xC!p (F(@ph{){C p ppFF + 0px(I9 +B  p,(x p( +Pp p, IhA9" F(x s( +`s 0p p sI h` pGqI` pG!oJpK((nHpGc pcp pGjI `hHpG"hKfI(Z`i"Cai@@a`\I 1` pG\I"(( ( (VH@pGiCiCa pGiCiCNIJjCJb pGKIJjCJb pGHIIj` pGEIiCa pGBIiCa pG?Ii` pG0F rF:J8L@2-#- - b0pdx,Гccpp/I `)I 1`(I p+I +h# +C +`(I`pG"HxpGprL`x@`p t`x(Ѡx(I @1c-bpprL`x( X`x@`pѠx(I @1c-bp@@@  @@src\soc_power.c(8JiJh@pGIHxxB@!"QpGI#1HxxB@!"QpGIF1HxxB@!"QpGH 0h"C"C`I #1Hpp#9HppF1HppHH@ pGp FF&IB +F :# Fd( Јx\(pxm@p!(ыp&0Fp pGp FF&yI#1B +F :# Fd( Јx\(pxm@p!(ыp&0Fp !p pGF~ p p FFvcIF1B +F :# Fd( Јx\(pxm@p!(ыp$XL Fpp FF!) SG0MLH@h8@Ʋ B JM0;A!F(Ff(p@NF#6xpx!"`5F#5;L(Сx`x!"V(Щxhx!"O(%2IpxTpx@pp!( pp+I`x 9T`x@`p!( `p %Ihx&1Thx@hp!( hpxpx!"!(Сx`x!"(Щxhx!"(H !IB H@BАB) pG pGH(X  src\soc_rand.c(H(& #H@(!! "Moh&HL(&`JI P`# `K`k``h(Hh(Hh(.o```(H  src\soc_ecb.c@@@0rJi# FCa,bM @,hB  +riCa(b0 @ IxJx@BppGJ H @ AxxCph+xB pG LF`h!>U!x) 1AX2FI2  +X($x%Fah xU'pHBhX(0X(H,FAh \-H!@hUGTpJF zSh +V]B 2XB@B pO>xF.q4%Fxh5@Y( xh@YFxh@Yy([IHh] +pB\* RBTAY"  +AQ0F0c\"FX*#Bْ CYW +CQcFX + !T +CYR +BQ"BT0FWH 4Ahh YXGL x(ah0XF xbh0X(FFFF+ӏ "H!`E`rtp"BpFR@TBpLFex |BӀ hFP`x@`p(FpF$mvOFyxBt qH!z@h +FR\*%BbF 2XBIɲB,!JT F 3P_O"+ P#B٪  F  PFSF6 +Q + -%k- + F3PBT=F>x F(xBih0X)xjh 1RXFFp(p FFF!F(F(3M Fih0X1FU! @ih ]@ )Љ( p p󵁰F(*"N5xrh)F FB%F#\,,ХB\0p(0XF0xrh0X(} 9F"qh#T U\+T@! +B pG src\soc_timer.c$J!KT``K`L `HiCiv%,CaLa`M)` h/.i&CahhJCi``!`!J `F ` @@@@pI +h*#F$JhRJ` *M`y"F@hB +r hC `bIh` p p[ +6  I`pGr!@HhC`*bH!`A`pG src\soc_evt.c `pG H`pGHh pG Hh pG +H@i pGHipGIiՉiB pG pG@L {zB&M'z!\ +FBCP|+++(C|+3H {zBtz +(z@rIȲX! tz +(z@rIȲX!G tz +(z@rIȲX! Gtz +(z@rIȲX! pM hrH$Dww80srHHurI``lqpp,qlpH!pDpHpu(0pI D A q[HjL pHe`ppL xx`xuL z(0 rpLy(ЩHpz( @&q%r`z!"PCJtHhIi` +H`` I`PHx((x(ЦpppFI "BCR :*@pG"BCQ +F@:b" 9 +wpG(() pG)zL#cp}J(p ( +) {DyD p : p `ppGX w  pGjH@xyA"PC[J <) (XhH (TfH wpUL`!B%%Pa"B!!BpFHF"PC@J(F|-(*;L'z"{B!#\B'/ +(B\"T +(@"{B/ {( {@" + s t`F+L( AC#H 8(|(z(0Hqz(qK|(L { +(t {z@B Z Ht!{`T { +(e {@ s`zFBF'xC|BbzB5+  src\rem.cD x P  $    m@az"QC|ar!r"BC'yC|t"!PCtz({DyD +z(А !! s! 2! +!! I0!h@(` z)!@ +)`h@h +h`z({t!z("FBh`F0H@xBx*8F0"0bz*bz#ZCKRhBKaz"QCJJhh  + +B:ؚB8ؙB60 +(``DJh +FB#BF + BcFbEY@ bE@@@B(_IHL(h +h` (rhh'0 +h`@zrzhr(( ( :!2 Hx)) AxBx((hJFhh + +BFBػBaz)gzaF":F#ZCKhB|/wY"tbz*bztar r/c"9FF"~KJCShB)ґ{zB!{Mt({ +(({z@BpIwH({,T({ +(({@(sz( !F|)`F(*=F *FBCbH!1t8{ +(bH{zIBacH]H{ET{ +) {Is|Bz(! tMM`zBt`r r!/(JNHa"PC@tt(BGHQ"PC@t t p FF?I$ FA6L qp){)%(i@ +h` (w. B *iQ  +i`(  +3hz!OHCN0hiFiF V(@hkh +K +B ت|*#ZCRhQ  +BӰh@ C`0h0`h@ +pa  F@IJ _I`CE(|(w XbH@B cJ#KC|B +) IɲBZH})a|B^ !)))(!)tz#{B!FV\B#+ +)N]VT +)I{ɲB+ {){I&! +!si|)*otz#{B$B\B#+ +)J\BT +)I{ɲB+{F* +{R"! +" +sH')|)j|* pF J!hCLt { +( {z@B H {%T { +( {@ spz(    D src\rem.c $  3x  ? F " HI wFI HaHJhB!`pGT @ JpQppGH"xAqBpppGpGpGpG  pGpGpGpGpGpGpGpG0F F \%F0= +- C0 +%hC08 RҲB0p FF +!*01 :dw,p# +C\D@ `@$ D@  `@[B 8 F3#',17iWO6h@3++bQHI ) OI#"$$$$$$$$  /$$c$$$$ $ +$ $ $$$$i! +Ca- b ,M4Hbb*I2H`2I`!F bO F F)( p !I` pG F(( ( ( `ah hA +) hI h` `H IhB pG pG@ @@ (  (8JiJh@pGF( F( uI x @+JhCH`pHoH@oI` F( hI x @+JhCJ`cHcI1` p FF( ^Hh!@@ (` pVHpF( @RI` NHFz( @JI1` EHp FFh((Fq(Р @ @,":5JiC +Ca 4Hp3H@p/JhC +C` pp FF8((F> p p&HpO!I&H`OpLF!x ) prN61hCa`C0`b (p pI +x*(JHh` p pGF (!@B  (( pG pG,  F%&(ZHcc CWHccC`UHE`CSHccCPHcc@C`'BLa  +NIcNHccccc C  +IOc/ c/ C,eddBHB8 ! +F#F@*L%@8N5`8N5`,%-=5MviF@CF@%Ca /Mv5hF@CF@%C5`I )p'L F `j h(FzjpL hR h `HhpG@@@@@src\soc_config.c@@@4 p$F F F!F****   + !!!!!!( Fp$H!pppGpM#k`Kh,$h`h. dB h`h(ppL FaxBТl + O%%OOOOFO x(Кp  px((h"ihh p ppF`hF x(Ї JIHB@`I`x/pB"Jq x( pw |ppFoL`` x(p nsI p" +q`"JqpJ` p`xpcLx( x( hH`p`x( p  $ d!`KBґ@h :Xh@@ FFOF?T B"",!1B MIBӯBӅB҇B(F0C B * (F(8F( ?H?IhB"F1F(F(*H!``ap F1!HC0f F.H +B B F( &H'IhB F +a(H!`p H< H  @@pG FFB H`C` L`x pp8 @@src\soc_flash.c0ut +D@` TH!p!pApp`pGI +x*h`h8gh`hxIJ++y++Cy++ +уhd;Bhy++ pGCy++hd;BҀhB pGF&h(ӡ #'M, x((((/q `h((`h&&0F(x(`h(`hx(&Шx(й O 8p`hy( 8q`h@y(7 xq`hhh``hh0` pIxnH!pah y)!qahIy)!Aqahh1`ahh`hL4`` `op p!! L FxB 0(FO%&7 7Z|Ġx(Рx( 0x(Рx( ~"0x p%q x(xG`}Ij}J@"a{JC@b PTx(Рx( kE0S x(Ox( * `H`Fx px(Рx( Zf00 x(Ѡx( LF8hq(yhB px( J0 hG&F6Bx( B0 x( =0x( +`x(Рx( 50 x( y( W%q`x()Ixsep (0F- +-$0F-  hGFpLx( p p Hapx(Х` pp p Hx) x)+Ёx)*Ёx)'!px)"Ѐx( L  =src\soc_radio_timeslot.c@H h 9NFx(( , F( + x2M(аx(#   '/p y( (q`y( hqh0`h`h"L4``V `p  (p y( (q`y( hqhh`h0` pIx   HjJ@ +JiCboLx p p p`pL  @~ +J!Q` +I h+А`h( P`h(pG @ipG@@pGpGpGpGpGF C* B!B  C*R x x@Ix x@IђF*,Ӄ xIp@R xIp@R xIp@R # +mF@F@,C*R xIp@R xIp@R xp )ҋppG) p@I)Ӄ""xxFCxCxxCC  +CpG +pJp pppG" Bs +BX B< B!F C"CBt B_ +BD B( B "  B e B + BRA BӋRAC BKRA B RA +BRA +BӋRAC +BKRA +B RA BRA BӋRAC BKRA B RABRABӋRACBKRAAFRAFpG]IB@BS@"F B- +B" +B ӉBӉBӉ:В BRA BӋRAC BKRA B RABRABӋRACBKRAAFcFRA[F@B+IBpGcF[@B FFpGpGpGpGY0x@x@ x@x@ pI[ pIRѡB 0#hK`K`K`pGJK`rF:JKhB K ` KG FF +KhG KhhGp t x h ﾭU9(  HIpGu j(HI hBHI`HI hBJ`[hG HI hBKh IBЀIBHh! +C`HGHGHG  ,@  ` $@k9UPOHpE i8x( (#( Jh KB`( +JG `pG JGJGJh,2hG ʭ;:? KIGK IG K IG K IG KIG KIG KIG KIG +KIG9.==o<<9c1=+0tFd%xdBFc][0GI a 9rpG  @UUU־dK2R R dOR|  I"hBpGV)+67 4_tpOϖBk/ϼQ \ No newline at end of file diff --git a/setup.py b/setup.py index 26e2db617..2fdc1ed3f 100644 --- a/setup.py +++ b/setup.py @@ -3,10 +3,18 @@ from setuptools import find_packages from setuptools import setup +from glob import glob # read the contents of README.md file fo use in pypi description directory = Path(__file__).parent long_description = (directory / 'README.md').read_text() +def relative(lst, base=''): + return list(map(lambda x: base + os.path.basename(x), lst)) + +data_files = [ + ('binaries', glob('resources/binaries/*')), +] + setup( name='cflib', version='0.1.24', @@ -45,4 +53,6 @@ 'pre-commit' ], }, + + data_files=data_files ) From 2a71890fc4e1b50ec532d06931524f986c8b9b04 Mon Sep 17 00:00:00 2001 From: Marcus Date: Sun, 28 Jan 2024 21:51:30 +0100 Subject: [PATCH 603/861] Reworked sd+bl bootloading with artifact version and flash address override --- cflib/bootloader/__init__.py | 170 ++++++++++++++++------- cflib/bootloader/boottypes.py | 7 +- cflib/bootloader/cloader.py | 2 + resources/binaries/nrf51-s110-and-bl.bin | Bin 126976 -> 96256 bytes 4 files changed, 127 insertions(+), 52 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 95661bd21..57a51ebed 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -53,7 +53,7 @@ __all__ = ['Bootloader'] Target = namedtuple('Target', ['platform', 'target', 'type', 'provides', 'requires']) -FlashArtifact = namedtuple('FlashArtifact', ['content', 'target']) +FlashArtifact = namedtuple('FlashArtifact', ['content', 'target', 'release']) class Bootloader: @@ -132,6 +132,75 @@ def start_bootloader(self, warm_boot=False, cf=None): def get_target(self, target_id): return self._cload.request_info_update(target_id) + def _check_and_remove_nrf51_softdevice_from_flashing(self, flash_artifacts: List[Target]): + # Detect what soft device we are running on the nRF51 and what we need and make sure they match + + + upgrade_provides = [] + + nrf_requires = [] + for a in flash_artifacts: + if a.target.target == 'nrf51': + for r in a.target.requires: + if r not in nrf_requires: + nrf_requires.append(r) + for p in a.target.provides: + if p not in upgrade_provides: + upgrade_provides.append(p) + + print("nRF51 has: {} and requires {} and upgrade provides {}".format(nrf_has, nrf_requires, upgrade_provides)) + + for r in nrf_requires: + if r not in nrf_has or r not in upgrade_provides: + raise Exception('Cannot flash nRF51, missing requirement: {}'.format(r)) + + + + return flash_artifacts + + def _get_current_nrf51_sd_version(self): + if self._cload.targets[TargetTypes.NRF51].start_page == 88: + return 'sd-s110' + elif self._cload.targets[TargetTypes.NRF51].start_page == 108: + return 'sd-s130' + else: + raise Exception('Unknown soft device running on nRF51') + + def _get_required_nrf51_sd_version(self, flash_artifacts: List[FlashArtifact]): + required_nrf_sd_version = None + for a in flash_artifacts: + if a.target.target == 'nrf51': + for r in a.target.requires: + if required_nrf_sd_version == None: + required_nrf_sd_version = r + if required_nrf_sd_version != r: + raise Exception('Cannot flash nRF51, conflicting requirements: {} and {}'.format(required_nrf_sd_version, r)) + + return required_nrf_sd_version + + def _get_provided_nrf51_sd_version(self, flash_artifacts: List[FlashArtifact]): + provided_nrf_sd_version = None + for a in flash_artifacts: + if a.target.target == 'nrf51': + for r in a.target.provides: + if provided_nrf_sd_version == None: + provided_nrf_sd_version = r + if provided_nrf_sd_version != r: + raise Exception('Cannot flash nRF51, conflicting requirements: {} and {}'.format(provided_nrf_sd_version, r)) + + return provided_nrf_sd_version + + def _get_provided_nrf51_bl_version(self, flash_artifacts: List[FlashArtifact]): + provided_nrf_bl_version = None + for a in flash_artifacts: + if a.target.target == 'nrf51' and a.target.type == 'bootloader+softdevice': + if provided_nrf_bl_version == None: + provided_nrf_bl_version = a.release + else: + raise Exception('One and only one bootloader+softdevice in zip file supported') + + return provided_nrf_bl_version + def flash(self, filename: str, targets: List[Target], cf=None, enable_console_log: Optional[bool] = False): # Separate flash targets from decks platform = self._get_platform_id() @@ -140,10 +209,12 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo # Fetch artifacts from source file artifacts = self._get_flash_artifacts_from_zip(filename) + for artifact in artifacts: + print("Found artifact for target: {}".format(artifact.target)) if len(artifacts) == 0: if len(targets) == 1: content = open(filename, 'br').read() - artifacts = [FlashArtifact(content, targets[0])] + artifacts = [FlashArtifact(content, targets[0], None)] else: raise (Exception('Cannot flash a .bin to more than one target!')) @@ -151,46 +222,41 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo flash_artifacts = [a for a in artifacts if a.target.platform == platform] deck_artifacts = [a for a in artifacts if a.target.platform == 'deck'] - # Detect what soft device we are running on the nRF51 and what we need and make sure they match - nrf_softdevice = [] - if self._cload.targets[TargetTypes.NRF51].start_page == 88: - nrf_softdevice.append('sd-s110') - elif self._cload.targets[TargetTypes.NRF51].start_page == 108: - nrf_softdevice.append('sd-s130') - else: - raise Exception('Unknown soft device running on nRF51') - - # Check if there is a nrf51 firmware in the manifest and skip the soft device - # flashing if we already have the correct one - nrf51_artifacts = [x for x in flash_artifacts if x.target.target == 'nrf51'] - if len(nrf51_artifacts) > 0: - nrf51_fw = [x for x in nrf51_artifacts if x.target.type == 'fw'] - if len(nrf51_fw) == 1: - if 'sd-s110' in nrf51_fw[0].target.requires and 'sd-s110' in nrf_softdevice: - print("nRF51 s110 already flashed on device, skipping") - flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] - elif 'sd-s130' in nrf51_fw[0].target.requires and 'sd-s130' in nrf_softdevice: - print("nRF51 s130 already flashed on device, skipping") - flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] - else: - raise Exception('Manifest should contain at most one nrf51 firmware file') - # TODO: Bootloader without firmware should be supported - # TODO: Bootloader without soft device should be supported - - - nrf51_artifacts = [x for x in flash_artifacts if x.target.target == 'nrf51'] - - # First flash the nRF51 soft device and bootloader if needed - rf51_sdbl = [x for x in nrf51_artifacts if x.target.type == 'bootloader+softdevice'] - - print("nRF flash page is {}".format(self._cload.targets[TargetTypes.NRF51].start_page)) + # Handle the special case of flashing soft device and bootloader for nRF51 + current_nrf_sd_version = self._get_current_nrf51_sd_version() + required_nrf_sd_version = self._get_required_nrf51_sd_version(flash_artifacts) + provided_nrf_sd_version = self._get_provided_nrf51_sd_version(flash_artifacts) + # TODO: Figure the versions out. It is an int we get but the JSON contains strings, decode from int representation + # to some string? + current_nrf_bl_version = str(self._cload.targets[TargetTypes.NRF51].version) if self._cload.targets[TargetTypes.NRF51].version != None else None + provided_nrf_bl_version = self._get_provided_nrf51_bl_version(flash_artifacts) + + print("nRF51 has: {} and requires {} and upgrade provides {}. Current bootloader version is [{}] but upgrade provides [{}]".format(current_nrf_sd_version, required_nrf_sd_version, provided_nrf_sd_version, current_nrf_bl_version, provided_nrf_bl_version)) + + if required_nrf_sd_version != None and \ + current_nrf_sd_version != required_nrf_sd_version and \ + provided_nrf_sd_version != required_nrf_sd_version: + raise Exception('Cannot flash nRF51: We have sd {}, need {} and have a zip with {}'.format(current_nrf_sd_version, required_nrf_sd_version, provided_nrf_sd_version)) + + # To avoid always flashing the bootloader and soft device (these might never change again) first check if we really need to. + # The original version of the bootloader is reported as None. + should_flash_nrf_sd = True + if current_nrf_sd_version == required_nrf_sd_version and current_nrf_bl_version == provided_nrf_bl_version: + should_flash_nrf_sd = False + #elif provided_nrf_sd_version == None: + # should_flash_nrf_sd = False + + if should_flash_nrf_sd: + print("Should flash nRF soft device") + rf51_sdbl_list = [x for x in flash_artifacts if x.target.type == 'bootloader+softdevice'] + if len(rf51_sdbl_list) != 1: + raise Exception('Only support for one and only one bootloader+softdevice in zip file') + nrf51_sdbl = rf51_sdbl_list[0] + + nrf_info = self._cload.targets[TargetTypes.NRF51] + page = nrf_info.flash_pages - (len(nrf51_sdbl.content) // nrf_info.page_size) + self._internal_flash(artifact=nrf51_sdbl, page_override=page) - if len(rf51_sdbl) > 0: - print("Flashing nRF51 soft device and bootloader") - self._flash_flash(rf51_sdbl, flash_targets) - # Now remove the soft device and bootloader from the list of artifacts to flash - # and then reboot into the new bootloader - flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] self._cload.reset_to_bootloader(TargetTypes.NRF51) uri = self._cload.link.uri self._cload.close() @@ -202,8 +268,12 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo print("Reconnected to new bootloader") started = self._cload.check_link_and_get_info() self._cload.request_info_update(TargetTypes.NRF51) - print("Flashed sd+bl, we are now at {}".format(self._cload.targets[TargetTypes.NRF51].start_page)) + + # Remove the softdevice+bootloader from the list of artifacts to flash + flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] # Also filter for nRF51 here? + for artifact in artifacts: + print("Found artifact for target: {}".format(artifact.target)) # Flash the MCU flash if len(targets) == 0 or len(flash_targets) > 0: @@ -300,23 +370,23 @@ def _get_flash_artifacts_from_zip(self, filename): # Handle version 1 of manifest where prerequisites for nRF soft-devices are not specified requires = [] if 'requires' not in metadata else metadata['requires'] provides = [] if 'provides' not in metadata else metadata['provides'] - if len(requires) == 0 and metadata['target'] == 'nrf51': + if len(requires) == 0 and metadata['target'] == 'nrf51' and metadata['type'] == 'fw': requires.append('sd-s110') - # If there is no requires for the nRF51 target then we also need the legacy s110 + # If there is no requires for the nRF51 fw target then we also need the legacy s110 # so add this to the file list afterwards add_legacy_nRF51_s110 = True - print("Legacy format detected for manifest, adding s110 requirement") target = Target(metadata['platform'], metadata['target'], metadata['type'], provides, requires) - flash_artifacts.append(FlashArtifact(content, target)) + flash_artifacts.append(FlashArtifact(content, target, metadata['release'])) if add_legacy_nRF51_s110: print("Legacy format detected for manifest, adding s110+bl binary from distro") from importlib.resources import files content = files('resources.binaries').joinpath('nrf51-s110-and-bl.bin').read_bytes() - target = Target('cf2', 'nrf51', 'bootloader+softdevice', ['sd-s110'], ['sd-s130']) - flash_artifacts.append(FlashArtifact(content, target)) + target = Target('cf2', 'nrf51', 'bootloader+softdevice', ['sd-s110'], []) + release = None + flash_artifacts.append(FlashArtifact(content, target, release)) return flash_artifacts @@ -341,7 +411,7 @@ def close(self): self._cload.close() self._cload.link = None - def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_files=1): + def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_files=1, page_override=None): target_info = self._cload.targets[TargetTypes.from_string(artifact.target.target)] @@ -349,6 +419,8 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ t_data = target_info start_page = target_info.start_page + if page_override is not None: + start_page = page_override # If used from a UI we need some extra things for reporting progress factor = (100.0 * t_data.page_size) / len(image) diff --git a/cflib/bootloader/boottypes.py b/cflib/bootloader/boottypes.py index 0c8239ac6..959c9d1e5 100644 --- a/cflib/bootloader/boottypes.py +++ b/cflib/bootloader/boottypes.py @@ -79,6 +79,7 @@ def __init__(self, id): self.buffer_pages = 0 self.flash_pages = 0 self.start_page = 0 + self.version = None self.cpuid = '' self.data = None @@ -86,9 +87,9 @@ def __str__(self): ret = '' ret += 'Target info: {} (0x{:X})\n'.format( TargetTypes.to_string(self.id), self.id) - ret += 'Flash pages: %d | Page size: %d | Buffer pages: %d |' \ - ' Start page: %d\n' % (self.flash_pages, self.page_size, - self.buffer_pages, self.start_page) + ret += 'Flash pages: {} | Page size: {} | Buffer pages: {} |' \ + ' Start page: {} | Version: {} \n'.format(self.flash_pages, self.page_size, + self.buffer_pages, self.start_page, self.version) ret += '%d KBytes of flash available for firmware image.' % ( (self.flash_pages - self.start_page) * self.page_size / 1024) return ret diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index f0401b2bf..6becd1609 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -201,6 +201,8 @@ def _update_info(self, target_id): if len(answer.data) > 22: self.targets[target_id].protocol_version = answer.datat[22] self.protocol_version = answer.datat[22] + if len(answer.data) > 23: + self.targets[target_id].version = answer.data[23] self.targets[target_id].page_size = tab[2] self.targets[target_id].buffer_pages = tab[3] self.targets[target_id].flash_pages = tab[4] diff --git a/resources/binaries/nrf51-s110-and-bl.bin b/resources/binaries/nrf51-s110-and-bl.bin index d6f8a70d8f1f2e8b9f8c64fb548e041f2358521e..0a682e7e0414ca8b12a8185c80d1380019f360ba 100644 GIT binary patch delta 12 TcmZp8z~1nIwW(rR1!DyOCG!Pt delta 202 hcmZqpz}oPDeY-*l;{)dH1|^Iah9G%>v0({g1pv{cN=N_z From cb1dcbaeabd8095dfd67d7e9ebf54d2a5c24a114 Mon Sep 17 00:00:00 2001 From: Marcus Date: Sun, 28 Jan 2024 22:22:23 +0100 Subject: [PATCH 604/861] Fixed PEP8 errors --- cflib/bootloader/__init__.py | 97 +++++++++++++++-------------------- cflib/bootloader/boottypes.py | 2 +- setup.py | 5 +- 3 files changed, 45 insertions(+), 59 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 57a51ebed..4fe52ed76 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -27,7 +27,6 @@ """ import json import logging -import os import sys import time import zipfile @@ -132,32 +131,6 @@ def start_bootloader(self, warm_boot=False, cf=None): def get_target(self, target_id): return self._cload.request_info_update(target_id) - def _check_and_remove_nrf51_softdevice_from_flashing(self, flash_artifacts: List[Target]): - # Detect what soft device we are running on the nRF51 and what we need and make sure they match - - - upgrade_provides = [] - - nrf_requires = [] - for a in flash_artifacts: - if a.target.target == 'nrf51': - for r in a.target.requires: - if r not in nrf_requires: - nrf_requires.append(r) - for p in a.target.provides: - if p not in upgrade_provides: - upgrade_provides.append(p) - - print("nRF51 has: {} and requires {} and upgrade provides {}".format(nrf_has, nrf_requires, upgrade_provides)) - - for r in nrf_requires: - if r not in nrf_has or r not in upgrade_provides: - raise Exception('Cannot flash nRF51, missing requirement: {}'.format(r)) - - - - return flash_artifacts - def _get_current_nrf51_sd_version(self): if self._cload.targets[TargetTypes.NRF51].start_page == 88: return 'sd-s110' @@ -171,10 +144,11 @@ def _get_required_nrf51_sd_version(self, flash_artifacts: List[FlashArtifact]): for a in flash_artifacts: if a.target.target == 'nrf51': for r in a.target.requires: - if required_nrf_sd_version == None: + if required_nrf_sd_version is None: required_nrf_sd_version = r if required_nrf_sd_version != r: - raise Exception('Cannot flash nRF51, conflicting requirements: {} and {}'.format(required_nrf_sd_version, r)) + raise Exception('Cannot flash nRF51, conflicting requirements: {} and {}'.format( + required_nrf_sd_version, r)) return required_nrf_sd_version @@ -183,10 +157,11 @@ def _get_provided_nrf51_sd_version(self, flash_artifacts: List[FlashArtifact]): for a in flash_artifacts: if a.target.target == 'nrf51': for r in a.target.provides: - if provided_nrf_sd_version == None: + if provided_nrf_sd_version is None: provided_nrf_sd_version = r if provided_nrf_sd_version != r: - raise Exception('Cannot flash nRF51, conflicting requirements: {} and {}'.format(provided_nrf_sd_version, r)) + raise Exception('Cannot flash nRF51, conflicting requirements: {} and {}'.format( + provided_nrf_sd_version, r)) return provided_nrf_sd_version @@ -194,10 +169,10 @@ def _get_provided_nrf51_bl_version(self, flash_artifacts: List[FlashArtifact]): provided_nrf_bl_version = None for a in flash_artifacts: if a.target.target == 'nrf51' and a.target.type == 'bootloader+softdevice': - if provided_nrf_bl_version == None: + if provided_nrf_bl_version is None: provided_nrf_bl_version = a.release else: - raise Exception('One and only one bootloader+softdevice in zip file supported') + raise Exception('One and only one bootloader+softdevice in zip file supported') return provided_nrf_bl_version @@ -210,7 +185,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo # Fetch artifacts from source file artifacts = self._get_flash_artifacts_from_zip(filename) for artifact in artifacts: - print("Found artifact for target: {}".format(artifact.target)) + print('Found artifact for target: {}'.format(artifact.target)) if len(artifacts) == 0: if len(targets) == 1: content = open(filename, 'br').read() @@ -226,33 +201,40 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo current_nrf_sd_version = self._get_current_nrf51_sd_version() required_nrf_sd_version = self._get_required_nrf51_sd_version(flash_artifacts) provided_nrf_sd_version = self._get_provided_nrf51_sd_version(flash_artifacts) - # TODO: Figure the versions out. It is an int we get but the JSON contains strings, decode from int representation - # to some string? - current_nrf_bl_version = str(self._cload.targets[TargetTypes.NRF51].version) if self._cload.targets[TargetTypes.NRF51].version != None else None + # TODO: Figure the versions out. It is an int we get but the JSON contains strings, decode from int + # representation to some string? + current_nrf_bl_version = None + if self._cload.targets[TargetTypes.NRF51].version is not None: + current_nrf_bl_version = str(self._cload.targets[TargetTypes.NRF51].version) provided_nrf_bl_version = self._get_provided_nrf51_bl_version(flash_artifacts) - print("nRF51 has: {} and requires {} and upgrade provides {}. Current bootloader version is [{}] but upgrade provides [{}]".format(current_nrf_sd_version, required_nrf_sd_version, provided_nrf_sd_version, current_nrf_bl_version, provided_nrf_bl_version)) + print('nRF51 has: {} and requires {} and upgrade provides {}. Current bootloader version is [{}] but upgrade ' + 'provides [{}]'.format( + current_nrf_sd_version, required_nrf_sd_version, provided_nrf_sd_version, + current_nrf_bl_version, provided_nrf_bl_version) + ) - if required_nrf_sd_version != None and \ - current_nrf_sd_version != required_nrf_sd_version and \ - provided_nrf_sd_version != required_nrf_sd_version: - raise Exception('Cannot flash nRF51: We have sd {}, need {} and have a zip with {}'.format(current_nrf_sd_version, required_nrf_sd_version, provided_nrf_sd_version)) + if required_nrf_sd_version is not None and \ + current_nrf_sd_version != required_nrf_sd_version and \ + provided_nrf_sd_version != required_nrf_sd_version: + raise Exception('Cannot flash nRF51: We have sd {}, need {} and have a zip with {}'.format( + current_nrf_sd_version, required_nrf_sd_version, provided_nrf_sd_version)) - # To avoid always flashing the bootloader and soft device (these might never change again) first check if we really need to. - # The original version of the bootloader is reported as None. + # To avoid always flashing the bootloader and soft device (these might never change again) first check + # if we really need to. The original version of the bootloader is reported as None. should_flash_nrf_sd = True if current_nrf_sd_version == required_nrf_sd_version and current_nrf_bl_version == provided_nrf_bl_version: should_flash_nrf_sd = False - #elif provided_nrf_sd_version == None: + # elif provided_nrf_sd_version == None: # should_flash_nrf_sd = False if should_flash_nrf_sd: - print("Should flash nRF soft device") + print('Should flash nRF soft device') rf51_sdbl_list = [x for x in flash_artifacts if x.target.type == 'bootloader+softdevice'] if len(rf51_sdbl_list) != 1: raise Exception('Only support for one and only one bootloader+softdevice in zip file') nrf51_sdbl = rf51_sdbl_list[0] - + nrf_info = self._cload.targets[TargetTypes.NRF51] page = nrf_info.flash_pages - (len(nrf51_sdbl.content) // nrf_info.page_size) self._internal_flash(artifact=nrf51_sdbl, page_override=page) @@ -260,20 +242,21 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self._cload.reset_to_bootloader(TargetTypes.NRF51) uri = self._cload.link.uri self._cload.close() - print("Closing bootloader link and reconnecting to new bootloader (" + uri + ")") + print('Closing bootloader link and reconnecting to new bootloader (' + uri + ')') self._cload = Cloader(uri, info_cb=None, in_boot_cb=None) self._cload.open_bootloader_uri(uri) - print("Reconnected to new bootloader") - started = self._cload.check_link_and_get_info() + print('Reconnected to new bootloader') + self._cload.check_link_and_get_info() self._cload.request_info_update(TargetTypes.NRF51) - + # Remove the softdevice+bootloader from the list of artifacts to flash - flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] # Also filter for nRF51 here? + flash_artifacts = [a for a in artifacts if a.target.type != + 'bootloader+softdevice'] # Also filter for nRF51 here? for artifact in artifacts: - print("Found artifact for target: {}".format(artifact.target)) + print('Found artifact for target: {}'.format(artifact.target)) # Flash the MCU flash if len(targets) == 0 or len(flash_targets) > 0: @@ -359,8 +342,8 @@ def _get_flash_artifacts_from_zip(self, filename): if manifest['version'] > 2: raise Exception('Wrong manifest version') - - print("Found manifest version: {}".format(manifest['version'])) + + print('Found manifest version: {}'.format(manifest['version'])) flash_artifacts = [] add_legacy_nRF51_s110 = False @@ -381,7 +364,7 @@ def _get_flash_artifacts_from_zip(self, filename): flash_artifacts.append(FlashArtifact(content, target, metadata['release'])) if add_legacy_nRF51_s110: - print("Legacy format detected for manifest, adding s110+bl binary from distro") + print('Legacy format detected for manifest, adding s110+bl binary from distro') from importlib.resources import files content = files('resources.binaries').joinpath('nrf51-s110-and-bl.bin').read_bytes() target = Target('cf2', 'nrf51', 'bootloader+softdevice', ['sd-s110'], []) @@ -532,7 +515,7 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ print('\nError during flash operation (code %d). Maybe' ' wrong radio link?' % self._cload.error_code) raise Exception() - + sys.stdout.write('\n') sys.stdout.flush() diff --git a/cflib/bootloader/boottypes.py b/cflib/bootloader/boottypes.py index 959c9d1e5..e3cc975f1 100644 --- a/cflib/bootloader/boottypes.py +++ b/cflib/bootloader/boottypes.py @@ -89,7 +89,7 @@ def __str__(self): TargetTypes.to_string(self.id), self.id) ret += 'Flash pages: {} | Page size: {} | Buffer pages: {} |' \ ' Start page: {} | Version: {} \n'.format(self.flash_pages, self.page_size, - self.buffer_pages, self.start_page, self.version) + self.buffer_pages, self.start_page, self.version) ret += '%d KBytes of flash available for firmware image.' % ( (self.flash_pages - self.start_page) * self.page_size / 1024) return ret diff --git a/setup.py b/setup.py index 2fdc1ed3f..a4b61bb1d 100644 --- a/setup.py +++ b/setup.py @@ -1,16 +1,19 @@ #!/usr/bin/env python3 +import os +from glob import glob from pathlib import Path from setuptools import find_packages from setuptools import setup -from glob import glob # read the contents of README.md file fo use in pypi description directory = Path(__file__).parent long_description = (directory / 'README.md').read_text() + def relative(lst, base=''): return list(map(lambda x: base + os.path.basename(x), lst)) + data_files = [ ('binaries', glob('resources/binaries/*')), ] From b891786567775ada58a475d664b2ea37a6643227 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 29 Jan 2024 13:52:24 +0100 Subject: [PATCH 605/861] Stay in bootloader if firmware if firmware is partially overwritten and no new firmware is flashed. --- cflib/bootloader/__init__.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 4fe52ed76..01d58e430 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -236,6 +236,11 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo nrf51_sdbl = rf51_sdbl_list[0] nrf_info = self._cload.targets[TargetTypes.NRF51] + # During bootloader update part of the firmware will be erased. If we're + # only flashing the bootloader and no firmware we should make sure the bootloader + # stays in bootloader mode and doesn't try to start the broken firmware, this is + # done by erasing the first page of the firmware. + self._internal_flash(FlashArtifact([0xFF] * nrf_info.page_size, nrf51_sdbl.target, None)) page = nrf_info.flash_pages - (len(nrf51_sdbl.content) // nrf_info.page_size) self._internal_flash(artifact=nrf51_sdbl, page_override=page) From b6973585fe407cc6bcce66cd0e8d7fae35e60be3 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 29 Jan 2024 14:25:15 +0100 Subject: [PATCH 606/861] Updated how version in bootloader info packet is encoded. --- cflib/bootloader/__init__.py | 2 -- cflib/bootloader/cloader.py | 11 +++++++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 01d58e430..4d616637a 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -201,8 +201,6 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo current_nrf_sd_version = self._get_current_nrf51_sd_version() required_nrf_sd_version = self._get_required_nrf51_sd_version(flash_artifacts) provided_nrf_sd_version = self._get_provided_nrf51_sd_version(flash_artifacts) - # TODO: Figure the versions out. It is an int we get but the JSON contains strings, decode from int - # representation to some string? current_nrf_bl_version = None if self._cload.targets[TargetTypes.NRF51].version is not None: current_nrf_bl_version = str(self._cload.targets[TargetTypes.NRF51].version) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 6becd1609..aeeddcb4f 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -201,8 +201,15 @@ def _update_info(self, target_id): if len(answer.data) > 22: self.targets[target_id].protocol_version = answer.datat[22] self.protocol_version = answer.datat[22] - if len(answer.data) > 23: - self.targets[target_id].version = answer.data[23] + if len(answer.data) > 23 and len(answer.data) > 26: + code_state = "" + if answer.data[23] & 0x8000 != 0: + code_state = "+" + answer.data[23] &= 0x7FFF + major = struct.unpack('H', answer.data[23:25])[0] + minor = answer.data[25] + patch = answer.data[26] + self.targets[target_id].version = "{}.{}.{}{}".format(major, minor, patch, code_state) self.targets[target_id].page_size = tab[2] self.targets[target_id].buffer_pages = tab[3] self.targets[target_id].flash_pages = tab[4] From 52ee2d7d7cbfb4b7107ffc2341a98637911b9ef2 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 29 Jan 2024 14:33:18 +0100 Subject: [PATCH 607/861] Fixed styling --- cflib/bootloader/cloader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index aeeddcb4f..98930d2be 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -202,14 +202,14 @@ def _update_info(self, target_id): self.targets[target_id].protocol_version = answer.datat[22] self.protocol_version = answer.datat[22] if len(answer.data) > 23 and len(answer.data) > 26: - code_state = "" + code_state = '' if answer.data[23] & 0x8000 != 0: - code_state = "+" + code_state = '+' answer.data[23] &= 0x7FFF major = struct.unpack('H', answer.data[23:25])[0] minor = answer.data[25] patch = answer.data[26] - self.targets[target_id].version = "{}.{}.{}{}".format(major, minor, patch, code_state) + self.targets[target_id].version = '{}.{}.{}{}'.format(major, minor, patch, code_state) self.targets[target_id].page_size = tab[2] self.targets[target_id].buffer_pages = tab[3] self.targets[target_id].flash_pages = tab[4] From 8c3a04098b20e5af2cd57b80fe4dafbca33b6d37 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 29 Jan 2024 16:12:17 +0100 Subject: [PATCH 608/861] Removed debug printouts for bootloading --- cflib/bootloader/__init__.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 4d616637a..179d97755 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -184,8 +184,6 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo # Fetch artifacts from source file artifacts = self._get_flash_artifacts_from_zip(filename) - for artifact in artifacts: - print('Found artifact for target: {}'.format(artifact.target)) if len(artifacts) == 0: if len(targets) == 1: content = open(filename, 'br').read() @@ -258,9 +256,6 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo flash_artifacts = [a for a in artifacts if a.target.type != 'bootloader+softdevice'] # Also filter for nRF51 here? - for artifact in artifacts: - print('Found artifact for target: {}'.format(artifact.target)) - # Flash the MCU flash if len(targets) == 0 or len(flash_targets) > 0: self._flash_flash(flash_artifacts, flash_targets) From 4ca21c8d960fb5986fff7b937018044407e3f5b0 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 29 Jan 2024 16:12:32 +0100 Subject: [PATCH 609/861] Fixed bootloader version decoding --- cflib/bootloader/cloader.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 98930d2be..2aa56421a 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -199,13 +199,13 @@ def _update_info(self, target_id): self.targets[target_id] = Target(target_id) self.targets[target_id].addr = target_id if len(answer.data) > 22: - self.targets[target_id].protocol_version = answer.datat[22] - self.protocol_version = answer.datat[22] + self.targets[target_id].protocol_version = answer.data[22] + self.protocol_version = answer.data[22] if len(answer.data) > 23 and len(answer.data) > 26: code_state = '' - if answer.data[23] & 0x8000 != 0: + if answer.data[24] & 0x80 != 0: code_state = '+' - answer.data[23] &= 0x7FFF + answer.data[24] &= 0x7F major = struct.unpack('H', answer.data[23:25])[0] minor = answer.data[25] patch = answer.data[26] From 711c7abaad98957514555376fec676ec963db9f6 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 29 Jan 2024 16:43:15 +0100 Subject: [PATCH 610/861] Report that we are flashing the bootloader (not just firmware) to the UI --- cflib/bootloader/__init__.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 179d97755..e1eb5c5f1 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -407,9 +407,13 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ factor = (100.0 * t_data.page_size) / len(image) progress = 0 + type_of_binary = 'Firmware' + if artifact.target.type == 'bootloader+softdevice': + type_of_binary = 'Bootloader+softdevice' + if self.progress_cb: self.progress_cb( - 'Firmware ({}/{}) Starting...'.format(current_file_number, total_files), + '{} ({}/{}) Starting...'.format(type_of_binary, current_file_number, total_files), int(progress)) else: sys.stdout.write( @@ -452,7 +456,8 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ if self.progress_cb: progress += factor - self.progress_cb('Firmware ({}/{}) Uploading buffer to {}...'.format( + self.progress_cb('{} ({}/{}) Uploading buffer to {}...'.format( + type_of_binary, current_file_number, total_files, TargetTypes.to_string(t_data.id)), @@ -465,7 +470,8 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ # Flash when the complete buffers are full if ctr >= t_data.buffer_pages: if self.progress_cb: - self.progress_cb('Firmware ({}/{}) Writing buffer to {}...'.format( + self.progress_cb('{} ({}/{}) Writing buffer to {}...'.format( + type_of_binary, current_file_number, total_files, TargetTypes.to_string(t_data.id)), @@ -492,7 +498,8 @@ def _internal_flash(self, artifact: FlashArtifact, current_file_number=1, total_ if ctr > 0: if self.progress_cb: - self.progress_cb('Firmware ({}/{}) Writing buffer to {}...'.format( + self.progress_cb('{} ({}/{}) Writing buffer to {}...'.format( + type_of_binary, current_file_number, total_files, TargetTypes.to_string(t_data.id)), From a6457f462192e9af839e2de42406c60200710fe6 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 30 Jan 2024 14:00:23 +0100 Subject: [PATCH 611/861] Fixed issue with trying to flash decks via bootloader --- cflib/bootloader/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index e1eb5c5f1..599bfc322 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -253,7 +253,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self._cload.request_info_update(TargetTypes.NRF51) # Remove the softdevice+bootloader from the list of artifacts to flash - flash_artifacts = [a for a in artifacts if a.target.type != + flash_artifacts = [a for a in flash_artifacts if a.target.type != 'bootloader+softdevice'] # Also filter for nRF51 here? # Flash the MCU flash From 2c4db007f0e34b11fd8f4a3c20cca38080d7c94f Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 31 Jan 2024 13:29:59 +0100 Subject: [PATCH 612/861] Fixed not being able to open s110 binary --- MANIFEST.in | 1 + cflib/bootloader/__init__.py | 4 ++-- .../resources}/binaries/nrf51-s110-and-bl.bin | Bin setup.py | 16 +++++----------- 4 files changed, 8 insertions(+), 13 deletions(-) create mode 100644 MANIFEST.in rename {resources => cflib/resources}/binaries/nrf51-s110-and-bl.bin (100%) diff --git a/MANIFEST.in b/MANIFEST.in new file mode 100644 index 000000000..e283ae52a --- /dev/null +++ b/MANIFEST.in @@ -0,0 +1 @@ +recursive-include cflib/resources/binaries *.bin diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 599bfc322..18370d9d5 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -363,8 +363,8 @@ def _get_flash_artifacts_from_zip(self, filename): if add_legacy_nRF51_s110: print('Legacy format detected for manifest, adding s110+bl binary from distro') - from importlib.resources import files - content = files('resources.binaries').joinpath('nrf51-s110-and-bl.bin').read_bytes() + from importlib.resources import read_binary + content = read_binary('cflib.resources.binaries', 'nrf51-s110-and-bl.bin') target = Target('cf2', 'nrf51', 'bootloader+softdevice', ['sd-s110'], []) release = None flash_artifacts.append(FlashArtifact(content, target, release)) diff --git a/resources/binaries/nrf51-s110-and-bl.bin b/cflib/resources/binaries/nrf51-s110-and-bl.bin similarity index 100% rename from resources/binaries/nrf51-s110-and-bl.bin rename to cflib/resources/binaries/nrf51-s110-and-bl.bin diff --git a/setup.py b/setup.py index a4b61bb1d..fb7476f47 100644 --- a/setup.py +++ b/setup.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -import os -from glob import glob from pathlib import Path from setuptools import find_packages @@ -9,14 +7,9 @@ directory = Path(__file__).parent long_description = (directory / 'README.md').read_text() - -def relative(lst, base=''): - return list(map(lambda x: base + os.path.basename(x), lst)) - - -data_files = [ - ('binaries', glob('resources/binaries/*')), -] +package_data = { + 'cflib.resources.binaries': ['cflib/resources/binaries/*.bin'], +} setup( name='cflib', @@ -57,5 +50,6 @@ def relative(lst, base=''): ], }, - data_files=data_files + include_package_data=True, + package_data=package_data ) From 9fbc3d0b8eff31ad40a93b52476d11056b49a8be Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 31 Jan 2024 14:50:51 +0100 Subject: [PATCH 613/861] Fix issue with new Target tuple and flashing decks --- cflib/bootloader/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 18370d9d5..ff01f1a3f 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -578,13 +578,13 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis continue # Check that we want to flash this deck - deck_target = [t for t in targets if t == Target('deck', deck.name, 'fw')] + deck_target = [t for t in targets if t == Target('deck', deck.name, 'fw', [], [])] if (not flash_all_targets) and len(deck_target) == 0: print(f'Skipping {deck.name}, not in the target list') continue # Check that we have an artifact for this deck - deck_artifacts = [a for a in artifacts if a.target == Target('deck', deck.name, 'fw')] + deck_artifacts = [a for a in artifacts if a.target == Target('deck', deck.name, 'fw', [], [])] if len(deck_artifacts) == 0: print(f'Skipping {deck.name}, no artifact for it in the .zip') continue From 307959228ba66a64b759e9e8f57f6cbea50b15d6 Mon Sep 17 00:00:00 2001 From: Marcus Date: Thu, 1 Feb 2024 13:42:30 +0100 Subject: [PATCH 614/861] Enable flashing nrf binaries alone --- cflib/bootloader/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index ff01f1a3f..850f1de98 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -221,8 +221,8 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo should_flash_nrf_sd = True if current_nrf_sd_version == required_nrf_sd_version and current_nrf_bl_version == provided_nrf_bl_version: should_flash_nrf_sd = False - # elif provided_nrf_sd_version == None: - # should_flash_nrf_sd = False + elif provided_nrf_sd_version is None: + should_flash_nrf_sd = False if should_flash_nrf_sd: print('Should flash nRF soft device') From e585bebdc21d1709e916de558b467d4f5bc44d51 Mon Sep 17 00:00:00 2001 From: Marcus Date: Thu, 1 Feb 2024 21:32:47 +0100 Subject: [PATCH 615/861] Fixed issue with bootloader where flashing just binaries of nrf51 or sd+bl did not work --- cflib/bootloader/__init__.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 850f1de98..14ba1bf5a 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -199,6 +199,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo current_nrf_sd_version = self._get_current_nrf51_sd_version() required_nrf_sd_version = self._get_required_nrf51_sd_version(flash_artifacts) provided_nrf_sd_version = self._get_provided_nrf51_sd_version(flash_artifacts) + update_contains_nrf_sd = any(x.target.type == 'bootloader+softdevice' for x in flash_artifacts) current_nrf_bl_version = None if self._cload.targets[TargetTypes.NRF51].version is not None: current_nrf_bl_version = str(self._cload.targets[TargetTypes.NRF51].version) @@ -221,7 +222,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo should_flash_nrf_sd = True if current_nrf_sd_version == required_nrf_sd_version and current_nrf_bl_version == provided_nrf_bl_version: should_flash_nrf_sd = False - elif provided_nrf_sd_version is None: + elif provided_nrf_sd_version is None and not update_contains_nrf_sd: should_flash_nrf_sd = False if should_flash_nrf_sd: From 7599d54da8a78f573ad2c2778d65a849c71c0652 Mon Sep 17 00:00:00 2001 From: Marcus Date: Wed, 7 Feb 2024 10:15:44 +0100 Subject: [PATCH 616/861] Updated downgrading s110 binary to support PA on CF21 --- .../resources/binaries/nrf51-s110-and-bl.bin | Bin 96256 -> 96256 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/cflib/resources/binaries/nrf51-s110-and-bl.bin b/cflib/resources/binaries/nrf51-s110-and-bl.bin index 0a682e7e0414ca8b12a8185c80d1380019f360ba..74542ecb0a1b00d477d3fad0e9b67e554142c01a 100644 GIT binary patch delta 1067 zcmYLHZA@EL7=GWjmtqSW&~~<^#rx5^mac&c>aYnT8Pv8kwrJeMxEYpZ$xYaPj0sVG&}6YdjB(3iBa%*y@y6)-!Pe{PSi`w_&imwf z&->i>oV=34ND8wb`hXo+)WSOy?=5P{D?VrXeadTVn(_}RW**|k>K?D2sp+-2a%C@H z(`wOsaW8k{A-62-Y1MOd%~>pS%Pzb;xBVDiCJYMr6!GH0#{YI$@B%95OB&^8GJ+F$ zQ~m+R5qWY%yMik+>Wqj%xsjPce1n{>o5ld)^g%R{5&feYeTlrOPhy_@r1!SITm-*v z-5Nc?d8lKe_{b#(4_qf`lqJ$y@5KvbpkBbNe7ycVVuCaq0@zEA8aVWmF+-Pz$&fRK zQ!Q(irX{tIQg`ypq3_+mG8wto*oVlHbi)Al%Bu~LI;O1*$`CP`xZrCg6ol016#6Xd zprr1Br#p+nXsXH?IO}Mi`o@8ZdB->Ps{?}`*qj9A_&q)cN`j1=4qEP(cQc!lnA3Rw z&3XWoC|NKG&YR`KaDw_Yso=4!SFhwRtAYo%CQ-Rg{xsR1{IX%{^erp=fwr!}4UM1a;Uly}m!zRO45f~3bBF-ql8=V7=_d$~il9 z2E*brkzj0C)b$LD=XifB;MO1^Zn4W?7yDRioC{a0t?}&#ibVH>=zO3_T5G&DW{*p*ub7v_yz4ihoZrA< zao^v(&b0xbi3w3Hd{iMhi@B|V0Z2X$?0AL__AK;;?;!M3Z8ocgZarx^5Nfi}pbM;5Qmf4X{wdmVq#l3a)GeisBi+d`lY69`7o4QxwoZnJ3I#<3v~oTQq$c$b*;1L*bbwublSxzCg5 zocGDOH%HMJMPqhB4{-i@BfLp5JZ~iL`&@}ps!Qi>s^6uEKgRbrcXY9*W5IEpEC z8HW+;Nw@JQTu{+eiRf3WnQ_E7$Yj|WY$Ck53oFP`^B%)?mYg)l@HV++_UwH*3oWLV z`p^kpqKT1g#kZ^gd>a@vmFz9|;FqMgT*T|@@$yTE5mIaMV>{Vz;ZY>7S@szoERyq< zPj~-VsJWqMM)b{evhU{x;2tch&sFvy){#V2FM8FRRlzdGmIv(sv0C}A<2e+?k-;(a z*;hat+5)LPh2r2yQ8O^bddIG?s983BV}G#NCBa%8v}0Ss9nd0V*xF?OC;zCj7RNgV z&wnWiKKGtUnzc)nAb%K?#5BOO72-5&s@uI#QAWV-A-I&^_JbK z!7+o+L#JphFi%#d@(Dv5KJBOXrt_N#dVimfruj*yEm(OnKj`X!+jN#Sa@OXuz7)_i z_4;3&VkojAe8dIG4Vy2~9X1Jadv}lnaTK*@^BkRyDJVjWUu6y+0&aZ&!RVCO5n5-C zbxz9Dg2Jytg>h1bPuly-1QV}kE^dQlO!-5ndC-2yJ;Dn);6O`fFN@8=me4vZ$<6&O zA;rBUPl+ge)!+-PGfvqXm`xxndn4ZPeHe-+`-+bg5n?56i8Tf}_hEUOUxWequKQ1L zMtj_>%!i#(h4RKjN1^Wl+4azyHL@}raLNj&Ft5sKE(uf%t9V%cS1@s_a84eO^~~pm zWS@xxj%BO><&D720@Q@f05PhMklMOEhAM_M*7a3iUZTrQJKc0oK2}fF>4;tARQ(aW zPkyWSKe3es2~vf0fc{;`5u0O#Zt91Q7b`3DT9Jd9Glk46z_5j1Z)F zkEYhp*dfZ}6n?eWIb+5K^>2Qr3_l}hr8zvOc6v@DzOG*L_A_6k-a^|4ZS!qdd!Xu< XR)FQ}SB+8E-1Y+h4*qT=TC3@QFN$Z7 From 29ac30b1c676b9d6452c0309340ab9a655868f8c Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Mon, 12 Feb 2024 11:02:47 +0100 Subject: [PATCH 617/861] update test release to pypi token --- .github/workflows/test-python-publish.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index dd15af6cb..ed2a17cea 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -34,6 +34,6 @@ jobs: - name: Publish package to TestPyPI uses: pypa/gh-action-pypi-publish@release/v1 with: - user: ${{ secrets.PYPI_USERNAME }} - password: ${{ secrets.PYPI_PASSWORD }} + username: __token__ + password: ${{ secrets.PYPI_TOKEN }} repository_url: https://test.pypi.org/legacy/ From cfa97d36216afe27d75158e4faa692d796712edd Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Mon, 12 Feb 2024 11:03:32 +0100 Subject: [PATCH 618/861] update release script to pypi token --- .github/workflows/python-publish.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml index 30be46c12..11f05a53d 100644 --- a/.github/workflows/python-publish.yml +++ b/.github/workflows/python-publish.yml @@ -34,5 +34,5 @@ jobs: - name: Publish package uses: pypa/gh-action-pypi-publish@release/v1 with: - user: ${{ secrets.PYPI_USERNAME }} - password: ${{ secrets.PYPI_PASSWORD }} + username: __token__ + password: ${{ secrets.PYPI_TOKEN }} From 0a312904dfd0ae8f48fa625d2fd9b44a5fc4f372 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Mon, 12 Feb 2024 11:26:17 +0100 Subject: [PATCH 619/861] Update test-python-publish.yml --- .github/workflows/test-python-publish.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index ed2a17cea..baabc27c1 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -35,5 +35,5 @@ jobs: uses: pypa/gh-action-pypi-publish@release/v1 with: username: __token__ - password: ${{ secrets.PYPI_TOKEN }} + password: ${{ secrets.PYPI_TEST_TOKEN }} repository_url: https://test.pypi.org/legacy/ From e40b53f371579cc6f692d3f4c8afb89574343e07 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 13 Feb 2024 13:18:00 +0100 Subject: [PATCH 620/861] Set version to 0.1.25 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index fb7476f47..d760a74e9 100644 --- a/setup.py +++ b/setup.py @@ -13,7 +13,7 @@ setup( name='cflib', - version='0.1.24', + version='0.1.25', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From fcd16675e3ab213372a3ab75a4e9531dc28ee6a4 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 13 Feb 2024 13:34:57 +0100 Subject: [PATCH 621/861] 0.1.25.1: Relax numpy dependency --- setup.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/setup.py b/setup.py index d760a74e9..a6373efe4 100644 --- a/setup.py +++ b/setup.py @@ -13,7 +13,7 @@ setup( name='cflib', - version='0.1.25', + version='0.1.25.1', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', @@ -40,7 +40,7 @@ 'pyusb>=1.0.0b2', 'libusb-package~=1.0', 'scipy~=1.7', - 'numpy>=1.20,<1.25', + 'numpy~=1.20', ], # $ pip install -e .[dev] From 342bc20803cf1529021ee2b624c4e3a88166ec22 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 20 Feb 2024 13:15:03 +0100 Subject: [PATCH 622/861] Added message for doing crash recovery request to the supervisor. --- cflib/crazyflie/platformservice.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py index 8b96c214a..262587a25 100644 --- a/cflib/crazyflie/platformservice.py +++ b/cflib/crazyflie/platformservice.py @@ -41,6 +41,7 @@ PLATFORM_SET_CONT_WAVE = 0 PLATFORM_REQUEST_ARMING = 1 +PLATFORM_REQUEST_CRASH_RECOVERY = 2 VERSION_GET_PROTOCOL = 0 VERSION_GET_FIRMWARE = 1 @@ -99,6 +100,16 @@ def send_arming_request(self, do_arm: bool): pk.data = (PLATFORM_REQUEST_ARMING, do_arm) self._cf.send_packet(pk) + def send_crash_recovery_request(self): + """ + Send crash recovery request + + """ + pk = CRTPPacket() + pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND) + pk.data = (PLATFORM_REQUEST_CRASH_RECOVERY, ) + self._cf.send_packet(pk) + def get_protocol_version(self): """ Return version of the CRTP protocol From fc162375ba262ddea9d404229aa75aabb97d2c4d Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 13 Mar 2024 17:03:14 +0100 Subject: [PATCH 623/861] Support for reading/writing persistent parameters from/to file --- cflib/localization/__init__.py | 4 +- cflib/localization/param_io.py | 85 +++++++++++++ test/localization/fixtures/parameters.yaml | 15 +++ test/localization/test_param_io.py | 140 +++++++++++++++++++++ 4 files changed, 243 insertions(+), 1 deletion(-) create mode 100644 cflib/localization/param_io.py create mode 100644 test/localization/fixtures/parameters.yaml create mode 100644 test/localization/test_param_io.py diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py index 7a9cc98d2..6f4252c3e 100644 --- a/cflib/localization/__init__.py +++ b/cflib/localization/__init__.py @@ -25,6 +25,7 @@ from .lighthouse_config_manager import LighthouseConfigWriter from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader +from .param_io import ParamFileManager __all__ = [ 'LighthouseBsGeoEstimator', @@ -32,4 +33,5 @@ 'LighthouseSweepAngleAverageReader', 'LighthouseSweepAngleReader', 'LighthouseConfigFileManager', - 'LighthouseConfigWriter'] + 'LighthouseConfigWriter', + 'ParamFileManager'] diff --git a/cflib/localization/param_io.py b/cflib/localization/param_io.py new file mode 100644 index 000000000..1b7676e6c --- /dev/null +++ b/cflib/localization/param_io.py @@ -0,0 +1,85 @@ +# -*- 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 yaml + +from cflib.crazyflie.param import Param, PersistentParamState + +class ParamFileManager(): + """Reads and writes parameter configurations from file""" + TYPE_ID = 'type' + TYPE = 'param_system_configuration' + VERSION_ID = 'version' + VERSION = '1' + PARAMS_ID = 'params' + + @staticmethod + def write(file_name, params={}): + file = open(file_name, 'w') + with file: + file_params = {} + for id, param in params.items(): + assert isinstance(param, PersistentParamState) + if isinstance(param, PersistentParamState): + if param.is_stored: + file_params[id] = {'is_stored': param.is_stored, 'default_value': param.default_value, 'stored_value': param.stored_value} + + data = { + ParamFileManager.TYPE_ID: ParamFileManager.TYPE, + ParamFileManager.VERSION_ID: ParamFileManager.VERSION, + ParamFileManager.PARAMS_ID: file_params + } + + yaml.dump(data, file) + + @staticmethod + def read(file_name): + file = open(file_name, 'r') + with file: + data = None + try: + data = yaml.safe_load(file) + except yaml.YAMLError as exc: + print(exc) + + if ParamFileManager.TYPE_ID not in data: + raise Exception('Type field missing') + + if data[ParamFileManager.TYPE_ID] != ParamFileManager.TYPE: + raise Exception('Unsupported file type') + + if ParamFileManager.VERSION_ID not in data: + raise Exception('Version field missing') + + if data[ParamFileManager.VERSION_ID] != ParamFileManager.VERSION: + raise Exception('Unsupported file version') + + def get_data(input_data): + persistent_params = {} + for id, param in input_data.items(): + persistent_params[id] = PersistentParamState(param['is_stored'], param['default_value'], param['stored_value']) + return persistent_params + + if ParamFileManager.PARAMS_ID in data: + return get_data(data[ParamFileManager.PARAMS_ID]) + else: + return {} diff --git a/test/localization/fixtures/parameters.yaml b/test/localization/fixtures/parameters.yaml new file mode 100644 index 000000000..e6bfc8a5f --- /dev/null +++ b/test/localization/fixtures/parameters.yaml @@ -0,0 +1,15 @@ +params: + activeMarker.back: + default_value: 3 + is_stored: true + stored_value: 10 + cppm.angPitch: + default_value: 50.0 + is_stored: true + stored_value: 55.0 + ctrlMel.i_range_z: + default_value: 0.4000000059604645 + is_stored: true + stored_value: 0.44999998807907104 +type: param_system_configuration +version: '1' diff --git a/test/localization/test_param_io.py b/test/localization/test_param_io.py new file mode 100644 index 000000000..f6865b496 --- /dev/null +++ b/test/localization/test_param_io.py @@ -0,0 +1,140 @@ +# -*- 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 unittest +from unittest.mock import ANY +from unittest.mock import mock_open +from unittest.mock import patch + +import yaml + +from cflib.localization import ParamFileManager + +class TestParamFileManager(unittest.TestCase): + def setUp(self): + self.data = { + 'type': 'param_system_configuration', + 'version': '1', + } + + @patch('yaml.safe_load') + def test_that_read_open_correct_file(self, mock_yaml_load): + # Fixture + mock_yaml_load.return_value = self.data + file_name = 'some/name.yaml' + + # Test + with patch('builtins.open', new_callable=mock_open()) as mock_file: + ParamFileManager.read(file_name) + + # Assert + mock_file.assert_called_with(file_name, 'r') + + @patch('yaml.safe_load') + def test_that_missing_file_type_raises(self, mock_yaml_load): + # Fixture + self.data.pop('type') + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + ParamFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_wrong_file_type_raises(self, mock_yaml_load): + # Fixture + self.data['type'] = 'wrong_type' + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + ParamFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_missing_version_raises(self, mock_yaml_load): + + # Fixture + self.data.pop('version') + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + ParamFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_wrong_version_raises(self, mock_yaml_load): + # Fixture + self.data['version'] = 'wrong_version' + mock_yaml_load.return_value = self.data + + # Test + # Assert + with self.assertRaises(Exception): + with patch('builtins.open', new_callable=mock_open()): + ParamFileManager.read('some/name.yaml') + + @patch('yaml.safe_load') + def test_that_no_data_returns_empty_default_data(self, mock_yaml_load): + # Fixture + mock_yaml_load.return_value = self.data + + # Test + with patch('builtins.open', new_callable=mock_open()): + actual_params = ParamFileManager.read('some/name.yaml') + + # Assert + self.assertEqual(0, len(actual_params)) + + @patch('yaml.dump') + def test_file_end_to_end_write_read(self, mock_yaml_dump): + # Fixture + fixture_file = 'test/localization/fixtures/parameters.yaml' + + file = open(fixture_file, 'r') + expected = yaml.safe_load(file) + file.close() + + # Test + params = ParamFileManager.read(fixture_file) + with patch('builtins.open', new_callable=mock_open()): + ParamFileManager.write('some/name.yaml', params=params) + + # Assert + mock_yaml_dump.assert_called_with(expected, ANY) + + @patch('yaml.dump') + def test_file_write_to_correct_file(self, mock_yaml_dump): + # Fixture + file_name = 'some/name.yaml' + + # Test + with patch('builtins.open', new_callable=mock_open()) as mock_file: + ParamFileManager.write(file_name) + + # Assert + mock_file.assert_called_with(file_name, 'w') From ed7fe9ae7581cbaeec06d5b8b4c0667a507ae687 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 14 Mar 2024 11:53:46 +0100 Subject: [PATCH 624/861] Improve stored param type identifier --- cflib/localization/param_io.py | 2 +- test/localization/fixtures/parameters.yaml | 2 +- test/localization/test_param_io.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/localization/param_io.py b/cflib/localization/param_io.py index 1b7676e6c..964a791ee 100644 --- a/cflib/localization/param_io.py +++ b/cflib/localization/param_io.py @@ -27,7 +27,7 @@ class ParamFileManager(): """Reads and writes parameter configurations from file""" TYPE_ID = 'type' - TYPE = 'param_system_configuration' + TYPE = 'persistent_param_state' VERSION_ID = 'version' VERSION = '1' PARAMS_ID = 'params' diff --git a/test/localization/fixtures/parameters.yaml b/test/localization/fixtures/parameters.yaml index e6bfc8a5f..b61abc437 100644 --- a/test/localization/fixtures/parameters.yaml +++ b/test/localization/fixtures/parameters.yaml @@ -11,5 +11,5 @@ params: default_value: 0.4000000059604645 is_stored: true stored_value: 0.44999998807907104 -type: param_system_configuration +type: persistent_param_state version: '1' diff --git a/test/localization/test_param_io.py b/test/localization/test_param_io.py index f6865b496..4a8f3826f 100644 --- a/test/localization/test_param_io.py +++ b/test/localization/test_param_io.py @@ -32,7 +32,7 @@ class TestParamFileManager(unittest.TestCase): def setUp(self): self.data = { - 'type': 'param_system_configuration', + 'type': 'persistent_param_state', 'version': '1', } From 35092d30348c647b23b5c2adda59b2254acad2c8 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 14 Mar 2024 11:54:19 +0100 Subject: [PATCH 625/861] Generalize ParamFileManager writing functionality Filtering for what to write to file should be done before calling the write method --- cflib/localization/param_io.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cflib/localization/param_io.py b/cflib/localization/param_io.py index 964a791ee..08682d916 100644 --- a/cflib/localization/param_io.py +++ b/cflib/localization/param_io.py @@ -40,8 +40,7 @@ def write(file_name, params={}): for id, param in params.items(): assert isinstance(param, PersistentParamState) if isinstance(param, PersistentParamState): - if param.is_stored: - file_params[id] = {'is_stored': param.is_stored, 'default_value': param.default_value, 'stored_value': param.stored_value} + file_params[id] = {'is_stored': param.is_stored, 'default_value': param.default_value, 'stored_value': param.stored_value} data = { ParamFileManager.TYPE_ID: ParamFileManager.TYPE, From 0e963da76ad3a7c593ec559f1cf1587363005bd8 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 14 Mar 2024 15:24:55 +0100 Subject: [PATCH 626/861] Remove unused Parameter import --- cflib/localization/param_io.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/param_io.py b/cflib/localization/param_io.py index 08682d916..7c8616755 100644 --- a/cflib/localization/param_io.py +++ b/cflib/localization/param_io.py @@ -22,7 +22,7 @@ import yaml -from cflib.crazyflie.param import Param, PersistentParamState +from cflib.crazyflie.param import PersistentParamState class ParamFileManager(): """Reads and writes parameter configurations from file""" From 23165d27e7f57b1c2c83459de3b84006438284fc Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 14 Mar 2024 15:33:22 +0100 Subject: [PATCH 627/861] Warn when trying to set non-existing params --- cflib/crazyflie/param.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 77d474a39..1dc199e8f 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -445,6 +445,9 @@ def persistent_store(self, complete_name, callback=None): @param callback Optional callback should take `complete_name` and boolean status as arguments """ element = self.toc.get_element_by_complete_name(complete_name) + if not element: + callback(complete_name, False) + return if not element.is_persistent(): raise AttributeError(f"Param '{complete_name}' is not persistent") From d12405270533e0bd49669f31c8488bf9b5a8af33 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 14 Mar 2024 15:35:12 +0100 Subject: [PATCH 628/861] Formatting --- cflib/localization/param_io.py | 14 ++++++++------ test/localization/test_param_io.py | 12 ++++++------ 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/cflib/localization/param_io.py b/cflib/localization/param_io.py index 7c8616755..7d337f46f 100644 --- a/cflib/localization/param_io.py +++ b/cflib/localization/param_io.py @@ -19,11 +19,11 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . - import yaml from cflib.crazyflie.param import PersistentParamState + class ParamFileManager(): """Reads and writes parameter configurations from file""" TYPE_ID = 'type' @@ -40,7 +40,8 @@ def write(file_name, params={}): for id, param in params.items(): assert isinstance(param, PersistentParamState) if isinstance(param, PersistentParamState): - file_params[id] = {'is_stored': param.is_stored, 'default_value': param.default_value, 'stored_value': param.stored_value} + file_params[id] = {'is_stored': param.is_stored, + 'default_value': param.default_value, 'stored_value': param.stored_value} data = { ParamFileManager.TYPE_ID: ParamFileManager.TYPE, @@ -49,7 +50,7 @@ def write(file_name, params={}): } yaml.dump(data, file) - + @staticmethod def read(file_name): file = open(file_name, 'r') @@ -71,13 +72,14 @@ def read(file_name): if data[ParamFileManager.VERSION_ID] != ParamFileManager.VERSION: raise Exception('Unsupported file version') - + def get_data(input_data): persistent_params = {} for id, param in input_data.items(): - persistent_params[id] = PersistentParamState(param['is_stored'], param['default_value'], param['stored_value']) + persistent_params[id] = PersistentParamState( + param['is_stored'], param['default_value'], param['stored_value']) return persistent_params - + if ParamFileManager.PARAMS_ID in data: return get_data(data[ParamFileManager.PARAMS_ID]) else: diff --git a/test/localization/test_param_io.py b/test/localization/test_param_io.py index 4a8f3826f..1154e8a85 100644 --- a/test/localization/test_param_io.py +++ b/test/localization/test_param_io.py @@ -19,7 +19,6 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . - import unittest from unittest.mock import ANY from unittest.mock import mock_open @@ -29,6 +28,7 @@ from cflib.localization import ParamFileManager + class TestParamFileManager(unittest.TestCase): def setUp(self): self.data = { @@ -48,7 +48,7 @@ def test_that_read_open_correct_file(self, mock_yaml_load): # Assert mock_file.assert_called_with(file_name, 'r') - + @patch('yaml.safe_load') def test_that_missing_file_type_raises(self, mock_yaml_load): # Fixture @@ -85,7 +85,7 @@ def test_that_missing_version_raises(self, mock_yaml_load): with self.assertRaises(Exception): with patch('builtins.open', new_callable=mock_open()): ParamFileManager.read('some/name.yaml') - + @patch('yaml.safe_load') def test_that_wrong_version_raises(self, mock_yaml_load): # Fixture @@ -106,10 +106,10 @@ def test_that_no_data_returns_empty_default_data(self, mock_yaml_load): # Test with patch('builtins.open', new_callable=mock_open()): actual_params = ParamFileManager.read('some/name.yaml') - + # Assert self.assertEqual(0, len(actual_params)) - + @patch('yaml.dump') def test_file_end_to_end_write_read(self, mock_yaml_dump): # Fixture @@ -126,7 +126,7 @@ def test_file_end_to_end_write_read(self, mock_yaml_dump): # Assert mock_yaml_dump.assert_called_with(expected, ANY) - + @patch('yaml.dump') def test_file_write_to_correct_file(self, mock_yaml_dump): # Fixture From 6d410bb8ba40fa28813ac18d84af57c649256816 Mon Sep 17 00:00:00 2001 From: dolfje Date: Sat, 30 Mar 2024 12:56:43 +0100 Subject: [PATCH 629/861] read_flash is unreliable When I tried to make a function that verified flash, it worked only 50% to have a correct page readout. Because the code only checks if the response is from the correct add, but not the correct part, it confuses the packets and add garbage in between. change sponsored by stagebees.com --- cflib/bootloader/cloader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 2aa56421a..53419da54 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -281,7 +281,7 @@ def read_flash(self, addr=0xFF, page=0x00): pk = None retry_counter = 5 while ((not pk or pk.header != 0xFF or - struct.unpack('= 0): pk = CRTPPacket() pk.set_header(0xFF, 0xFF) From c2408ef22b9e6b2231346dd288792bb35319e3a8 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Fri, 19 Apr 2024 09:42:48 +0200 Subject: [PATCH 630/861] Update uart_communication.md --- docs/development/uart_communication.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/development/uart_communication.md b/docs/development/uart_communication.md index ec56c7fe2..35068c423 100644 --- a/docs/development/uart_communication.md +++ b/docs/development/uart_communication.md @@ -5,6 +5,8 @@ page_id: uart_communication This page describes how to control your Crazyflie via UART, e.g. with a direct connection to a Raspberry Pi or with your computer through an FTDI cable. +> Currently there is an issue with the [new Raspberry pi 5](https://www.raspberrypi.com/documentation/computers/configuration.html#raspberry-pi-5) and these instructions. Please check the status of [this ticket](https://github.com/bitcraze/crazyflie-firmware/issues/1355). + ## Physical Connection To control the Crazyflie via UART first establish a physical connection between the Crazyflie and the controlling device. On the Crazyflie use the pins for UART2 which are on the right expansion connector TX2 (pin 1) and RX2 (pin 2). From 50a4abc573b337b3df22183948d2253f9e4f3fbf Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 24 Apr 2024 15:23:09 +0200 Subject: [PATCH 631/861] mention rate mode setpoint set in doc --- cflib/crazyflie/commander.py | 9 +++++---- docs/user-guides/python_api.md | 5 +++++ 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 1f0501436..96f52d454 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -71,11 +71,12 @@ def send_setpoint(self, roll, pitch, yawrate, thrust): """ Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the copter. - The meaning of these values is depended on the mode of the RPYT commander in the firmware - Default settings are Roll, pitch, yawrate and thrust + The meaning of these values is depended on the mode of the RPYT commander in the firmware. + The roll, pitch and yaw can be set in a rate or absolute mode with paramter flightmode.stabMode*. + Default settings are roll, pitch, yawrate and thrust - roll, pitch are in degrees - yawrate is in degrees/s + roll, pitch are in degrees, + yawrate is in degrees/s, thrust is an integer value ranging from 10001 (next to no power) to 60000 (full power) """ if thrust > 0xFFFF or thrust < 0: diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 7063192d9..0e844395c 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -246,12 +246,17 @@ setpoint with the same thrust but with roll/pitch/yawrate = 0, this will make the Crazyflie stop accelerating. After 2 seconds without new setpoint the Crazyflie will cut power from the motors. + Note that this command implements a motor lock mechanism that is intended to avoid flyaway when connecting a gamepad. You must send one command with thrust = 0 in order to unlock the command. This unlock procedure needs to be repeated if the watchdog described above kicks-in. +This the current default behavior, but it is depended on the [Supervisor configuration in the Crazyflie firmware](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/supervisor/). + +> Note: If you'd like to send rate commands instead, make sure that the [flightmode.stabModeRoll/Pitch/Yaw](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/params/#flightmode) is set to rate (0). + ### Other commander setpoints sending If your Crazyflie has a positioning system (Loco, flowdeck, MoCap, Lighthouse), you can also send velocity or position setpoints, like for instance: From aeb1370f6b5e2d621326afaf8eb862076f04e923 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 24 Apr 2024 14:38:11 +0200 Subject: [PATCH 632/861] PyQt5->6: Use fully qualified enum for QtCore --- examples/aideck/fpv.py | 57 ++++++++++--------- .../multiranger/multiranger_pointcloud.py | 42 +++++++------- 2 files changed, 50 insertions(+), 49 deletions(-) diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 8fa971bb6..6f5177692 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -113,39 +113,40 @@ def __init__(self, URI): self.mainLayout.addWidget(self.image_frame) self.gridLayout = QtWidgets.QGridLayout() - self.gridLayout.addWidget(QtWidgets.QLabel('Position (X/Y/Z)'), 0, 0, 1, 1, QtCore.Qt.AlignLeft) - self.gridLayout.addWidget(QtWidgets.QLabel('Pose (roll/pitch/yaw)'), 1, 0, 1, 1, QtCore.Qt.AlignLeft) + self.gridLayout.addWidget(QtWidgets.QLabel('Position (X/Y/Z)'), 0, 0, 1, 1, QtCore.Qt.AlignmentFlag.AlignLeft) + self.gridLayout.addWidget(QtWidgets.QLabel('Pose (roll/pitch/yaw)'), 1, + 0, 1, 1, QtCore.Qt.AlignmentFlag.AlignLeft) self.labels = { 'stateEstimate.x': { 'widget': QtWidgets.QLabel('X'), 'x_grid': 0, 'y_grid': 1, - 'alignment': QtCore.Qt.AlignLeft + 'alignment': QtCore.Qt.AlignmentFlag.AlignLeft }, 'stateEstimate.y': { 'widget': QtWidgets.QLabel('Y'), 'x_grid': 0, 'y_grid': 2, - 'alignment': QtCore.Qt.AlignLeft + 'alignment': QtCore.Qt.AlignmentFlag.AlignLeft }, 'stateEstimate.z': { 'widget': QtWidgets.QLabel('Z'), 'x_grid': 0, 'y_grid': 3, - 'alignment': QtCore.Qt.AlignLeft + 'alignment': QtCore.Qt.AlignmentFlag.AlignLeft }, 'stabilizer.roll': { 'widget': QtWidgets.QLabel('roll'), 'x_grid': 1, 'y_grid': 1, - 'alignment': QtCore.Qt.AlignLeft + 'alignment': QtCore.Qt.AlignmentFlag.AlignLeft }, 'stabilizer.pitch': { 'widget': QtWidgets.QLabel('pitch'), 'x_grid': 1, 'y_grid': 2, - 'alignment': QtCore.Qt.AlignLeft + 'alignment': QtCore.Qt.AlignmentFlag.AlignLeft }, 'stabilizer.yaw': { 'widget': QtWidgets.QLabel('yaw'), 'x_grid': 1, 'y_grid': 3, - 'alignment': QtCore.Qt.AlignLeft + 'alignment': QtCore.Qt.AlignmentFlag.AlignLeft } } @@ -191,48 +192,48 @@ def updateImage(self, image): def keyPressEvent(self, event): if (not event.isAutoRepeat()): - if (event.key() == QtCore.Qt.Key_Left): + if (event.key() == QtCore.Qt.Key.Key_Left): self.updateHover('y', 1) - if (event.key() == QtCore.Qt.Key_Right): + if (event.key() == QtCore.Qt.Key.Key_Right): self.updateHover('y', -1) - if (event.key() == QtCore.Qt.Key_Up): + if (event.key() == QtCore.Qt.Key.Key_Up): self.updateHover('x', 1) - if (event.key() == QtCore.Qt.Key_Down): + if (event.key() == QtCore.Qt.Key.Key_Down): self.updateHover('x', -1) - if (event.key() == QtCore.Qt.Key_A): + if (event.key() == QtCore.Qt.Key.Key_A): self.updateHover('yaw', -70) - if (event.key() == QtCore.Qt.Key_D): + if (event.key() == QtCore.Qt.Key.Key_D): self.updateHover('yaw', 70) - if (event.key() == QtCore.Qt.Key_Z): + if (event.key() == QtCore.Qt.Key.Key_Z): self.updateHover('yaw', -200) - if (event.key() == QtCore.Qt.Key_X): + if (event.key() == QtCore.Qt.Key.Key_X): self.updateHover('yaw', 200) - if (event.key() == QtCore.Qt.Key_W): + if (event.key() == QtCore.Qt.Key.Key_W): self.updateHover('height', 0.1) - if (event.key() == QtCore.Qt.Key_S): + if (event.key() == QtCore.Qt.Key.Key_S): self.updateHover('height', -0.1) def keyReleaseEvent(self, event): if (not event.isAutoRepeat()): - if (event.key() == QtCore.Qt.Key_Left): + if (event.key() == QtCore.Qt.Key.Key_Left): self.updateHover('y', 0) - if (event.key() == QtCore.Qt.Key_Right): + if (event.key() == QtCore.Qt.Key.Key_Right): self.updateHover('y', 0) - if (event.key() == QtCore.Qt.Key_Up): + if (event.key() == QtCore.Qt.Key.Key_Up): self.updateHover('x', 0) - if (event.key() == QtCore.Qt.Key_Down): + if (event.key() == QtCore.Qt.Key.Key_Down): self.updateHover('x', 0) - if (event.key() == QtCore.Qt.Key_A): + if (event.key() == QtCore.Qt.Key.Key_A): self.updateHover('yaw', 0) - if (event.key() == QtCore.Qt.Key_D): + if (event.key() == QtCore.Qt.Key.Key_D): self.updateHover('yaw', 0) - if (event.key() == QtCore.Qt.Key_W): + if (event.key() == QtCore.Qt.Key.Key_W): self.updateHover('height', 0) - if (event.key() == QtCore.Qt.Key_S): + if (event.key() == QtCore.Qt.Key.Key_S): self.updateHover('height', 0) - if (event.key() == QtCore.Qt.Key_Z): + if (event.key() == QtCore.Qt.Key.Key_Z): self.updateHover('yaw', 0) - if (event.key() == QtCore.Qt.Key_X): + if (event.key() == QtCore.Qt.Key.Key_X): self.updateHover('yaw', 0) def sendHoverCommand(self): diff --git a/examples/multiranger/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py index 1555b7a7e..6041ff966 100644 --- a/examples/multiranger/multiranger_pointcloud.py +++ b/examples/multiranger/multiranger_pointcloud.py @@ -68,7 +68,7 @@ except ImportError: pass -from PyQt5 import QtCore, QtWidgets +from PyQt6 import QtCore, QtWidgets logging.basicConfig(level=logging.INFO) @@ -230,48 +230,48 @@ def __init__(self, keyupdateCB): def on_key_press(self, event): if (not event.native.isAutoRepeat()): - if (event.native.key() == QtCore.Qt.Key_Left): + if (event.native.key() == QtCore.Qt.Key.Key_Left): self.keyCB('y', 1) - if (event.native.key() == QtCore.Qt.Key_Right): + if (event.native.key() == QtCore.Qt.Key.Key_Right): self.keyCB('y', -1) - if (event.native.key() == QtCore.Qt.Key_Up): + if (event.native.key() == QtCore.Qt.Key.Key_Up): self.keyCB('x', 1) - if (event.native.key() == QtCore.Qt.Key_Down): + if (event.native.key() == QtCore.Qt.Key.Key_Down): self.keyCB('x', -1) - if (event.native.key() == QtCore.Qt.Key_A): + if (event.native.key() == QtCore.Qt.Key.Key_A): self.keyCB('yaw', -70) - if (event.native.key() == QtCore.Qt.Key_D): + if (event.native.key() == QtCore.Qt.Key.Key_D): self.keyCB('yaw', 70) - if (event.native.key() == QtCore.Qt.Key_Z): + if (event.native.key() == QtCore.Qt.Key.Key_Z): self.keyCB('yaw', -200) - if (event.native.key() == QtCore.Qt.Key_X): + if (event.native.key() == QtCore.Qt.Key.Key_X): self.keyCB('yaw', 200) - if (event.native.key() == QtCore.Qt.Key_W): + if (event.native.key() == QtCore.Qt.Key.Key_W): self.keyCB('height', 0.1) - if (event.native.key() == QtCore.Qt.Key_S): + 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_Left): + if (event.native.key() == QtCore.Qt.Key.Key_Left): self.keyCB('y', 0) - if (event.native.key() == QtCore.Qt.Key_Right): + if (event.native.key() == QtCore.Qt.Key.Key_Right): self.keyCB('y', 0) - if (event.native.key() == QtCore.Qt.Key_Up): + if (event.native.key() == QtCore.Qt.Key.Key_Up): self.keyCB('x', 0) - if (event.native.key() == QtCore.Qt.Key_Down): + if (event.native.key() == QtCore.Qt.Key.Key_Down): self.keyCB('x', 0) - if (event.native.key() == QtCore.Qt.Key_A): + if (event.native.key() == QtCore.Qt.Key.Key_A): self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key_D): + if (event.native.key() == QtCore.Qt.Key.Key_D): self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key_W): + if (event.native.key() == QtCore.Qt.Key.Key_W): self.keyCB('height', 0) - if (event.native.key() == QtCore.Qt.Key_S): + if (event.native.key() == QtCore.Qt.Key.Key_S): self.keyCB('height', 0) - if (event.native.key() == QtCore.Qt.Key_Z): + if (event.native.key() == QtCore.Qt.Key.Key_Z): self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key_X): + if (event.native.key() == QtCore.Qt.Key.Key_X): self.keyCB('yaw', 0) def set_position(self, pos): From 207c5a12f25fa00e52d9f6c6f2b8c74e4df7886f Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 24 Apr 2024 14:49:33 +0200 Subject: [PATCH 633/861] PyQt5->6: Update Exec function name --- examples/aideck/fpv.py | 2 +- examples/multiranger/multiranger_pointcloud.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 6f5177692..5bf0e51f1 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -286,4 +286,4 @@ def closeEvent(self, event): appQt = QtWidgets.QApplication(sys.argv) win = MainWindow(URI) win.show() - appQt.exec_() + appQt.exec() diff --git a/examples/multiranger/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py index 6041ff966..3c42a41f9 100644 --- a/examples/multiranger/multiranger_pointcloud.py +++ b/examples/multiranger/multiranger_pointcloud.py @@ -361,4 +361,4 @@ def set_measurement(self, measurements): appQt = QtWidgets.QApplication(sys.argv) win = MainWindow(URI) win.show() - appQt.exec_() + appQt.exec() From 9a19ceb96da490d4efe039b782ec1b93acff5e29 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 7 May 2024 13:30:58 +0200 Subject: [PATCH 634/861] Update docs/user-guides/python_api.md --- docs/user-guides/python_api.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index 0e844395c..a81b2da7f 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -253,7 +253,7 @@ one command with thrust = 0 in order to unlock the command. This unlock procedure needs to be repeated if the watchdog described above kicks-in. -This the current default behavior, but it is depended on the [Supervisor configuration in the Crazyflie firmware](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/supervisor/). +This the current default behavior, but it depends on the [Supervisor configuration in the Crazyflie firmware](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/supervisor/). > Note: If you'd like to send rate commands instead, make sure that the [flightmode.stabModeRoll/Pitch/Yaw](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/params/#flightmode) is set to rate (0). From 06de96ef63a4065dadf9ca9210629a40541fcb1b Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 7 May 2024 13:34:04 +0200 Subject: [PATCH 635/861] Update cflib/crazyflie/commander.py --- cflib/crazyflie/commander.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 96f52d454..6916c1f5e 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -72,7 +72,7 @@ def send_setpoint(self, roll, pitch, yawrate, thrust): Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the copter. The meaning of these values is depended on the mode of the RPYT commander in the firmware. - The roll, pitch and yaw can be set in a rate or absolute mode with paramter flightmode.stabMode*. + The roll, pitch and yaw can be set in a rate or absolute mode with parameter group `flightmode` with variables `stabModeRoll`, `.stabModeRoll` and `.stabModeRoll`. Default settings are roll, pitch, yawrate and thrust roll, pitch are in degrees, From 2d3422ec59d7ad43ddf0f01b9095a0646999a918 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Tue, 7 May 2024 13:40:54 +0200 Subject: [PATCH 636/861] Update cflib/crazyflie/commander.py --- cflib/crazyflie/commander.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 6916c1f5e..4f78da782 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -72,7 +72,8 @@ def send_setpoint(self, roll, pitch, yawrate, thrust): Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the copter. The meaning of these values is depended on the mode of the RPYT commander in the firmware. - The roll, pitch and yaw can be set in a rate or absolute mode with parameter group `flightmode` with variables `stabModeRoll`, `.stabModeRoll` and `.stabModeRoll`. + The roll, pitch and yaw can be set in a rate or absolute mode with parameter group + `flightmode` with variables `stabModeRoll`, `.stabModeRoll` and `.stabModeRoll`. Default settings are roll, pitch, yawrate and thrust roll, pitch are in degrees, From b875fca4a94cbd53367f7b24b2115343ad2c646e Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Tue, 7 May 2024 13:23:17 +0200 Subject: [PATCH 637/861] pcap logging; Timestamps wrapping after 59 seconds. The datetime object does not hold total seconds since Epoch time.The seconds variable is only seconds since last full minute. We need a deltatime since Epoch start to get total seconds since then --- cflib/crtp/pcap.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crtp/pcap.py b/cflib/crtp/pcap.py index bef44755f..d00e9b02c 100644 --- a/cflib/crtp/pcap.py +++ b/cflib/crtp/pcap.py @@ -1,6 +1,6 @@ import os import struct -from datetime import datetime +import time from enum import IntEnum @@ -139,6 +139,6 @@ def _assemble_record(self, link_type, receive, address, channel, devid, crtp_pac ) def _pcap_header(self, len): - ts = datetime.now() - - return struct.pack(' Date: Tue, 7 May 2024 13:23:17 +0200 Subject: [PATCH 638/861] pcap logging; Timestamps wrapping after 59 seconds. The datetime object does not hold total seconds since Epoch time.The seconds variable is only seconds since last full minute. We need a deltatime since Epoch start to get total seconds since then --- cflib/crtp/pcap.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crtp/pcap.py b/cflib/crtp/pcap.py index bef44755f..d00e9b02c 100644 --- a/cflib/crtp/pcap.py +++ b/cflib/crtp/pcap.py @@ -1,6 +1,6 @@ import os import struct -from datetime import datetime +import time from enum import IntEnum @@ -139,6 +139,6 @@ def _assemble_record(self, link_type, receive, address, channel, devid, crtp_pac ) def _pcap_header(self, len): - ts = datetime.now() - - return struct.pack(' Date: Tue, 4 Jun 2024 14:19:57 +0200 Subject: [PATCH 639/861] Add paramfilehelper.py for writing a full file of params to a device All the params will be written to eeprom (persistent values) --- cflib/utils/param_file_helper.py | 38 ++++++++++++++ test/utils/fixtures/single_param.yaml | 7 +++ test/utils/test_param_file_helper.py | 71 +++++++++++++++++++++++++++ 3 files changed, 116 insertions(+) create mode 100644 cflib/utils/param_file_helper.py create mode 100644 test/utils/fixtures/single_param.yaml create mode 100644 test/utils/test_param_file_helper.py diff --git a/cflib/utils/param_file_helper.py b/cflib/utils/param_file_helper.py new file mode 100644 index 000000000..83c01f524 --- /dev/null +++ b/cflib/utils/param_file_helper.py @@ -0,0 +1,38 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# 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; 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 . +from cflib.crazyflie import Crazyflie +from cflib.localization.param_io import ParamFileManager + + +class ParamFileHelper: + def __init__(self, crazyflie): + if isinstance(crazyflie, Crazyflie): + self._cf = crazyflie + else: + raise TypeError("ParamFileHelper only takes a Crazyflie Object") + + def store_params_from_file(self, filename): + print(self._cf.param) + params = ParamFileManager().read(filename) + for param, state in params.items(): + self._cf.param.set_value(param, state.stored_value) + self._cf.param.persistent_store(param) diff --git a/test/utils/fixtures/single_param.yaml b/test/utils/fixtures/single_param.yaml new file mode 100644 index 000000000..4c16c331b --- /dev/null +++ b/test/utils/fixtures/single_param.yaml @@ -0,0 +1,7 @@ +params: + activeMarker.back: + default_value: 3 + is_stored: true + stored_value: 10 +type: persistent_param_state +version: '1' diff --git a/test/utils/test_param_file_helper.py b/test/utils/test_param_file_helper.py new file mode 100644 index 000000000..549a13cf9 --- /dev/null +++ b/test/utils/test_param_file_helper.py @@ -0,0 +1,71 @@ +# -*- 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 . +import unittest +from unittest.mock import MagicMock +from unittest.mock import patch +from cflib.utils.param_file_helper import ParamFileHelper +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + +class ParamFileHelperTests(unittest.TestCase): + + def setUp(self): + self.cf_mock = MagicMock(spec=Crazyflie) + self.helper = ParamFileHelper(self.cf_mock) + + def test_ParamFileHelper_SyncCrazyflieAsParam_ThrowsException(self): + cf_mock = MagicMock(spec=SyncCrazyflie) + helper = None + try: + helper = ParamFileHelper(cf_mock) + except Exception as e: + self.assertIsNone(helper) + else: + self.fail('Expect exception') + + + def test_ParamFileHelper_Crazyflie_Object(self): + helper = ParamFileHelper(self.cf_mock) + self.assertIsNotNone(helper) + + @patch('cflib.crazyflie.Param') + def test_ParamFileHelper_writesAndStoresParamFromFileToCrazyflie(self, mock_Param): + cf_mock = MagicMock(spec=Crazyflie) + cf_mock.param = mock_Param + helper = ParamFileHelper(cf_mock) + helper.store_params_from_file('test/utils/fixtures/single_param.yaml') + mock_Param.set_value.assert_called_once_with('activeMarker.back',10) + mock_Param.persistent_store.assert_called_once_with('activeMarker.back') + + + @patch('cflib.crazyflie.Param') + def test_ParamFileHelper_writesAndStoresAllParamsFromFileToCrazyflie(self, mock_Param): + cf_mock = MagicMock(spec=Crazyflie) + cf_mock.param = mock_Param + helper = ParamFileHelper(cf_mock) + helper.store_params_from_file('test/utils/fixtures/five_params.yaml') + self.assertEquals(5,len(mock_Param.set_value.mock_calls)) + self.assertEquals(5,len(mock_Param.persistent_store.mock_calls)) + + + + \ No newline at end of file From 3db0bcddf75b3a4382dcec941f8c7f2df7447097 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 5 Jun 2024 10:40:43 +0200 Subject: [PATCH 640/861] Add synchronicity to writing persistant params. After each param, write it persistent and wait for the result --- cflib/utils/param_file_helper.py | 23 +++++++++-- test/utils/fixtures/five_params.yaml | 23 +++++++++++ test/utils/test_param_file_helper.py | 60 +++++++++++++++++++++++++--- 3 files changed, 97 insertions(+), 9 deletions(-) create mode 100644 test/utils/fixtures/five_params.yaml diff --git a/cflib/utils/param_file_helper.py b/cflib/utils/param_file_helper.py index 83c01f524..2dce8899e 100644 --- a/cflib/utils/param_file_helper.py +++ b/cflib/utils/param_file_helper.py @@ -21,18 +21,35 @@ # along with this program. If not, see . from cflib.crazyflie import Crazyflie from cflib.localization.param_io import ParamFileManager +from threading import Event class ParamFileHelper: def __init__(self, crazyflie): if isinstance(crazyflie, Crazyflie): self._cf = crazyflie + self.persistent_sema = None + self.success = False else: raise TypeError("ParamFileHelper only takes a Crazyflie Object") - + + + def _persistent_stored_callback(self, complete_name, success): + self.success = success + if not success: + print(f'Persistent params: failed to store {complete_name}!') + else: + print(f'Persistent params: stored {complete_name}!') + self.persistent_sema.set() + + def store_params_from_file(self, filename): - print(self._cf.param) params = ParamFileManager().read(filename) for param, state in params.items(): + self.persistent_sema = Event() self._cf.param.set_value(param, state.stored_value) - self._cf.param.persistent_store(param) + self._cf.param.persistent_store(param, self._persistent_stored_callback) + self.persistent_sema.wait() + if not self.success: + break + return self.success diff --git a/test/utils/fixtures/five_params.yaml b/test/utils/fixtures/five_params.yaml new file mode 100644 index 000000000..4f75c8e40 --- /dev/null +++ b/test/utils/fixtures/five_params.yaml @@ -0,0 +1,23 @@ +params: + activeMarker.back: + default_value: 3 + is_stored: true + stored_value: 10 + activeMarker.front: + default_value: 3 + is_stored: true + stored_value: 10 + activeMarker.left: + default_value: 3 + is_stored: true + stored_value: 10 + cppm.angPitch: + default_value: 50.0 + is_stored: true + stored_value: 55.0 + ctrlMel.i_range_z: + default_value: 0.4000000059604645 + is_stored: true + stored_value: 0.44999998807907104 +type: persistent_param_state +version: '1' diff --git a/test/utils/test_param_file_helper.py b/test/utils/test_param_file_helper.py index 549a13cf9..32f0bab56 100644 --- a/test/utils/test_param_file_helper.py +++ b/test/utils/test_param_file_helper.py @@ -25,6 +25,7 @@ from cflib.utils.param_file_helper import ParamFileHelper from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from threading import Event class ParamFileHelperTests(unittest.TestCase): @@ -49,22 +50,69 @@ def test_ParamFileHelper_Crazyflie_Object(self): @patch('cflib.crazyflie.Param') def test_ParamFileHelper_writesAndStoresParamFromFileToCrazyflie(self, mock_Param): + #Setup cf_mock = MagicMock(spec=Crazyflie) cf_mock.param = mock_Param helper = ParamFileHelper(cf_mock) - helper.store_params_from_file('test/utils/fixtures/single_param.yaml') - mock_Param.set_value.assert_called_once_with('activeMarker.back',10) - mock_Param.persistent_store.assert_called_once_with('activeMarker.back') + #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + def mock_wait(self,timeout=None): + helper._persistent_stored_callback('activeMarker.back', True) + return + + with patch.object(Event, 'wait', new=mock_wait): + self.assertTrue(helper.store_params_from_file('test/utils/fixtures/single_param.yaml')) + mock_Param.set_value.assert_called_once_with('activeMarker.back',10) + mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback) + + @patch('cflib.crazyflie.Param') + def test_ParamFileHelper_writesParamAndFailsToSetPersistantShouldReturnFalse(self, mock_Param): + #Setup + cf_mock = MagicMock(spec=Crazyflie) + cf_mock.param = mock_Param + helper = ParamFileHelper(cf_mock) + #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + def mock_wait(self,timeout=None): + helper._persistent_stored_callback('activeMarker.back', False) + return + + with patch.object(Event, 'wait', new=mock_wait): + self.assertFalse(helper.store_params_from_file('test/utils/fixtures/single_param.yaml')) + mock_Param.set_value.assert_called_once_with('activeMarker.back',10) + mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback) + @patch('cflib.crazyflie.Param') + def test_ParamFileHelper_TryWriteSeveralParamsPersistantShouldBreakAndReturnFalse(self, mock_Param): + #Setup + cf_mock = MagicMock(spec=Crazyflie) + cf_mock.param = mock_Param + helper = ParamFileHelper(cf_mock) + #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + def mock_wait(self,timeout=None): + helper._persistent_stored_callback('activeMarker.back', False) + return + + with patch.object(Event, 'wait', new=mock_wait): + #Test and assert + self.assertFalse(helper.store_params_from_file('test/utils/fixtures/five_params.yaml')) + #Assert it breaks directly by checking number of calls + mock_Param.set_value.assert_called_once_with('activeMarker.back',10) + mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback) @patch('cflib.crazyflie.Param') def test_ParamFileHelper_writesAndStoresAllParamsFromFileToCrazyflie(self, mock_Param): + #Setup cf_mock = MagicMock(spec=Crazyflie) cf_mock.param = mock_Param helper = ParamFileHelper(cf_mock) - helper.store_params_from_file('test/utils/fixtures/five_params.yaml') - self.assertEquals(5,len(mock_Param.set_value.mock_calls)) - self.assertEquals(5,len(mock_Param.persistent_store.mock_calls)) + #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + def mock_wait(self,timeout=None): + helper._persistent_stored_callback('something', True) + return + with patch.object(Event, 'wait', new=mock_wait): + #Test and Assert + self.assertTrue(helper.store_params_from_file('test/utils/fixtures/five_params.yaml')) + self.assertEquals(5,len(mock_Param.set_value.mock_calls)) + self.assertEquals(5,len(mock_Param.persistent_store.mock_calls)) From a0df110d990690f8c8858b5c75e5e47c71c0bf9b Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 5 Jun 2024 11:10:14 +0200 Subject: [PATCH 641/861] Add example script that uses the ParamFileHelper. The script will write all supplied parameters to the crazyflie and store them Re-format code to code standard --- cflib/utils/param_file_helper.py | 10 +-- .../parameters/persistent_params_from_file.py | 60 ++++++++++++++++ test/utils/test_param_file_helper.py | 69 ++++++++++--------- 3 files changed, 101 insertions(+), 38 deletions(-) create mode 100644 examples/parameters/persistent_params_from_file.py diff --git a/cflib/utils/param_file_helper.py b/cflib/utils/param_file_helper.py index 2dce8899e..08ae21c35 100644 --- a/cflib/utils/param_file_helper.py +++ b/cflib/utils/param_file_helper.py @@ -19,20 +19,23 @@ # 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 . +from threading import Event + from cflib.crazyflie import Crazyflie from cflib.localization.param_io import ParamFileManager -from threading import Event class ParamFileHelper: + '''ParamFileHelper is a helper to synchonously write multiple paramteters + from a file and store them in persistent memory''' + def __init__(self, crazyflie): if isinstance(crazyflie, Crazyflie): self._cf = crazyflie self.persistent_sema = None self.success = False else: - raise TypeError("ParamFileHelper only takes a Crazyflie Object") - + raise TypeError('ParamFileHelper only takes a Crazyflie Object') def _persistent_stored_callback(self, complete_name, success): self.success = success @@ -42,7 +45,6 @@ def _persistent_stored_callback(self, complete_name, success): print(f'Persistent params: stored {complete_name}!') self.persistent_sema.set() - def store_params_from_file(self, filename): params = ParamFileManager().read(filename) for param, state in params.items(): diff --git a/examples/parameters/persistent_params_from_file.py b/examples/parameters/persistent_params_from_file.py new file mode 100644 index 000000000..dfb5008d6 --- /dev/null +++ b/examples/parameters/persistent_params_from_file.py @@ -0,0 +1,60 @@ +# -*- 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 . +""" +Example to show how to write several persistent parameters from a yaml file. +The params in the file should be formatted like this; + +params: + activeMarker.back: + default_value: 3 + is_stored: true + stored_value: 30 +type: persistent_param_state +version: '1' +""" +import argparse +import logging + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper +from cflib.utils.param_file_helper import ParamFileHelper + + +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__': + parser = argparse.ArgumentParser() + parser.add_argument('-f', '--file', type=str, help='The yaml file containing the arguments. ') + args = parser.parse_args() + + cflib.crtp.init_drivers() + + cf = Crazyflie(rw_cache='./cache') + with SyncCrazyflie(uri, cf=cf) as scf: + writer = ParamFileHelper(scf.cf) + writer.store_params_from_file(args.file) diff --git a/test/utils/test_param_file_helper.py b/test/utils/test_param_file_helper.py index 32f0bab56..a06a8c953 100644 --- a/test/utils/test_param_file_helper.py +++ b/test/utils/test_param_file_helper.py @@ -20,12 +20,14 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import unittest +from threading import Event from unittest.mock import MagicMock from unittest.mock import patch -from cflib.utils.param_file_helper import ParamFileHelper + from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from threading import Event +from cflib.utils.param_file_helper import ParamFileHelper + class ParamFileHelperTests(unittest.TestCase): @@ -38,82 +40,81 @@ def test_ParamFileHelper_SyncCrazyflieAsParam_ThrowsException(self): helper = None try: helper = ParamFileHelper(cf_mock) - except Exception as e: - self.assertIsNone(helper) + except Exception: + self.assertIsNone(helper) else: self.fail('Expect exception') - def test_ParamFileHelper_Crazyflie_Object(self): helper = ParamFileHelper(self.cf_mock) - self.assertIsNotNone(helper) + self.assertIsNotNone(helper) @patch('cflib.crazyflie.Param') def test_ParamFileHelper_writesAndStoresParamFromFileToCrazyflie(self, mock_Param): - #Setup + # Setup cf_mock = MagicMock(spec=Crazyflie) cf_mock.param = mock_Param helper = ParamFileHelper(cf_mock) - #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world - def mock_wait(self,timeout=None): + # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + + def mock_wait(self, timeout=None): helper._persistent_stored_callback('activeMarker.back', True) return - + with patch.object(Event, 'wait', new=mock_wait): self.assertTrue(helper.store_params_from_file('test/utils/fixtures/single_param.yaml')) - mock_Param.set_value.assert_called_once_with('activeMarker.back',10) + mock_Param.set_value.assert_called_once_with('activeMarker.back', 10) mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback) - + @patch('cflib.crazyflie.Param') def test_ParamFileHelper_writesParamAndFailsToSetPersistantShouldReturnFalse(self, mock_Param): - #Setup + # Setup cf_mock = MagicMock(spec=Crazyflie) cf_mock.param = mock_Param helper = ParamFileHelper(cf_mock) - #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world - def mock_wait(self,timeout=None): + # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + + def mock_wait(self, timeout=None): helper._persistent_stored_callback('activeMarker.back', False) return - + with patch.object(Event, 'wait', new=mock_wait): self.assertFalse(helper.store_params_from_file('test/utils/fixtures/single_param.yaml')) - mock_Param.set_value.assert_called_once_with('activeMarker.back',10) + mock_Param.set_value.assert_called_once_with('activeMarker.back', 10) mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback) @patch('cflib.crazyflie.Param') def test_ParamFileHelper_TryWriteSeveralParamsPersistantShouldBreakAndReturnFalse(self, mock_Param): - #Setup + # Setup cf_mock = MagicMock(spec=Crazyflie) cf_mock.param = mock_Param helper = ParamFileHelper(cf_mock) - #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world - def mock_wait(self,timeout=None): + # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + + def mock_wait(self, timeout=None): helper._persistent_stored_callback('activeMarker.back', False) return - + with patch.object(Event, 'wait', new=mock_wait): - #Test and assert + # Test and assert self.assertFalse(helper.store_params_from_file('test/utils/fixtures/five_params.yaml')) - #Assert it breaks directly by checking number of calls - mock_Param.set_value.assert_called_once_with('activeMarker.back',10) + # Assert it breaks directly by checking number of calls + mock_Param.set_value.assert_called_once_with('activeMarker.back', 10) mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback) @patch('cflib.crazyflie.Param') def test_ParamFileHelper_writesAndStoresAllParamsFromFileToCrazyflie(self, mock_Param): - #Setup + # Setup cf_mock = MagicMock(spec=Crazyflie) cf_mock.param = mock_Param helper = ParamFileHelper(cf_mock) - #Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world - def mock_wait(self,timeout=None): + # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world + + def mock_wait(self, timeout=None): helper._persistent_stored_callback('something', True) return with patch.object(Event, 'wait', new=mock_wait): - #Test and Assert + # Test and Assert self.assertTrue(helper.store_params_from_file('test/utils/fixtures/five_params.yaml')) - self.assertEquals(5,len(mock_Param.set_value.mock_calls)) - self.assertEquals(5,len(mock_Param.persistent_store.mock_calls)) - - - - \ No newline at end of file + self.assertEquals(5, len(mock_Param.set_value.mock_calls)) + self.assertEquals(5, len(mock_Param.persistent_store.mock_calls)) From c789ffcb7ca1ab7f997d8f88c7686732a21315a4 Mon Sep 17 00:00:00 2001 From: hml1810 Date: Fri, 21 Jun 2024 12:32:01 +0200 Subject: [PATCH 642/861] Update wireshark.md added information on how to generate pcaps on windows based computers. --- docs/development/wireshark.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/docs/development/wireshark.md b/docs/development/wireshark.md index a4c64c2fe..c8980fd5c 100644 --- a/docs/development/wireshark.md +++ b/docs/development/wireshark.md @@ -37,6 +37,13 @@ To tell the CFLIB to generate a PCAP file of what it thinks the CRTP traffic loo $ CRTP_PCAP_LOG=filename.pcap python3 examples/swarm/hl-commander-swarm.py $ wireshark filename.pcap ``` +and on Windows based computers in a shell window: + +```bash +> set CRTP_PCAP_LOG=filename.pcap +> python3 examples/swarm/hl-commander-swarm.py +> wireshark filename.pcap +``` To generate a PCAP file and open it with Wireshark. You can also use the text based `tshark` tool, and you can add a filter, for instance, only shoow CRTP port 8 (Highlevel setpoint): ```bash From d6e5c0acb3a33de24c3bae33b82de858dba22ae9 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 27 Jun 2024 11:10:24 +0200 Subject: [PATCH 643/861] Fix lighthouse geometry estimation example * Fixed broken while loops that were incorrectly exiting immediately without retries. * Gathered data recording code into functions. Implemented timeout and retry mechanism in record_angles_average. * Added timeouts for recording data --- .../multi_bs_geometry_estimation.py | 87 +++++++++++-------- 1 file changed, 52 insertions(+), 35 deletions(-) diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py index 955c06d4f..4f6dc7c15 100644 --- a/examples/lighthouse/multi_bs_geometry_estimation.py +++ b/examples/lighthouse/multi_bs_geometry_estimation.py @@ -72,7 +72,7 @@ REFERENCE_DIST = 1.0 -def record_angles_average(scf: SyncCrazyflie) -> LhCfPoseSample: +def record_angles_average(scf: SyncCrazyflie, timeout: float = 5.0) -> LhCfPoseSample: """Record angles and average over the samples to reduce noise""" recorded_angles = None @@ -85,7 +85,10 @@ def ready_cb(averages): reader = LighthouseSweepAngleAverageReader(scf.cf, ready_cb) reader.start_angle_collection() - is_ready.wait() + + if not is_ready.wait(timeout): + print('Recording timed out.') + return None angles_calibrated = {} for bs_id, data in recorded_angles.items(): @@ -303,6 +306,49 @@ def estimate_from_file(file_name: str): 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}...') @@ -313,44 +359,15 @@ def connect_and_estimate(uri: str, file_name: str | None = None): print('Step 2. Put the Crazyflie where you want the origin of your coordinate system.') - origin = None - do_repeat = True - while do_repeat: - input('Press return when ready. ') - print(' Recording...') - measurement = record_angles_average(scf) - do_repeat = False - if measurement is not None: - origin = measurement - else: - do_repeat = True + 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 = [] - do_repeat = True - while do_repeat: - input('Press return when ready. ') - print(' Recording...') - measurement = record_angles_average(scf) - do_repeat = False - if measurement is not None: - x_axis = [measurement] - else: - do_repeat = True + 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, type "r" before you hit enter to repeat the step.') - xy_plane = [] - do_repeat = True - while do_repeat: - do_repeat = 'r' == input('Press return when ready. ').lower() - print(' Recording...') - measurement = record_angles_average(scf) - if measurement is not None: - xy_plane.append(measurement) - else: - do_repeat = True + print('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 ' + From a93844a643a84ba44e8ae32771abe355a2456869 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Thu, 27 Jun 2024 15:33:47 +0200 Subject: [PATCH 644/861] Add ip address instead of CRTP address to fpv example This example needs to connect to the crazyflie over tcp. --- examples/aideck/fpv.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 5bf0e51f1..411387a33 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -68,7 +68,11 @@ logging.basicConfig(level=logging.INFO) -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') +# 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 From 7feb51e26ff3fdcb8da0c3352326499f4b07ab7a Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 1 Jul 2024 10:31:35 +0200 Subject: [PATCH 645/861] Update version to 0.1.26 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index a6373efe4..ebfbc8c28 100644 --- a/setup.py +++ b/setup.py @@ -13,7 +13,7 @@ setup( name='cflib', - version='0.1.25.1', + version='0.1.26', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From 3d0682cefb0081460dac83a4fa2f6d3105ddb834 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Thu, 8 Aug 2024 13:04:12 +0200 Subject: [PATCH 646/861] Update checkout and artiact actions to v4. v3 gives deprecation warnings --- .github/workflows/CI.yml | 2 +- .github/workflows/dependency_check.yml | 6 +++--- .github/workflows/python-publish.yml | 2 +- .github/workflows/test-python-publish.yml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index f5658f071..5204c1992 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -16,7 +16,7 @@ jobs: steps: - name: Checkout repo - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Build run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index d3903dbbd..62e847543 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -12,7 +12,7 @@ jobs: # image: python:latest # # steps: -# - uses: actions/checkout@v3 +# - uses: actions/checkout@v4 # # - name: python version # run: python --version @@ -26,7 +26,7 @@ jobs: image: python:3.11 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: python version run: python --version @@ -40,7 +40,7 @@ jobs: image: python:3.7 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: python version run: python --version diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml index 11f05a53d..0dc3d068c 100644 --- a/.github/workflows/python-publish.yml +++ b/.github/workflows/python-publish.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Set up Python uses: actions/setup-python@v4 with: diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index baabc27c1..444e635f2 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Set up Python uses: actions/setup-python@v4 with: From 61180d8d8ffb3119a34ff22d191906f64a5d8060 Mon Sep 17 00:00:00 2001 From: Matej Karasek Date: Fri, 13 Sep 2024 13:38:20 +0200 Subject: [PATCH 647/861] High Level commander - adding Spiral and linear Go To --- cflib/crazyflie/high_level_commander.py | 37 +++++++++++++++++++++---- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 7966193e3..90f573ff4 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -41,12 +41,13 @@ class HighLevelCommander(): COMMAND_SET_GROUP_MASK = 0 COMMAND_STOP = 3 - COMMAND_GO_TO = 4 COMMAND_START_TRAJECTORY = 5 COMMAND_DEFINE_TRAJECTORY = 6 COMMAND_TAKEOFF_2 = 7 COMMAND_LAND_2 = 8 - + COMMAND_SPIRAL = 11 + COMMAND_GO_TO_2 = 12 + ALL_GROUPS = 0 TRAJECTORY_LOCATION_MEM = 1 @@ -131,7 +132,7 @@ def stop(self, group_mask=ALL_GROUPS): self.COMMAND_STOP, group_mask)) - def go_to(self, x, y, z, yaw, duration_s, relative=False, + def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, group_mask=ALL_GROUPS): """ Go to an absolute or relative position @@ -142,15 +143,41 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, :param yaw: Yaw (radians) :param duration_s: Time it should take to reach the position (s) :param relative: True if x, y, z is relative to the current position + :param linear: True to use linear interpolation instead of a smooth polynomial :param group_mask: Mask for which CFs this should apply to """ - self._send_packet(struct.pack(' Date: Mon, 16 Sep 2024 11:32:10 +0200 Subject: [PATCH 648/861] Bump supported python versions Deprecate 3.7 since we dont even test it and its ancient. Add 3.12 since its now supported on all OSs --- docs/installation/install.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 778b75b80..0029b6385 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -5,9 +5,8 @@ page_id: install ## Requirements -This project requires Python 3.7 - 3.11. +This project requires Python 3.8 - 3.12. -> Python 3.12 is not supported as it has issues with missing packages (see [this ticket](https://github.com/bitcraze/crazyflie-lib-python/issues/425)) See below sections for more platform-specific requirements. ## Install from Source From 42f4bf10465b072d98679064c650d9ba88ed0ea2 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Mon, 16 Sep 2024 11:41:00 +0200 Subject: [PATCH 649/861] Update documentation for how to run python 3.12 on macOs --- docs/installation/install.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/docs/installation/install.md b/docs/installation/install.md index 0029b6385..3fcc5381d 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -77,3 +77,15 @@ With linux, the crazyradio is easily recognized, but you have to setup UDEVpermi ### Windows Look at the [Zadig crazyradio instructions](https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/usbwindows/) to install crazyradio on Windows + +### macOs +If you are using python 3.12 on mac you need to install libusb using homebrew. +``` +$ brew install libusb +``` + +If your Homebrew installation is in a non default location or if you want to install libusb in some other way you +might need to link the libusb library like this; +``` +$ export DYLD_LIBRARY_PATH="YOUR_HOMEBREW_PATH/lib:$DYLD_LIBRARY_PATH" +``` From 7c6676749ed607d4a66b3b84ce48a8378e98e99f Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Wed, 18 Sep 2024 14:41:24 +0200 Subject: [PATCH 650/861] Update install.md --- docs/installation/install.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 3fcc5381d..d75d4c6aa 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -78,7 +78,7 @@ With linux, the crazyradio is easily recognized, but you have to setup UDEVpermi Look at the [Zadig crazyradio instructions](https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/usbwindows/) to install crazyradio on Windows -### macOs +### macOS If you are using python 3.12 on mac you need to install libusb using homebrew. ``` $ brew install libusb From 5177a16fbe316e4851002bf2b965696dde3c6d1f Mon Sep 17 00:00:00 2001 From: Matej Karasek Date: Wed, 25 Sep 2024 13:49:48 +0200 Subject: [PATCH 651/861] Revert "High Level commander - adding Spiral and linear Go To" This reverts commit 61180d8d8ffb3119a34ff22d191906f64a5d8060. --- cflib/crazyflie/high_level_commander.py | 37 ++++--------------------- 1 file changed, 5 insertions(+), 32 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 90f573ff4..7966193e3 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -41,13 +41,12 @@ class HighLevelCommander(): COMMAND_SET_GROUP_MASK = 0 COMMAND_STOP = 3 + COMMAND_GO_TO = 4 COMMAND_START_TRAJECTORY = 5 COMMAND_DEFINE_TRAJECTORY = 6 COMMAND_TAKEOFF_2 = 7 COMMAND_LAND_2 = 8 - COMMAND_SPIRAL = 11 - COMMAND_GO_TO_2 = 12 - + ALL_GROUPS = 0 TRAJECTORY_LOCATION_MEM = 1 @@ -132,7 +131,7 @@ def stop(self, group_mask=ALL_GROUPS): self.COMMAND_STOP, group_mask)) - def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, + def go_to(self, x, y, z, yaw, duration_s, relative=False, group_mask=ALL_GROUPS): """ Go to an absolute or relative position @@ -143,41 +142,15 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, :param yaw: Yaw (radians) :param duration_s: Time it should take to reach the position (s) :param relative: True if x, y, z is relative to the current position - :param linear: True to use linear interpolation instead of a smooth polynomial :param group_mask: Mask for which CFs this should apply to """ - self._send_packet(struct.pack(' Date: Wed, 25 Sep 2024 13:54:44 +0200 Subject: [PATCH 652/861] adding Spiral and Goto2 allowing linear motion --- cflib/crazyflie/high_level_commander.py | 49 +++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 7966193e3..964289bc6 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -46,6 +46,8 @@ class HighLevelCommander(): COMMAND_DEFINE_TRAJECTORY = 6 COMMAND_TAKEOFF_2 = 7 COMMAND_LAND_2 = 8 + COMMAND_SPIRAL = 11 + COMMAND_GO_TO_2 = 12 ALL_GROUPS = 0 @@ -151,6 +153,53 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, x, y, z, yaw, duration_s)) + + def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False, + group_mask=ALL_GROUPS): + """ + Go to an absolute or relative position + + :param x: X (m) + :param y: Y (m) + :param z: Z (m) + :param yaw: Yaw (radians) + :param duration_s: Time it should take to reach the position (s) + :param relative: True if x, y, z is relative to the current position + :param linear: True to use linear interpolation instead of a smooth polynomial + :param group_mask: Mask for which CFs this should apply to + """ + self._send_packet(struct.pack(' Date: Wed, 25 Sep 2024 17:15:27 +0200 Subject: [PATCH 653/861] Echo timestamps to calculate latency --- cflib/crtp/radiodriver.py | 67 ++++++++++++++++++++++++++++++++++----- 1 file changed, 59 insertions(+), 8 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 7d22b6c52..9399e8f7f 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -49,8 +49,10 @@ from urllib.parse import parse_qs from urllib.parse import urlparse +import numpy as np + import cflib.drivers.crazyradio as crazyradio -from .crtpstack import CRTPPacket +from .crtpstack import CRTPPacket, CRTPPort from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver from cflib.drivers.crazyradio import Crazyradio @@ -534,6 +536,12 @@ def __init__(self, radio, inQueue, outQueue, self._retry_sum = 0 self.rate_limit = rate_limit + self._packed_last_ping_time = None + self._last_ping_time = 0 + self._latencies = [] + self._p95_latency = None + self._ping_timeout = 3 + self._curr_up = 0 self._curr_down = 1 @@ -632,8 +640,12 @@ def run(self): # If there is a copter in range, the packet is analysed and the # next packet to send is prepared if (len(data) > 0): - inPacket = CRTPPacket(data[0], list(data[1:])) - self._in_queue.put(inPacket) + if data[1:] == self._packed_last_ping_time: + # This is a ping response + self._calculate_latency(struct.unpack(" self._ping_timeout): + out_packet = CRTPPacket() + out_packet.set_header(CRTPPort.LINKCTRL, 0) + + # Pack the current time as the ping timestamp + current_time = time.time() + out_packet.data = struct.pack(" 100: + self._latencies.pop(0) + self._p95_latency = np.percentile(self._latencies, 95) + print("95th percentile latency: {} ms".format(self._p95_latency)) + + # Indicate that the next ping can be sent + self._packed_last_ping_time = None def set_retries_before_disconnect(nr_of_retries): global _nr_of_retries From fda63c1158d1b4565a032561cbc4038a5680ee94 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 26 Sep 2024 12:40:49 +0200 Subject: [PATCH 654/861] Refactor latency measurement into dedicated Latency class - Moved latency measurement from the radio driver to a new Latency class for better organization and readability. - Generalized the LinkStatistics class to allow for easy addition of other link statistics in the future. --- cflib/crazyflie/__init__.py | 32 ++++- cflib/crazyflie/link_statistics.py | 186 +++++++++++++++++++++++++++++ cflib/crtp/radiodriver.py | 56 +-------- 3 files changed, 219 insertions(+), 55 deletions(-) create mode 100644 cflib/crazyflie/link_statistics.py diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index a64930683..679b32bde 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -44,6 +44,7 @@ from .commander import Commander from .console import Console from .extpos import Extpos +from .link_statistics import LinkStatistics from .localization import Localization from .log import Log from .mem import Memory @@ -121,6 +122,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.mem = Memory(self) self.platform = PlatformService(self) self.appchannel = Appchannel(self) + self.link_statistics = LinkStatistics(self) self.link_uri = '' @@ -153,6 +155,11 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.fully_connected.add_callback( lambda uri: logger.info('Callback->Connection completed [%s]', uri)) + self.connected.add_callback( + lambda uri: self.link_statistics.start()) + self.disconnected.add_callback( + lambda uri: self.link_statistics.stop()) + def _disconnected(self, link_uri): """ Callback when disconnected.""" self.connected_ts = None @@ -283,6 +290,14 @@ def add_port_callback(self, port, cb): def remove_port_callback(self, port, cb): """Remove the callback cb on port""" self.incoming.remove_port_callback(port, cb) + + def add_header_callback(self, cb, port, channel, port_mask=0xFF, channel_mask=0xFF): + """Add a callback to cb on port and channel""" + self.incoming.add_header_callback(cb, port, channel, port_mask, channel_mask) + + def remove_header_callback(self, cb, port, channel, port_mask=0xFF, channel_mask=0xFF): + """Remove the callback cb on port and channel""" + self.incoming.remove_header_callback(cb, port, channel, port_mask, channel_mask) def _no_answer_do_retry(self, pk, pattern): """Resend packets that we have not gotten answers to""" @@ -380,9 +395,7 @@ def add_port_callback(self, port, cb): def remove_port_callback(self, port, cb): """Remove a callback for data that comes on a specific port""" logger.debug('Removing callback on port [%d] to [%s]', port, cb) - for port_callback in self.cb: - if port_callback.port == port and port_callback.callback == cb: - self.cb.remove(port_callback) + self.remove_header_callback(cb, port, 0, 0xff, 0x0) def add_header_callback(self, cb, port, channel, port_mask=0xFF, channel_mask=0xFF): @@ -394,6 +407,19 @@ def add_header_callback(self, cb, port, channel, port_mask=0xFF, self.cb.append(_CallbackContainer(port, port_mask, channel, channel_mask, cb)) + def remove_header_callback(self, cb, port, channel, port_mask=0xFF, + channel_mask=0xFF): + """ + Remove a callback for a specific port/header callback with the + possibility to add a mask for channel and port for multiple + hits for same callback. + """ + for port_callback in self.cb: + if port_callback.port == port and port_callback.port_mask == port_mask and \ + port_callback.channel == channel and port_callback.channel_mask == channel_mask and \ + port_callback.callback == cb: + self.cb.remove(port_callback) + def run(self): while True: if self.cf.link is None: diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py new file mode 100644 index 000000000..3dfe28819 --- /dev/null +++ b/cflib/crazyflie/link_statistics.py @@ -0,0 +1,186 @@ +# -*- 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 . + +""" +This module provides tools for tracking statistics related to the communication +link between the Crazyflie and the lib. Currently, it focuses on tracking latency +but is designed to be extended with additional link statistics in the future. +""" + +from cflib.crtp.crtpstack import CRTPPort, CRTPPacket +from threading import Thread, Event +import time +import struct +import numpy as np + +__author__ = "Bitcraze AB" +__all__ = ["LinkStatistics"] + +PING_HEADER = 0x0 +ECHO_CHANNEL = 0 + + +class LinkStatistics: + """ + LinkStatistics class manages the collection of various statistics related to the + communication link between the Crazyflie and the lib. + + This class serves as a high-level manager, initializing and coordinating multiple + statistics trackers, such as Latency. It allows starting and stopping all + statistics trackers simultaneously. Future statistics can be added to extend + the class's functionality. + + Attributes: + _cf (Crazyflie): A reference to the Crazyflie instance. + latency (Latency): An instance of the Latency class that tracks latency statistics. + """ + + def __init__(self, crazyflie): + self._cf = crazyflie + + self.latency = Latency(self._cf) + + def start(self): + """ + Start collecting all statistics. + """ + self.latency.start() + + def stop(self): + """ + Stop collecting all statistics. + """ + self.latency.stop() + + +class Latency: + """ + The Latency class measures and tracks the latency of the communication link + between the Crazyflie and the lib. + + This class periodically sends ping requests to the Crazyflie and tracks + the round-trip time (latency). It calculates and stores the 95th percentile + latency over a rolling window of recent latency measurements. + + Attributes: + _cf (Crazyflie): A reference to the Crazyflie instance. + latency (float): The current calculated 95th percentile latency in milliseconds. + _stop_event (Event): An event object to control the stopping of the ping thread. + _ping_thread_instance (Thread): Thread instance for sending ping requests at intervals. + """ + + def __init__(self, crazyflie): + self._cf = crazyflie + self._cf.add_header_callback(self._ping_response, CRTPPort.LINKCTRL, 0) + self._stop_event = Event() + self._ping_thread_instance = Thread(target=self._ping_thread) + self.latency = 0 + + def start(self): + """ + Start the latency tracking process. + + This method initiates a background thread that sends ping requests + at regular intervals to measure and track latency statistics. + """ + self._ping_thread_instance.start() + + def stop(self): + """ + Stop the latency tracking process. + + This method stops the background thread and ceases sending further + ping requests, halting latency measurement. + """ + self._stop_event.set() + self._ping_thread_instance.join() + + def _ping_thread(self, interval: float = 1.0) -> None: + """ + Background thread method that sends a ping to the Crazyflie at regular intervals. + + This method runs in a separate thread and continues to send ping requests + until the stop event is set. + + Args: + interval (float): The time (in seconds) to wait between ping requests. Default is 1 second. + """ + while not self._stop_event.is_set(): + self.ping() + time.sleep(interval) + + def ping(self) -> None: + """ + Send a ping request to the Crazyflie to measure latency. + + A ping packet is sent to the Crazyflie with the current timestamp and a + header identifier to differentiate it from other echo responses. The latency + is calculated upon receiving the response. + """ + ping_packet = CRTPPacket() + ping_packet.set_header(CRTPPort.LINKCTRL, ECHO_CHANNEL) + + # Pack the current time as the ping timestamp + current_time = time.time() + ping_packet.data = struct.pack(" 100: + self._latencies.pop(0) + p95_latency = np.percentile(self._latencies, 95) + return p95_latency diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 9399e8f7f..5f773cc0a 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -52,7 +52,7 @@ import numpy as np import cflib.drivers.crazyradio as crazyradio -from .crtpstack import CRTPPacket, CRTPPort +from .crtpstack import CRTPPacket from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver from cflib.drivers.crazyradio import Crazyradio @@ -536,12 +536,6 @@ def __init__(self, radio, inQueue, outQueue, self._retry_sum = 0 self.rate_limit = rate_limit - self._packed_last_ping_time = None - self._last_ping_time = 0 - self._latencies = [] - self._p95_latency = None - self._ping_timeout = 3 - self._curr_up = 0 self._curr_down = 1 @@ -572,7 +566,6 @@ def _send_packet_safe(self, cr, packet): self._curr_down = 1 - self._curr_down if resp and resp.ack: self._curr_up = 1 - self._curr_up - return resp def run(self): @@ -640,12 +633,8 @@ def run(self): # If there is a copter in range, the packet is analysed and the # next packet to send is prepared if (len(data) > 0): - if data[1:] == self._packed_last_ping_time: - # This is a ping response - self._calculate_latency(struct.unpack(" self._ping_timeout): - out_packet = CRTPPacket() - out_packet.set_header(CRTPPort.LINKCTRL, 0) - - # Pack the current time as the ping timestamp - current_time = time.time() - out_packet.data = struct.pack(" 100: - self._latencies.pop(0) - self._p95_latency = np.percentile(self._latencies, 95) - print("95th percentile latency: {} ms".format(self._p95_latency)) - - # Indicate that the next ping can be sent - self._packed_last_ping_time = None def set_retries_before_disconnect(nr_of_retries): global _nr_of_retries From bb2e55c05c9c7ad58680954be1167371e0dbdf8e Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 26 Sep 2024 12:43:39 +0200 Subject: [PATCH 655/861] Undo redundant radiodriver changes --- cflib/crtp/radiodriver.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 5f773cc0a..7d22b6c52 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -49,8 +49,6 @@ from urllib.parse import parse_qs from urllib.parse import urlparse -import numpy as np - import cflib.drivers.crazyradio as crazyradio from .crtpstack import CRTPPacket from .exceptions import WrongUriType @@ -566,6 +564,7 @@ def _send_packet_safe(self, cr, packet): self._curr_down = 1 - self._curr_down if resp and resp.ack: self._curr_up = 1 - self._curr_up + return resp def run(self): @@ -653,12 +652,10 @@ def run(self): # get the next packet to send of relaxation (wait 10ms) outPacket = None - - if not outPacket: - try: - outPacket = self._out_queue.get(True, waitTime) - except queue.Empty: - outPacket = None + try: + outPacket = self._out_queue.get(True, waitTime) + except queue.Empty: + outPacket = None dataOut = array.array('B') From 1b1f1b129cd3de958b6fd90bad22a55425f8e06a Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 26 Sep 2024 12:57:10 +0200 Subject: [PATCH 656/861] =?UTF-8?q?=F0=9F=90=8D=20Python=20Formatting=20Fi?= =?UTF-8?q?asco:=20The=20Quest=20for=20Perfect=20Quotation!?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit In a valiant attempt to appease the relentless gods of autopep8 and the all-seeing eyes of CI, I embarked on an epic journey to fix double-quoted strings and reorder imports. --- cflib/crazyflie/__init__.py | 24 ++++++++++++------------ cflib/crazyflie/link_statistics.py | 22 ++++++++++++---------- 2 files changed, 24 insertions(+), 22 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 679b32bde..015188f44 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -290,7 +290,7 @@ def add_port_callback(self, port, cb): def remove_port_callback(self, port, cb): """Remove the callback cb on port""" self.incoming.remove_port_callback(port, cb) - + def add_header_callback(self, cb, port, channel, port_mask=0xFF, channel_mask=0xFF): """Add a callback to cb on port and channel""" self.incoming.add_header_callback(cb, port, channel, port_mask, channel_mask) @@ -408,17 +408,17 @@ def add_header_callback(self, cb, port, channel, port_mask=0xFF, channel, channel_mask, cb)) def remove_header_callback(self, cb, port, channel, port_mask=0xFF, - channel_mask=0xFF): - """ - Remove a callback for a specific port/header callback with the - possibility to add a mask for channel and port for multiple - hits for same callback. - """ - for port_callback in self.cb: - if port_callback.port == port and port_callback.port_mask == port_mask and \ - port_callback.channel == channel and port_callback.channel_mask == channel_mask and \ - port_callback.callback == cb: - self.cb.remove(port_callback) + channel_mask=0xFF): + """ + Remove a callback for a specific port/header callback with the + possibility to add a mask for channel and port for multiple + hits for same callback. + """ + for port_callback in self.cb: + if port_callback.port == port and port_callback.port_mask == port_mask and \ + port_callback.channel == channel and port_callback.channel_mask == channel_mask and \ + port_callback.callback == cb: + self.cb.remove(port_callback) def run(self): while True: diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py index 3dfe28819..16c4fe154 100644 --- a/cflib/crazyflie/link_statistics.py +++ b/cflib/crazyflie/link_statistics.py @@ -19,21 +19,23 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . - """ This module provides tools for tracking statistics related to the communication link between the Crazyflie and the lib. Currently, it focuses on tracking latency but is designed to be extended with additional link statistics in the future. """ - -from cflib.crtp.crtpstack import CRTPPort, CRTPPacket -from threading import Thread, Event -import time import struct +import time +from threading import Event +from threading import Thread + import numpy as np -__author__ = "Bitcraze AB" -__all__ = ["LinkStatistics"] +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort + +__author__ = 'Bitcraze AB' +__all__ = ['LinkStatistics'] PING_HEADER = 0x0 ECHO_CHANNEL = 0 @@ -141,7 +143,7 @@ def ping(self) -> None: # Pack the current time as the ping timestamp current_time = time.time() - ping_packet.data = struct.pack(" Date: Thu, 26 Sep 2024 15:06:02 +0200 Subject: [PATCH 657/861] Enhance Latency class to allow restarting the ping thread - Modified start() and stop() methods for better thread management. - Implemented thread reinitialization on start after stop. --- cflib/crazyflie/link_statistics.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py index 16c4fe154..14630405a 100644 --- a/cflib/crazyflie/link_statistics.py +++ b/cflib/crazyflie/link_statistics.py @@ -94,7 +94,7 @@ def __init__(self, crazyflie): self._cf = crazyflie self._cf.add_header_callback(self._ping_response, CRTPPort.LINKCTRL, 0) self._stop_event = Event() - self._ping_thread_instance = Thread(target=self._ping_thread) + self._ping_thread_instance = None self.latency = 0 def start(self): @@ -104,7 +104,10 @@ def start(self): This method initiates a background thread that sends ping requests at regular intervals to measure and track latency statistics. """ - self._ping_thread_instance.start() + if self._ping_thread_instance is None or not self._ping_thread_instance.is_alive(): + self._stop_event.clear() + self._ping_thread_instance = Thread(target=self._ping_thread) + self._ping_thread_instance.start() def stop(self): """ @@ -114,7 +117,9 @@ def stop(self): ping requests, halting latency measurement. """ self._stop_event.set() - self._ping_thread_instance.join() + if self._ping_thread_instance is not None: + self._ping_thread_instance.join() + self._ping_thread_instance = None def _ping_thread(self, interval: float = 1.0) -> None: """ From a80e67005b81ff9edccd3e4d67393ac2780e86bf Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 26 Sep 2024 16:17:56 +0200 Subject: [PATCH 658/861] Add Caller to indicate updated latency + example how to use --- cflib/crazyflie/link_statistics.py | 3 ++ examples/link_quality/latency.py | 55 ++++++++++++++++++++++++++++++ 2 files changed, 58 insertions(+) create mode 100644 examples/link_quality/latency.py diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py index 14630405a..1c536fdac 100644 --- a/cflib/crazyflie/link_statistics.py +++ b/cflib/crazyflie/link_statistics.py @@ -33,6 +33,7 @@ from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort +from cflib.utils.callbacks import Caller __author__ = 'Bitcraze AB' __all__ = ['LinkStatistics'] @@ -96,6 +97,7 @@ def __init__(self, crazyflie): self._stop_event = Event() self._ping_thread_instance = None self.latency = 0 + self.latencyUpdated = Caller() def start(self): """ @@ -167,6 +169,7 @@ def _ping_response(self, packet): if received_header != PING_HEADER: return self.latency = self._calculate_p95_latency(received_timestamp) + self.latencyUpdated.call(self.latency) def _calculate_p95_latency(self, timestamp): """ diff --git a/examples/link_quality/latency.py b/examples/link_quality/latency.py new file mode 100644 index 000000000..ece87577c --- /dev/null +++ b/examples/link_quality/latency.py @@ -0,0 +1,55 @@ +# -*- 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.latencyUpdated.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) From 4136a69eb0ccc3dcfd66fa728e482cbcc884a9d7 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 30 Sep 2024 12:51:25 +0200 Subject: [PATCH 659/861] " -> ' --- examples/link_quality/latency.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/link_quality/latency.py b/examples/link_quality/latency.py index ece87577c..b73c92b72 100644 --- a/examples/link_quality/latency.py +++ b/examples/link_quality/latency.py @@ -27,20 +27,20 @@ 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") +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") + print(f'Latency: {latency:.3f} ms') -if __name__ == "__main__": +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") + cf = Crazyflie(rw_cache='./cache') # Add a callback to whenever we receive an updated latency estimate # @@ -49,7 +49,7 @@ def latency_callback(latency: float): # 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.") + print('[host] Connected, use ctrl-c to quit.') while True: time.sleep(1) From 3b7d7bbfb1d6d63ce98978f7cbcabc5bd6fa7f90 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Tue, 1 Oct 2024 07:57:44 +0200 Subject: [PATCH 660/861] fix autopep8 wrapper check --- cflib/crazyflie/high_level_commander.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 964289bc6..188f2b1f2 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -153,9 +153,9 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, x, y, z, yaw, duration_s)) - + def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False, - group_mask=ALL_GROUPS): + group_mask=ALL_GROUPS): """ Go to an absolute or relative position @@ -176,9 +176,9 @@ def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False, x, y, z, yaw, duration_s)) - + def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=False, - group_mask=ALL_GROUPS): + group_mask=ALL_GROUPS): """ Follow a spiral-like segment (spline approximation of a spiral/arc for <= 90-degree segments) From da4fcbb235c801194c0aa3563dc01c103df580db Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 3 Oct 2024 10:28:46 +0200 Subject: [PATCH 661/861] Receive uplink RSSI - Refactor link quality callback to contain signal health obj with additional signal health information - Parse uplink RSSI in ack into signal health obj --- cflib/crazyflie/__init__.py | 12 ++-- cflib/crtp/__init__.py | 4 +- cflib/crtp/cflinkcppdriver.py | 18 ++--- cflib/crtp/crtpdriver.py | 4 +- cflib/crtp/radiodriver.py | 39 +++++----- cflib/crtp/signal_health.py | 71 +++++++++++++++++++ cflib/crtp/usbdriver.py | 19 +++-- sys_test/swarm_test_rig/test_response_time.py | 4 +- 8 files changed, 120 insertions(+), 51 deletions(-) create mode 100644 cflib/crtp/signal_health.py diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index a64930683..988cc4958 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -99,6 +99,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.packet_sent = Caller() # Called when the link driver updates the link quality measurement self.link_quality_updated = Caller() + self.uplink_rssi_updated = Caller() self.state = State.DISCONNECTED @@ -204,9 +205,12 @@ def _link_error_cb(self, errmsg): self.connection_lost.call(self.link_uri, errmsg) self.state = State.DISCONNECTED - def _link_quality_cb(self, percentage): - """Called from link driver to report link quality""" - self.link_quality_updated.call(percentage) + def _signal_health_cb(self, signal_health): + """Called from link driver to report signal health""" + if signal_health.link_quality: + self.link_quality_updated.call(signal_health.link_quality) + if signal_health.uplink_rssi: + self.uplink_rssi_updated.call(signal_health.uplink_rssi) def _check_for_initial_packet_cb(self, data): """ @@ -229,7 +233,7 @@ def open_link(self, link_uri): self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver( - link_uri, self._link_quality_cb, self._link_error_cb) + link_uri, self._signal_health_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 756e9a66c..ddb950b92 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -89,14 +89,14 @@ def get_interfaces_status(): return status -def get_link_driver(uri, link_quality_callback=None, link_error_callback=None): +def get_link_driver(uri, signal_health_callback=None, link_error_callback=None): """Return the link driver for the given URI. Returns None if no driver was found for the URI or the URI was not well formatted for the matching driver.""" for driverClass in CLASSES: try: instance = driverClass() - instance.connect(uri, link_quality_callback, link_error_callback) + instance.connect(uri, signal_health_callback, link_error_callback) return instance except WrongUriType: continue diff --git a/cflib/crtp/cflinkcppdriver.py b/cflib/crtp/cflinkcppdriver.py index ab69f0773..fff1ebfc3 100644 --- a/cflib/crtp/cflinkcppdriver.py +++ b/cflib/crtp/cflinkcppdriver.py @@ -34,6 +34,7 @@ from .crtpstack import CRTPPacket from cflib.crtp.crtpdriver import CRTPDriver +from cflib.crtp.signal_health import SignalHealth __author__ = 'Bitcraze AB' __all__ = ['CfLinkCppDriver'] @@ -54,22 +55,23 @@ def __init__(self): self.needs_resending = False self._connection = None + self._signal_health = SignalHealth() - def connect(self, uri, link_quality_callback, link_error_callback): + def connect(self, uri, signal_health_callback, link_error_callback): """Connect the driver to a specified URI @param uri Uri of the link to open - @param link_quality_callback Callback to report link quality in percent + @param signal_health_callback Callback to report signal health @param link_error_callback Callback to report errors (will result in disconnection) """ self._connection = cflinkcpp.Connection(uri) self.uri = uri - self._link_quality_callback = link_quality_callback + self._signal_health_callback = signal_health_callback self._link_error_callback = link_error_callback - if uri.startswith('radio://') and link_quality_callback is not None: + if uri.startswith('radio://') and signal_health_callback is not None: self._last_connection_stats = self._connection.statistics self._recompute_link_quality_timer() @@ -181,13 +183,13 @@ def _recompute_link_quality_timer(self): sent_count = stats.sent_count - self._last_connection_stats.sent_count ack_count = stats.ack_count - self._last_connection_stats.ack_count if sent_count > 0: - link_quality = min(ack_count, sent_count) / sent_count * 100.0 + self._signal_health.link_quality = min(ack_count, sent_count) / sent_count * 100.0 else: - link_quality = 1 + self._signal_health.link_quality = 1 self._last_connection_stats = stats - if self._link_quality_callback is not None: - self._link_quality_callback(link_quality) + if self._signal_health_callback is not None: + self._signal_health_callback(self._signal_health) if sent_count > 10 and ack_count == 0 and self._link_error_callback is not None: self._link_error_callback('Too many packets lost') diff --git a/cflib/crtp/crtpdriver.py b/cflib/crtp/crtpdriver.py index 2f620fd30..d38247731 100644 --- a/cflib/crtp/crtpdriver.py +++ b/cflib/crtp/crtpdriver.py @@ -42,11 +42,11 @@ def __init__(self): """ self.needs_resending = True - def connect(self, uri, link_quality_callback, link_error_callback): + def connect(self, uri, signal_health_callback, link_error_callback): """Connect the driver to a specified URI @param uri Uri of the link to open - @param link_quality_callback Callback to report link quality in percent + @param signal_health_callback Callback to report signal health @param link_error_callback Callback to report errors (will result in disconnection) """ diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 7d22b6c52..e37b8b37d 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -30,7 +30,6 @@ """ import array import binascii -import collections import logging import queue import re @@ -53,6 +52,7 @@ from .crtpstack import CRTPPacket from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver +from cflib.crtp.signal_health import SignalHealth from cflib.drivers.crazyradio import Crazyradio @@ -241,20 +241,20 @@ def __init__(self): self._radio = None self.uri = '' self.link_error_callback = None - self.link_quality_callback = None + self.signal_health_callback = None self.in_queue = None self.out_queue = None self._thread = None self.needs_resending = True - def connect(self, uri, link_quality_callback, link_error_callback): + def connect(self, uri, signal_health_callback, link_error_callback): """ Connect the link driver to a specified URI of the format: radio:////[250K,1M,2M] - The callback for linkQuality can be called at any moment from the - driver to report back the link quality in percentage. The - callback from linkError will be called when a error occurs with + The callback for signal health can be called at any moment from the + driver to report back the signal health. The callback from linkError + will be called when a error occurs with an error message. """ @@ -283,7 +283,7 @@ def connect(self, uri, link_quality_callback, link_error_callback): self._thread = _RadioDriverThread(self._radio, self.in_queue, self.out_queue, - link_quality_callback, + signal_health_callback, link_error_callback, self, rate_limit) @@ -381,7 +381,7 @@ def restart(self): self._thread = _RadioDriverThread(self._radio, self.in_queue, self.out_queue, - self.link_quality_callback, + self.signal_health_callback, self.link_error_callback, self) self._thread.start() @@ -401,7 +401,7 @@ def close(self): # Clear callbacks self.link_error_callback = None - self.link_quality_callback = None + self.signal_health_callback = None def _scan_radio_channels(self, radio: _SharedRadioInstance, start=0, stop=125): @@ -520,7 +520,7 @@ class _RadioDriverThread(threading.Thread): Crazyradio USB driver. """ def __init__(self, radio, inQueue, outQueue, - link_quality_callback, link_error_callback, link, rate_limit: Optional[int]): + signal_health_callback, link_error_callback, link, rate_limit: Optional[int]): """ Create the object """ threading.Thread.__init__(self) self._radio = radio @@ -528,10 +528,9 @@ def __init__(self, radio, inQueue, outQueue, self._out_queue = outQueue self._sp = False self._link_error_callback = link_error_callback - self._link_quality_callback = link_quality_callback + self._signal_health_callback = signal_health_callback + self._signal_health = SignalHealth() self._retry_before_disconnect = _nr_of_retries - self._retries = collections.deque() - self._retry_sum = 0 self.rate_limit = rate_limit self._curr_up = 0 @@ -606,16 +605,10 @@ def run(self): if ackStatus is None: logger.info('Dongle reported ACK status == None') continue - - if (self._link_quality_callback is not None): - # track the mean of a sliding window of the last N packets - retry = 10 - ackStatus.retry - self._retries.append(retry) - self._retry_sum += retry - if len(self._retries) > 100: - self._retry_sum -= self._retries.popleft() - link_quality = float(self._retry_sum) / len(self._retries) * 10 - self._link_quality_callback(link_quality) + else: + self._signal_health.update(ackStatus) + if (self._signal_health_callback is not None): + self._signal_health_callback(self._signal_health) # If no copter, retry if ackStatus.ack is False: diff --git a/cflib/crtp/signal_health.py b/cflib/crtp/signal_health.py new file mode 100644 index 000000000..057034101 --- /dev/null +++ b/cflib/crtp/signal_health.py @@ -0,0 +1,71 @@ +import collections +import time + +import numpy as np + + +class SignalHealth: + """ + Tracks the health of the signal by monitoring link quality and uplink RSSI + using exponential moving averages. + """ + + def __init__(self, alpha=0.1): + """ + Initialize the SignalHealth class. + + :param alpha: Weight for the exponential moving average (default 0.1) + """ + self.alpha = alpha + self.link_quality = 0 + self.uplink_rssi = 0 + self._retries = collections.deque() + self._retry_sum = 0 + + def update(self, ack): + """ + Update the signal health based on the acknowledgment data. + + :param ack: Acknowledgment object containing retry and RSSI data. + """ + self._update_link_quality(ack) + self._update_rssi(ack) + + def _update_link_quality(self, ack): + """ + Updates the link quality based on the acknowledgment data. + + :param ack: Acknowledgment object with retry data. + """ + retry = 10 - ack.retry + self._retries.append(retry) + self._retry_sum += retry + if len(self._retries) > 100: + self._retry_sum -= self._retries.popleft() + self.link_quality = float(self._retry_sum) / len(self._retries) * 10 + + def _update_rssi(self, ack): + """ + Updates the uplink RSSI based on the acknowledgment signal. + + :param ack: Acknowledgment object with RSSI data. + """ + if not hasattr(self, '_rssi_timestamps'): + self._rssi_timestamps = collections.deque(maxlen=100) + if not hasattr(self, '_rssi_values'): + self._rssi_values = collections.deque(maxlen=100) + + # update RSSI if the acknowledgment contains RSSI data + if ack.ack and len(ack.data) > 2 and ack.data[0] & 0xf3 == 0xf3 and ack.data[1] == 0x01: + instantaneous_rssi = ack.data[2] + self._rssi_values.append(instantaneous_rssi) + self._rssi_timestamps.append(time.time()) + + # Calculate time-weighted average RSSI + if len(self._rssi_timestamps) >= 2: # At least 2 points are needed to calculate differences + time_diffs = np.diff(self._rssi_timestamps, prepend=time.time()) + weights = np.exp(-time_diffs) + weighted_average = np.sum(weights * self._rssi_values) / np.sum(weights) + self.uplink_rssi = weighted_average + else: + self.uplink_rssi = instantaneous_rssi # Return the raw value if not enough data diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index 859244438..eb18fed27 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -52,21 +52,20 @@ def __init__(self): self.cfusb = None self.uri = '' self.link_error_callback = None - self.link_quality_callback = None + self.signal_health_callback = None self.in_queue = None self.out_queue = None self._thread = None self.needs_resending = False - def connect(self, uri, link_quality_callback, link_error_callback): + def connect(self, uri, signal_health_callback, link_error_callback): """ Connect the link driver to a specified URI of the format: radio:////[250K,1M,2M] - The callback for linkQuality can be called at any moment from the - driver to report back the link quality in percentage. The - callback from linkError will be called when a error occurs with - an error message. + The callback for signal health can be called at any moment from + the driver to report back the signal health. The callback from + linkError will be called when a error occurs with an error message. """ # check if the URI is a radio URI @@ -100,7 +99,7 @@ def connect(self, uri, link_quality_callback, link_error_callback): # Launch the comm thread self._thread = _UsbReceiveThread(self.cfusb, self.in_queue, - link_quality_callback, + signal_health_callback, link_error_callback) self._thread.start() @@ -152,7 +151,7 @@ def restart(self): return self._thread = _UsbReceiveThread(self.cfusb, self.in_queue, - self.link_quality_callback, + self.signal_health_callback, self.link_error_callback) self._thread.start() @@ -208,7 +207,7 @@ class _UsbReceiveThread(threading.Thread): Radio link receiver thread used to read data from the Crazyradio USB driver. """ - def __init__(self, cfusb, inQueue, link_quality_callback, + def __init__(self, cfusb, inQueue, signal_health_callback, link_error_callback): """ Create the object """ threading.Thread.__init__(self) @@ -216,7 +215,7 @@ def __init__(self, cfusb, inQueue, link_quality_callback, self.in_queue = inQueue self.sp = False self.link_error_callback = link_error_callback - self.link_quality_callback = link_quality_callback + self.signal_health_callback = signal_health_callback def stop(self): """ Stop the thread """ diff --git a/sys_test/swarm_test_rig/test_response_time.py b/sys_test/swarm_test_rig/test_response_time.py index da9222fce..582a9519f 100644 --- a/sys_test/swarm_test_rig/test_response_time.py +++ b/sys_test/swarm_test_rig/test_response_time.py @@ -128,14 +128,14 @@ def _is_response_correct_seq_nr(self, response, seq_nr): return False def connect_link(self, uri): - link = cflib.crtp.get_link_driver(uri, self._link_quality_cb, + link = cflib.crtp.get_link_driver(uri, self._signal_health_cb, self._link_error_cb) self.assertIsNotNone(link) self.links.append(link) return link - def _link_quality_cb(self, percentage): + def _signal_health_cb(self, signal_health): pass def _link_error_cb(self, errmsg): From 4435c7c19f125cd2d9ea807d9e2a922201ba814f Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 3 Oct 2024 13:10:30 +0200 Subject: [PATCH 662/861] add congestion statistics --- cflib/crtp/radiodriver.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index e37b8b37d..ba35a5a01 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -584,6 +584,12 @@ def run(self): break self._link.needs_resending = not self._has_safelink + previous_time_stamp = time.time() + amount_null_packets_up = 0 + amount_packets_up = 0 + amount_null_packets_down = 0 + amount_packets_down = 0 + while (True): if (self._sp): break @@ -620,10 +626,18 @@ def run(self): continue self._retry_before_disconnect = _nr_of_retries + ## Find null packets in the downlink and count them data = ackStatus.data + mask = 0b11110011 + empty_ack_packet = int(data[0]) & mask + + if empty_ack_packet == 0xF3: + amount_null_packets_down += 1 + amount_packets_down += 1 # If there is a copter in range, the packet is analysed and the # next packet to send is prepared + # TODO: THis seems not to work since there is always a byte filled in the data even with null packets if (len(data) > 0): inPacket = CRTPPacket(data[0], list(data[1:])) self._in_queue.put(inPacket) @@ -660,7 +674,25 @@ def run(self): else: dataOut.append(ord(X)) else: + # If no packet to send, send a null packet dataOut.append(0xFF) + amount_null_packets_up += 1 + amount_packets_up += 1 + + # Low level stats every second + if time.time() - previous_time_stamp > 1: + rate_up = amount_packets_up / (time.time() - previous_time_stamp) + rate_down = amount_packets_down / (time.time() - previous_time_stamp) + congestion_up = 1.0 - amount_null_packets_up / amount_packets_up + congestion_down = 1.0 - amount_null_packets_down / amount_packets_down + + amount_packets_up = 0 + amount_null_packets_up = 0 + amount_packets_down = 0 + amount_null_packets_down = 0 + previous_time_stamp = time.time() + + self._link_quality_low_level_callback(rate_up, rate_down, congestion_up, congestion_down) def set_retries_before_disconnect(nr_of_retries): From 73963e01b0e4a1a9337f28bfce00e4379c63397f Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 3 Oct 2024 15:57:34 +0200 Subject: [PATCH 663/861] Move bandwidth congestion and packet rate into signal health class Return a dictionary with all updated signal quality statistics in the callback, rather than the entire class. --- cflib/crazyflie/__init__.py | 20 +++++++-- cflib/crtp/radiodriver.py | 41 ++----------------- cflib/crtp/signal_health.py | 81 ++++++++++++++++++++++++++++++------- 3 files changed, 86 insertions(+), 56 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 988cc4958..d96028b7f 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -100,6 +100,10 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): # Called when the link driver updates the link quality measurement self.link_quality_updated = Caller() self.uplink_rssi_updated = Caller() + self.uplink_rate_updated = Caller() + self.downlink_rate_updated = Caller() + self.uplink_congestion_updated = Caller() + self.downlink_congestion_updated = Caller() self.state = State.DISCONNECTED @@ -207,10 +211,18 @@ def _link_error_cb(self, errmsg): def _signal_health_cb(self, signal_health): """Called from link driver to report signal health""" - if signal_health.link_quality: - self.link_quality_updated.call(signal_health.link_quality) - if signal_health.uplink_rssi: - self.uplink_rssi_updated.call(signal_health.uplink_rssi) + if 'link_quality' in signal_health: + self.link_quality_updated.call(signal_health['link_quality']) + if 'uplink_rssi' in signal_health: + self.uplink_rssi_updated.call(signal_health['uplink_rssi']) + if 'uplink_rate' in signal_health: + self.uplink_rate_updated.call(signal_health['uplink_rate']) + if 'downlink_rate' in signal_health: + self.downlink_rate_updated.call(signal_health['downlink_rate']) + if 'uplink_congestion' in signal_health: + self.uplink_congestion_updated.call(signal_health['uplink_congestion']) + if 'downlink_congestion' in signal_health: + self.downlink_congestion_updated.call(signal_health['downlink_congestion']) def _check_for_initial_packet_cb(self, data): """ diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index ba35a5a01..4ffd04ee3 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -528,8 +528,7 @@ def __init__(self, radio, inQueue, outQueue, self._out_queue = outQueue self._sp = False self._link_error_callback = link_error_callback - self._signal_health_callback = signal_health_callback - self._signal_health = SignalHealth() + self._signal_health = SignalHealth(signal_health_callback) self._retry_before_disconnect = _nr_of_retries self.rate_limit = rate_limit @@ -584,12 +583,6 @@ def run(self): break self._link.needs_resending = not self._has_safelink - previous_time_stamp = time.time() - amount_null_packets_up = 0 - amount_packets_up = 0 - amount_null_packets_down = 0 - amount_packets_down = 0 - while (True): if (self._sp): break @@ -611,10 +604,6 @@ def run(self): if ackStatus is None: logger.info('Dongle reported ACK status == None') continue - else: - self._signal_health.update(ackStatus) - if (self._signal_health_callback is not None): - self._signal_health_callback(self._signal_health) # If no copter, retry if ackStatus.ack is False: @@ -626,18 +615,11 @@ def run(self): continue self._retry_before_disconnect = _nr_of_retries - ## Find null packets in the downlink and count them data = ackStatus.data - mask = 0b11110011 - empty_ack_packet = int(data[0]) & mask - - if empty_ack_packet == 0xF3: - amount_null_packets_down += 1 - amount_packets_down += 1 # If there is a copter in range, the packet is analysed and the # next packet to send is prepared - # TODO: THis seems not to work since there is always a byte filled in the data even with null packets + # TODO: This does not seem to work since there is always a byte filled in the data even with null packets if (len(data) > 0): inPacket = CRTPPacket(data[0], list(data[1:])) self._in_queue.put(inPacket) @@ -676,23 +658,8 @@ def run(self): else: # If no packet to send, send a null packet dataOut.append(0xFF) - amount_null_packets_up += 1 - amount_packets_up += 1 - - # Low level stats every second - if time.time() - previous_time_stamp > 1: - rate_up = amount_packets_up / (time.time() - previous_time_stamp) - rate_down = amount_packets_down / (time.time() - previous_time_stamp) - congestion_up = 1.0 - amount_null_packets_up / amount_packets_up - congestion_down = 1.0 - amount_null_packets_down / amount_packets_down - - amount_packets_up = 0 - amount_null_packets_up = 0 - amount_packets_down = 0 - amount_null_packets_down = 0 - previous_time_stamp = time.time() - - self._link_quality_low_level_callback(rate_up, rate_down, congestion_up, congestion_down) + + self._signal_health.update(ackStatus, outPacket) def set_retries_before_disconnect(nr_of_retries): diff --git a/cflib/crtp/signal_health.py b/cflib/crtp/signal_health.py index 057034101..d44354e08 100644 --- a/cflib/crtp/signal_health.py +++ b/cflib/crtp/signal_health.py @@ -10,39 +10,46 @@ class SignalHealth: using exponential moving averages. """ - def __init__(self, alpha=0.1): + def __init__(self, signal_health_callback, alpha=0.1): """ Initialize the SignalHealth class. :param alpha: Weight for the exponential moving average (default 0.1) """ - self.alpha = alpha - self.link_quality = 0 - self.uplink_rssi = 0 + self._alpha = alpha + self._signal_health_callback = signal_health_callback + self._retries = collections.deque() self._retry_sum = 0 - def update(self, ack): + def update(self, ack, packet_out): """ Update the signal health based on the acknowledgment data. :param ack: Acknowledgment object containing retry and RSSI data. """ + self.signal_health = {} + self._update_link_quality(ack) self._update_rssi(ack) + self._update_rate_and_congestion(ack, packet_out) + + if self.signal_health: + self._signal_health_callback(self.signal_health) def _update_link_quality(self, ack): """ - Updates the link quality based on the acknowledgment data. + Updates the link quality based on the number of retries. :param ack: Acknowledgment object with retry data. """ - retry = 10 - ack.retry - self._retries.append(retry) - self._retry_sum += retry - if len(self._retries) > 100: - self._retry_sum -= self._retries.popleft() - self.link_quality = float(self._retry_sum) / len(self._retries) * 10 + if ack: + retry = 10 - ack.retry + self._retries.append(retry) + self._retry_sum += retry + if len(self._retries) > 100: + self._retry_sum -= self._retries.popleft() + self.signal_health['link_quality'] = float(self._retry_sum) / len(self._retries) * 10 def _update_rssi(self, ack): """ @@ -66,6 +73,50 @@ def _update_rssi(self, ack): time_diffs = np.diff(self._rssi_timestamps, prepend=time.time()) weights = np.exp(-time_diffs) weighted_average = np.sum(weights * self._rssi_values) / np.sum(weights) - self.uplink_rssi = weighted_average - else: - self.uplink_rssi = instantaneous_rssi # Return the raw value if not enough data + self.signal_health['uplink_rssi'] = weighted_average + + def _update_rate_and_congestion(self, ack, packet_out): + """ + Updates the packet rate and bandwidth congestion based on the acknowledgment data. + + :param ack: Acknowledgment object with congestion data. + """ + if not hasattr(self, '_previous_time_stamp'): + self._previous_time_stamp = time.time() + if not hasattr(self, '_amount_null_packets_up'): + self._amount_null_packets_up = 0 + if not hasattr(self, '_amount_packets_up'): + self._amount_packets_up = 0 + if not hasattr(self, '_amount_null_packets_down'): + self._amount_null_packets_down = 0 + if not hasattr(self, '_amount_packets_down'): + self._amount_packets_down = 0 + + self._amount_packets_up += 1 # everytime this function is called, a packet is sent + if not packet_out: # if the packet is empty, we send a null packet + self._amount_null_packets_up += 1 + + # Find null packets in the downlink and count them + mask = 0b11110011 + if ack.data: + empty_ack_packet = int(ack.data[0]) & mask + + if empty_ack_packet == 0xF3: + self._amount_null_packets_down += 1 + self._amount_packets_down += 1 + + # rate and congestion stats every N seconds + if time.time() - self._previous_time_stamp > 0.1: + # self._uplink_rate = self._amount_packets_up / (time.time() - self._previous_time_stamp) + self.signal_health['uplink_rate'] = self._amount_packets_up / (time.time() - self._previous_time_stamp) + self.signal_health['downlink_rate'] = self._amount_packets_down / \ + (time.time() - self._previous_time_stamp) + self.signal_health['uplink_congestion'] = 1.0 - self._amount_null_packets_up / self._amount_packets_up + self.signal_health['downlink_congestion'] = 1.0 - \ + self._amount_null_packets_down / self._amount_packets_down + + self._amount_packets_up = 0 + self._amount_null_packets_up = 0 + self._amount_packets_down = 0 + self._amount_null_packets_down = 0 + self._previous_time_stamp = time.time() From 5344f6986de6e2b5593b4899497d573fa91e3d7d Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Thu, 3 Oct 2024 18:05:39 +0200 Subject: [PATCH 664/861] Single goto with a version check & limit of spiral parameters --- cflib/crazyflie/high_level_commander.py | 72 +++++++++++++------------ 1 file changed, 38 insertions(+), 34 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 188f2b1f2..58f607fcc 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -25,7 +25,9 @@ """ Used for sending high level setpoints to the Crazyflie """ +import math import struct +import warnings from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort @@ -133,32 +135,11 @@ def stop(self, group_mask=ALL_GROUPS): self.COMMAND_STOP, group_mask)) - def go_to(self, x, y, z, yaw, duration_s, relative=False, + def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, group_mask=ALL_GROUPS): """ Go to an absolute or relative position - :param x: X (m) - :param y: Y (m) - :param z: Z (m) - :param yaw: Yaw (radians) - :param duration_s: Time it should take to reach the position (s) - :param relative: True if x, y, z is relative to the current position - :param group_mask: Mask for which CFs this should apply to - """ - self._send_packet(struct.pack(' 2*math.pi: + angle = 2*math.pi + warnings.warning('Spiral angle saturated at 2pi as it was too large') + elif angle < -2*math.pi: + angle = -2*math.pi + warnings.warning('Spiral angle saturated at -2pi as it was too small') + if r0 < 0: + r0 = 0 + warnings.warning('Initial radius set to 0 as it cannot be negative') + if rF < 0: + rF = 0 + warnings.warning('Final radius set to 0 as it cannot be negative') self._send_packet(struct.pack(' Date: Thu, 3 Oct 2024 19:02:27 +0200 Subject: [PATCH 665/861] bugfixes --- cflib/crazyflie/high_level_commander.py | 52 +++++++++++++------------ 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 58f607fcc..4a7792f7f 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -27,7 +27,6 @@ """ import math import struct -import warnings from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort @@ -151,8 +150,8 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, """ if self._cf.platform.get_protocol_version() < 8: if linear: - warnings.warning('Linear mode is not supported in protocol version < 8, update your Crazyflie firmware') - self._send_packet(struct.pack(' 2*math.pi: - angle = 2*math.pi - warnings.warning('Spiral angle saturated at 2pi as it was too large') - elif angle < -2*math.pi: - angle = -2*math.pi - warnings.warning('Spiral angle saturated at -2pi as it was too small') - if r0 < 0: - r0 = 0 - warnings.warning('Initial radius set to 0 as it cannot be negative') - if rF < 0: - rF = 0 - warnings.warning('Final radius set to 0 as it cannot be negative') - self._send_packet(struct.pack(' 2*math.pi: + angle = 2*math.pi + print('Warning: Spiral angle saturated at 2pi as it was too large') + elif angle < -2*math.pi: + angle = -2*math.pi + print('Warning: Spiral angle saturated at -2pi as it was too small') + if r0 < 0: + r0 = 0 + print('Warning: Initial radius set to 0 as it cannot be negative') + if rF < 0: + rF = 0 + print('Warning: Final radius set to 0 as it cannot be negative') + self._send_packet(struct.pack(' Date: Thu, 3 Oct 2024 19:08:23 +0200 Subject: [PATCH 666/861] fix formating --- cflib/crazyflie/high_level_commander.py | 46 ++++++++++++------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 4a7792f7f..98ab846d7 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -150,7 +150,7 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, """ if self._cf.platform.get_protocol_version() < 8: if linear: - print('Warning: Linear mode is not supported in protocol version < 8, update the firmware of your crazyflie') + print('Warning: Linear mode not supported in protocol version < 8, update your crazyflie\'s firmware') self._send_packet(struct.pack(' 2*math.pi: - angle = 2*math.pi - print('Warning: Spiral angle saturated at 2pi as it was too large') - elif angle < -2*math.pi: - angle = -2*math.pi - print('Warning: Spiral angle saturated at -2pi as it was too small') - if r0 < 0: - r0 = 0 - print('Warning: Initial radius set to 0 as it cannot be negative') - if rF < 0: - rF = 0 - print('Warning: Final radius set to 0 as it cannot be negative') - self._send_packet(struct.pack(' 2*math.pi: + angle = 2*math.pi + print('Warning: Spiral angle saturated at 2pi as it was too large') + elif angle < -2*math.pi: + angle = -2*math.pi + print('Warning: Spiral angle saturated at -2pi as it was too small') + if r0 < 0: + r0 = 0 + print('Warning: Initial radius set to 0 as it cannot be negative') + if rF < 0: + rF = 0 + print('Warning: Final radius set to 0 as it cannot be negative') + self._send_packet(struct.pack(' Date: Fri, 4 Oct 2024 12:30:22 +0200 Subject: [PATCH 667/861] Only try to callback signal health when set --- cflib/crtp/signal_health.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crtp/signal_health.py b/cflib/crtp/signal_health.py index d44354e08..ed66a6f41 100644 --- a/cflib/crtp/signal_health.py +++ b/cflib/crtp/signal_health.py @@ -34,7 +34,7 @@ def update(self, ack, packet_out): self._update_rssi(ack) self._update_rate_and_congestion(ack, packet_out) - if self.signal_health: + if self.signal_health and self._signal_health_callback: self._signal_health_callback(self.signal_health) def _update_link_quality(self, ack): From 2e7ddcce43d9549638c835dc52d2980e510b6f3e Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Fri, 4 Oct 2024 13:47:18 +0200 Subject: [PATCH 668/861] Add callback for handling link errors in disconnected state Previously, there was no way to hook into link errors when the state was disconnected. On the initial connection attempt, a callback would be triggered for a failed connection, allowing scripts to handle the error. However, subsequent attempts in the disconnected state did not trigger any callback, forcing autonomous scripts to blindly retry with a timeout. This commit introduces a `disconnected_link_error` callback, enabling scripts to respond to link errors more effectively and retry connections programmatically. --- cflib/crazyflie/__init__.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index a64930683..cb9a0eed4 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -93,6 +93,8 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): # Called if establishing of the link fails (i.e times out) self.connection_failed = Caller() + # Called if link driver has an error while state is DISCONNECTED + self.disconnected_link_error = Caller() # Called for every packet received self.packet_received = Caller() # Called for every packet sent @@ -198,10 +200,12 @@ def _link_error_cb(self, errmsg): self.link = None if (self.state == State.INITIALIZED): self.connection_failed.call(self.link_uri, errmsg) - if (self.state == State.CONNECTED or + elif (self.state == State.CONNECTED or self.state == State.SETUP_FINISHED): self.disconnected.call(self.link_uri) self.connection_lost.call(self.link_uri, errmsg) + elif (self.state == State.DISCONNECTED): + self.disconnected_link_error.call(self.link_uri, errmsg) self.state = State.DISCONNECTED def _link_quality_cb(self, percentage): From 608e1bb44823f452d0b10b81604d02536d7c0242 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 8 Oct 2024 10:47:36 +0200 Subject: [PATCH 669/861] Update nrf compile instructions for radio test --- examples/radio/radio-test.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/examples/radio/radio-test.py b/examples/radio/radio-test.py index d7129977e..f96e947e6 100644 --- a/examples/radio/radio-test.py +++ b/examples/radio/radio-test.py @@ -10,11 +10,10 @@ 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 - ```make BLE=0 CONFIG=-DRSSI_ACK_PACKET```. - Additionally, the Crazyflie must be using the default address 0xE7E7E7E7E7. - See https://github.com/bitcraze/crazyflie-lib-python/issues/131 for more - informations. + 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 From dbc8a257230f77f635ce2187d20d2138a5256036 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 8 Oct 2024 11:37:20 +0200 Subject: [PATCH 670/861] Put led-ring examples in their own folder + rename them for consistency --- examples/{basicLedmemSync.py => led-ring/led_mem_sync.py} | 0 examples/{basicLedparamSync.py => led-ring/led_param_sync.py} | 0 examples/{basicLedTiming.py => led-ring/led_timing.py} | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename examples/{basicLedmemSync.py => led-ring/led_mem_sync.py} (100%) rename examples/{basicLedparamSync.py => led-ring/led_param_sync.py} (100%) rename examples/{basicLedTiming.py => led-ring/led_timing.py} (100%) diff --git a/examples/basicLedmemSync.py b/examples/led-ring/led_mem_sync.py similarity index 100% rename from examples/basicLedmemSync.py rename to examples/led-ring/led_mem_sync.py diff --git a/examples/basicLedparamSync.py b/examples/led-ring/led_param_sync.py similarity index 100% rename from examples/basicLedparamSync.py rename to examples/led-ring/led_param_sync.py diff --git a/examples/basicLedTiming.py b/examples/led-ring/led_timing.py similarity index 100% rename from examples/basicLedTiming.py rename to examples/led-ring/led_timing.py From 253725545a93fc84f66a17f5936d5cf025b943cc Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 8 Oct 2024 11:47:59 +0200 Subject: [PATCH 671/861] In led examples, rename sync -> set to better reflect what happens --- examples/led-ring/{led_mem_sync.py => led_mem_set.py} | 0 examples/led-ring/{led_param_sync.py => led_param_set.py} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename examples/led-ring/{led_mem_sync.py => led_mem_set.py} (100%) rename examples/led-ring/{led_param_sync.py => led_param_set.py} (100%) diff --git a/examples/led-ring/led_mem_sync.py b/examples/led-ring/led_mem_set.py similarity index 100% rename from examples/led-ring/led_mem_sync.py rename to examples/led-ring/led_mem_set.py diff --git a/examples/led-ring/led_param_sync.py b/examples/led-ring/led_param_set.py similarity index 100% rename from examples/led-ring/led_param_sync.py rename to examples/led-ring/led_param_set.py From 7e06e035476c4d334dba046a0b8a818d35391d79 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 9 Oct 2024 14:29:30 +0200 Subject: [PATCH 672/861] Clarify that the erase-ow example, in fact, erases the ow memory. Request user confirmation before executing. --- examples/memory/erase-ow.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/examples/memory/erase-ow.py b/examples/memory/erase-ow.py index 15a529647..16a0bcb54 100644 --- a/examples/memory/erase-ow.py +++ b/examples/memory/erase-ow.py @@ -22,8 +22,11 @@ # 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 lists its contents. +EEPROM memories and erases its contents. """ import logging import sys @@ -42,7 +45,7 @@ class EEPROMExample: """ - Simple example listing the EEPROMs found and lists its contents. + Simple example listing the EEPROMs found and erases its contents. """ def __init__(self, link_uri): @@ -122,6 +125,8 @@ def _disconnected(self, link_uri): if __name__ == '__main__': + input('Warning, this will erase EEPROM memory, press enter to continue...') + # Initialize the low-level drivers cflib.crtp.init_drivers() From 58402744cb3c5be448efc321bfbf5f205b9cea4f Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 9 Oct 2024 14:47:12 +0200 Subject: [PATCH 673/861] Write the default (as claimed) radio address (0xE7E7E7E7E7) in eeprom write example --- examples/memory/write-eeprom.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/memory/write-eeprom.py b/examples/memory/write-eeprom.py index 9dc0a7c8f..be07b7feb 100644 --- a/examples/memory/write-eeprom.py +++ b/examples/memory/write-eeprom.py @@ -83,7 +83,7 @@ def _connected(self, link_uri): elems['roll_trim'] = 0.0 elems['radio_channel'] = 80 elems['radio_speed'] = 2 - elems['radio_address'] = 0xDEADBEEF + elems['radio_address'] = 0xE7E7E7E7E7 mems[0].write_data(self._data_written) From c25bf38efecd573182c38dd4f014033c19c6131d Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 10 Oct 2024 14:22:45 +0200 Subject: [PATCH 674/861] Fix bootloader version check --- cflib/bootloader/__init__.py | 10 +++++++--- setup.py | 1 + 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 14ba1bf5a..4d723b04d 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -37,6 +37,8 @@ from typing import Optional from typing import Tuple +from packaging.version import Version + from .boottypes import BootVersion from .boottypes import TargetTypes from .cloader import Cloader @@ -202,10 +204,10 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo update_contains_nrf_sd = any(x.target.type == 'bootloader+softdevice' for x in flash_artifacts) current_nrf_bl_version = None if self._cload.targets[TargetTypes.NRF51].version is not None: - current_nrf_bl_version = str(self._cload.targets[TargetTypes.NRF51].version) - provided_nrf_bl_version = self._get_provided_nrf51_bl_version(flash_artifacts) + current_nrf_bl_version = Version(str(self._cload.targets[TargetTypes.NRF51].version)) + provided_nrf_bl_version = Version(self._get_provided_nrf51_bl_version(flash_artifacts)) - print('nRF51 has: {} and requires {} and upgrade provides {}. Current bootloader version is [{}] but upgrade ' + print('nRF51 has: {} and requires {} and upgrade provides {}. Current bootloader version is [{}] and upgrade ' 'provides [{}]'.format( current_nrf_sd_version, required_nrf_sd_version, provided_nrf_sd_version, current_nrf_bl_version, provided_nrf_bl_version) @@ -252,6 +254,8 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo print('Reconnected to new bootloader') self._cload.check_link_and_get_info() self._cload.request_info_update(TargetTypes.NRF51) + else: + print('No need to flash nRF soft device') # Remove the softdevice+bootloader from the list of artifacts to flash flash_artifacts = [a for a in flash_artifacts if a.target.type != diff --git a/setup.py b/setup.py index ebfbc8c28..aae527cdd 100644 --- a/setup.py +++ b/setup.py @@ -41,6 +41,7 @@ 'libusb-package~=1.0', 'scipy~=1.7', 'numpy~=1.20', + 'packaging~=24.0', ], # $ pip install -e .[dev] From 7c9ddf4f8d526febac3ce550a679c3528c13d7de Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Fri, 11 Oct 2024 11:54:10 +0200 Subject: [PATCH 675/861] Fix crash when bootloader version is None (happens when doing cload) --- cflib/bootloader/__init__.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 4d723b04d..eb2c482a2 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -205,7 +205,9 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo current_nrf_bl_version = None if self._cload.targets[TargetTypes.NRF51].version is not None: current_nrf_bl_version = Version(str(self._cload.targets[TargetTypes.NRF51].version)) - provided_nrf_bl_version = Version(self._get_provided_nrf51_bl_version(flash_artifacts)) + provided_nrf_bl_version = None + if self._get_provided_nrf51_bl_version(flash_artifacts) is not None: + provided_nrf_bl_version = Version(self._get_provided_nrf51_bl_version(flash_artifacts)) print('nRF51 has: {} and requires {} and upgrade provides {}. Current bootloader version is [{}] and upgrade ' 'provides [{}]'.format( From 3550e03a93a946b6a481662222e598138edae46d Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 14 Oct 2024 11:48:45 +0200 Subject: [PATCH 676/861] Update index.md --- docs/index.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/index.md b/docs/index.md index bfefafcdf..162562d09 100644 --- a/docs/index.md +++ b/docs/index.md @@ -4,7 +4,7 @@ page_id: home --- cflib is an API written in Python that is used to communicate with the Crazyflie -and Crazyflie 2.0 quadcopters. It is intended to be used by client software to +and Crazyflie 2.X quadcopters. It is intended to be used by client software to communicate with and control a Crazyflie quadcopter. For instance the [Crazyflie PC client](https://github.com/bitcraze/crazyflie-clients-python) uses the cflib. ## Contribute From 67bd23eaf2d3a888b351808a9c124cd0c8c1a1d2 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 14 Oct 2024 13:08:06 +0200 Subject: [PATCH 677/861] Update install.md --- docs/installation/install.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index d75d4c6aa..d89785c80 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -28,7 +28,7 @@ pip uninstall cflib Note: If you are developing for the cflib you must use python3. On Ubuntu (20.04+) use `pip3` instead of `pip`. -### Linux, OSX, Windows +### Linux, macOS, Windows The following should be executed in the root of the crazyflie-lib-python file tree. @@ -44,8 +44,8 @@ you can skip this section. * To deactivate the virtualenv when you are done using it `deactivate` -### Pre commit hooks (Ubuntu) -If you want some extra help with keeping to the mandated python coding style you can install hooks that verify your style at commit time. This is done by running: +### Pre-commit hooks (Ubuntu) +If you want some extra help with keeping to the mandated Python coding style you can install hooks that verify your style at commit time. This is done by running: ``` $ pip3 install pre-commit ``` From 3588a3dbbb28d041b8362af1008cdf6cfdfcded0 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 14 Oct 2024 14:34:12 +0200 Subject: [PATCH 678/861] Update matlab.md --- docs/development/matlab.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/docs/development/matlab.md b/docs/development/matlab.md index df5ea1732..b5d5a8677 100644 --- a/docs/development/matlab.md +++ b/docs/development/matlab.md @@ -3,12 +3,11 @@ title: Using Matlab with the Crazyflie API page_id: matlab --- +**Note that these are old instructions and they might not work anymore** To use the Python cflib with matlab, you will need to install the python 'matlab engine' and then can access all matlab commands directly from python. -*Note that these are old instructions and they might not work anymore** - Tried with ------------- From 5f1841ed3599733fd1512b11c5abf827ddab45f3 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 14 Oct 2024 16:33:58 +0200 Subject: [PATCH 679/861] Use `qtm_rt` instead of deprecated `qtm` Python package. --- examples/mocap/qualisys_hl_commander.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/mocap/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py index 59eadc4f3..b46d369c1 100644 --- a/examples/mocap/qualisys_hl_commander.py +++ b/examples/mocap/qualisys_hl_commander.py @@ -33,7 +33,7 @@ import xml.etree.cElementTree as ET from threading import Thread -import qtm +import qtm_rt from scipy.spatial.transform import Rotation import cflib.crtp @@ -106,7 +106,7 @@ async def _connect(self): qtm_instance = await self._discover() host = qtm_instance.host print('Connecting to QTM on ' + host) - self.connection = await qtm.connect(host) + self.connection = await qtm_rt.connect(host) params = await self.connection.get_parameters(parameters=['6d']) xml = ET.fromstring(params) @@ -117,7 +117,7 @@ async def _connect(self): on_packet=self._on_packet) async def _discover(self): - async for qtm_instance in qtm.Discover('0.0.0.0'): + async for qtm_instance in qtm_rt.Discover('0.0.0.0'): return qtm_instance def _on_packet(self, packet): From 3f9c6df006507bcd5913bf5b844bd0533c44dc2e Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Tue, 15 Oct 2024 15:59:05 +0200 Subject: [PATCH 680/861] Update pyqt in fpv example --- examples/aideck/fpv.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 411387a33..8a00f52a0 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -64,7 +64,7 @@ except ImportError: pass -from PyQt5 import QtCore, QtWidgets, QtGui +from PyQt6 import QtCore, QtWidgets, QtGui logging.basicConfig(level=logging.INFO) From 090fbe5787d5501c902b3e9292d6c98646f4baa1 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 16 Oct 2024 10:58:03 +0200 Subject: [PATCH 681/861] Bump required Python version to 3.10 --- .github/workflows/dependency_check.yml | 4 ++-- docs/installation/install.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 62e847543..a9e0317f7 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -34,10 +34,10 @@ jobs: - name: install lib run: pip install -e . - minimum-python37: + minimum-python310: runs-on: ubuntu-latest container: - image: python:3.7 + image: python:3.10 steps: - uses: actions/checkout@v4 diff --git a/docs/installation/install.md b/docs/installation/install.md index d89785c80..f8c696dda 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -5,7 +5,7 @@ page_id: install ## Requirements -This project requires Python 3.8 - 3.12. +This project requires Python 3.10+. See below sections for more platform-specific requirements. From 7c669c2840bee9b3647660455d61ef3b2f1bc662 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 16 Oct 2024 10:58:17 +0200 Subject: [PATCH 682/861] Re-enable latest Python dependency check --- .github/workflows/dependency_check.yml | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index a9e0317f7..f2d09c69d 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -6,19 +6,19 @@ on: - cron: '0 2 * * *' jobs: -# python-latest: -# runs-on: ubuntu-latest -# container: -# image: python:latest -# -# steps: -# - uses: actions/checkout@v4 -# -# - name: python version -# run: python --version -# -# - name: install lib -# run: pip install -e . + python-latest: + runs-on: ubuntu-latest + container: + image: python:latest + + steps: + - uses: actions/checkout@v4 + + - name: python version + run: python --version + + - name: install lib + run: pip install -e . python-python311: runs-on: ubuntu-latest From d5ddf210a5c462afe9312aaf975f9cff86fd8114 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 16 Oct 2024 11:55:34 +0200 Subject: [PATCH 683/861] Consistent "Crazyflie 2.x" formatting --- docs/index.md | 4 ++-- sys_test/single_cf_grounded/README.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/index.md b/docs/index.md index 162562d09..fbb76ef50 100644 --- a/docs/index.md +++ b/docs/index.md @@ -1,10 +1,10 @@ --- title: Home -page_id: home +page_id: home --- cflib is an API written in Python that is used to communicate with the Crazyflie -and Crazyflie 2.X quadcopters. It is intended to be used by client software to +and Crazyflie 2.x quadcopters. It is intended to be used by client software to communicate with and control a Crazyflie quadcopter. For instance the [Crazyflie PC client](https://github.com/bitcraze/crazyflie-clients-python) uses the cflib. ## Contribute diff --git a/sys_test/single_cf_grounded/README.md b/sys_test/single_cf_grounded/README.md index 2a893e02b..b9eb4e9ca 100644 --- a/sys_test/single_cf_grounded/README.md +++ b/sys_test/single_cf_grounded/README.md @@ -1,4 +1,4 @@ -# Tests for a single Crazyflie 2.X (USB & Crazyradio) without flight +# Tests for a single Crazyflie 2.x (USB & Crazyradio) without flight ## Preparation From efbf417f6a1006838c9f3015854ce11f13f5fb1b Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Wed, 16 Oct 2024 14:12:09 +0200 Subject: [PATCH 684/861] Update to version 0.1.27 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index aae527cdd..fdcd5e5a5 100644 --- a/setup.py +++ b/setup.py @@ -13,7 +13,7 @@ setup( name='cflib', - version='0.1.26', + version='0.1.27', packages=find_packages(exclude=['examples', 'test']), description='Crazyflie python driver', From d504247d385640f79a06c41422649a2ddd924a72 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 17 Oct 2024 17:00:24 +0200 Subject: [PATCH 685/861] Stop building universal wheels --- setup.cfg | 2 +- setup.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/setup.cfg b/setup.cfg index 3c6e79cf3..aa76baec6 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,2 +1,2 @@ [bdist_wheel] -universal=1 +universal=0 diff --git a/setup.py b/setup.py index fdcd5e5a5..096f1f87e 100644 --- a/setup.py +++ b/setup.py @@ -30,7 +30,6 @@ 'Development Status :: 4 - Beta', 'License :: OSI Approved :: GNU General Public License v3 (GPLv3)', 'Topic :: System :: Hardware :: Hardware Drivers', - 'Programming Language :: Python :: 2', 'Programming Language :: Python :: 3' ], From 81bfd4a65186a845fbbf449879ecaa1cca00584d Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 22 Oct 2024 16:22:04 +0200 Subject: [PATCH 686/861] Refactor SignalHealth to RadioLinkStatistics for clarity Renamed SignalHealth class to RadioLinkStatistics to better reflect its responsibility of handling and processing radio-specific metrics. --- cflib/crazyflie/__init__.py | 30 ++++++++-------- cflib/crtp/__init__.py | 4 +-- cflib/crtp/cflinkcppdriver.py | 20 +++++------ cflib/crtp/crtpdriver.py | 4 +-- ...nal_health.py => radio_link_statistics.py} | 34 ++++++++++--------- cflib/crtp/radiodriver.py | 22 ++++++------ cflib/crtp/usbdriver.py | 17 +++++----- sys_test/swarm_test_rig/test_response_time.py | 4 +-- 8 files changed, 68 insertions(+), 67 deletions(-) rename cflib/crtp/{signal_health.py => radio_link_statistics.py} (75%) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 1472f3002..d8c4fa919 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -220,20 +220,20 @@ def _link_error_cb(self, errmsg): self.disconnected_link_error.call(self.link_uri, errmsg) self.state = State.DISCONNECTED - def _signal_health_cb(self, signal_health): - """Called from link driver to report signal health""" - if 'link_quality' in signal_health: - self.link_quality_updated.call(signal_health['link_quality']) - if 'uplink_rssi' in signal_health: - self.uplink_rssi_updated.call(signal_health['uplink_rssi']) - if 'uplink_rate' in signal_health: - self.uplink_rate_updated.call(signal_health['uplink_rate']) - if 'downlink_rate' in signal_health: - self.downlink_rate_updated.call(signal_health['downlink_rate']) - if 'uplink_congestion' in signal_health: - self.uplink_congestion_updated.call(signal_health['uplink_congestion']) - if 'downlink_congestion' in signal_health: - self.downlink_congestion_updated.call(signal_health['downlink_congestion']) + def _radio_link_statistics_cb(self, radio_link_statistics): + """Called from link driver to report radio link statistics""" + if 'link_quality' in radio_link_statistics: + self.link_quality_updated.call(radio_link_statistics['link_quality']) + if 'uplink_rssi' in radio_link_statistics: + self.uplink_rssi_updated.call(radio_link_statistics['uplink_rssi']) + if 'uplink_rate' in radio_link_statistics: + self.uplink_rate_updated.call(radio_link_statistics['uplink_rate']) + if 'downlink_rate' in radio_link_statistics: + self.downlink_rate_updated.call(radio_link_statistics['downlink_rate']) + if 'uplink_congestion' in radio_link_statistics: + self.uplink_congestion_updated.call(radio_link_statistics['uplink_congestion']) + if 'downlink_congestion' in radio_link_statistics: + self.downlink_congestion_updated.call(radio_link_statistics['downlink_congestion']) def _check_for_initial_packet_cb(self, data): """ @@ -256,7 +256,7 @@ def open_link(self, link_uri): self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver( - link_uri, self._signal_health_cb, self._link_error_cb) + link_uri, self._radio_link_statistics_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index ddb950b92..3d410554b 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -89,14 +89,14 @@ def get_interfaces_status(): return status -def get_link_driver(uri, signal_health_callback=None, link_error_callback=None): +def get_link_driver(uri, radio_link_statistics_callback=None, link_error_callback=None): """Return the link driver for the given URI. Returns None if no driver was found for the URI or the URI was not well formatted for the matching driver.""" for driverClass in CLASSES: try: instance = driverClass() - instance.connect(uri, signal_health_callback, link_error_callback) + instance.connect(uri, radio_link_statistics_callback, link_error_callback) return instance except WrongUriType: continue diff --git a/cflib/crtp/cflinkcppdriver.py b/cflib/crtp/cflinkcppdriver.py index fff1ebfc3..9d760a7a7 100644 --- a/cflib/crtp/cflinkcppdriver.py +++ b/cflib/crtp/cflinkcppdriver.py @@ -34,7 +34,7 @@ from .crtpstack import CRTPPacket from cflib.crtp.crtpdriver import CRTPDriver -from cflib.crtp.signal_health import SignalHealth +from cflib.crtp.radio_link_statistics import RadioLinkStatistics __author__ = 'Bitcraze AB' __all__ = ['CfLinkCppDriver'] @@ -55,23 +55,23 @@ def __init__(self): self.needs_resending = False self._connection = None - self._signal_health = SignalHealth() + self._radio_link_statistics = RadioLinkStatistics() - def connect(self, uri, signal_health_callback, link_error_callback): + def connect(self, uri, radio_link_statistics_callback, link_error_callback): """Connect the driver to a specified URI @param uri Uri of the link to open - @param signal_health_callback Callback to report signal health + @param radio_link_statistics_callback Callback to report radio link statistics @param link_error_callback Callback to report errors (will result in disconnection) """ self._connection = cflinkcpp.Connection(uri) self.uri = uri - self._signal_health_callback = signal_health_callback + self._radio_link_statistics_callback = radio_link_statistics_callback self._link_error_callback = link_error_callback - if uri.startswith('radio://') and signal_health_callback is not None: + if uri.startswith('radio://') and radio_link_statistics_callback is not None: self._last_connection_stats = self._connection.statistics self._recompute_link_quality_timer() @@ -183,13 +183,13 @@ def _recompute_link_quality_timer(self): sent_count = stats.sent_count - self._last_connection_stats.sent_count ack_count = stats.ack_count - self._last_connection_stats.ack_count if sent_count > 0: - self._signal_health.link_quality = min(ack_count, sent_count) / sent_count * 100.0 + self._radio_link_statistics.link_quality = min(ack_count, sent_count) / sent_count * 100.0 else: - self._signal_health.link_quality = 1 + self._radio_link_statistics.link_quality = 1 self._last_connection_stats = stats - if self._signal_health_callback is not None: - self._signal_health_callback(self._signal_health) + if self._radio_link_statistics_callback is not None: + self._radio_link_statistics_callback(self._radio_link_statistics) if sent_count > 10 and ack_count == 0 and self._link_error_callback is not None: self._link_error_callback('Too many packets lost') diff --git a/cflib/crtp/crtpdriver.py b/cflib/crtp/crtpdriver.py index d38247731..598b29e3d 100644 --- a/cflib/crtp/crtpdriver.py +++ b/cflib/crtp/crtpdriver.py @@ -42,11 +42,11 @@ def __init__(self): """ self.needs_resending = True - def connect(self, uri, signal_health_callback, link_error_callback): + def connect(self, uri, radio_link_statistics_callback, link_error_callback): """Connect the driver to a specified URI @param uri Uri of the link to open - @param signal_health_callback Callback to report signal health + @param radio_link_statistics_callback Callback to report radio link statistics @param link_error_callback Callback to report errors (will result in disconnection) """ diff --git a/cflib/crtp/signal_health.py b/cflib/crtp/radio_link_statistics.py similarity index 75% rename from cflib/crtp/signal_health.py rename to cflib/crtp/radio_link_statistics.py index ed66a6f41..d2a68e0f6 100644 --- a/cflib/crtp/signal_health.py +++ b/cflib/crtp/radio_link_statistics.py @@ -4,38 +4,38 @@ import numpy as np -class SignalHealth: +class RadioLinkStatistics: """ - Tracks the health of the signal by monitoring link quality and uplink RSSI - using exponential moving averages. + Tracks the health of the signal by monitoring link quality, uplink RSSI, + packet rates, and congestion. """ - def __init__(self, signal_health_callback, alpha=0.1): + def __init__(self, radio_link_statistics_callback, alpha=0.1): """ - Initialize the SignalHealth class. + Initialize the RadioLinkStatistics class. :param alpha: Weight for the exponential moving average (default 0.1) """ self._alpha = alpha - self._signal_health_callback = signal_health_callback + self._radio_link_statistics_callback = radio_link_statistics_callback self._retries = collections.deque() self._retry_sum = 0 def update(self, ack, packet_out): """ - Update the signal health based on the acknowledgment data. + Update the radio link statistics based on the acknowledgment data. :param ack: Acknowledgment object containing retry and RSSI data. """ - self.signal_health = {} + self.radio_link_statistics = {} self._update_link_quality(ack) self._update_rssi(ack) self._update_rate_and_congestion(ack, packet_out) - if self.signal_health and self._signal_health_callback: - self._signal_health_callback(self.signal_health) + if self.radio_link_statistics and self._radio_link_statistics_callback: + self._radio_link_statistics_callback(self.radio_link_statistics) def _update_link_quality(self, ack): """ @@ -49,7 +49,7 @@ def _update_link_quality(self, ack): self._retry_sum += retry if len(self._retries) > 100: self._retry_sum -= self._retries.popleft() - self.signal_health['link_quality'] = float(self._retry_sum) / len(self._retries) * 10 + self.radio_link_statistics['link_quality'] = float(self._retry_sum) / len(self._retries) * 10 def _update_rssi(self, ack): """ @@ -73,7 +73,7 @@ def _update_rssi(self, ack): time_diffs = np.diff(self._rssi_timestamps, prepend=time.time()) weights = np.exp(-time_diffs) weighted_average = np.sum(weights * self._rssi_values) / np.sum(weights) - self.signal_health['uplink_rssi'] = weighted_average + self.radio_link_statistics['uplink_rssi'] = weighted_average def _update_rate_and_congestion(self, ack, packet_out): """ @@ -108,11 +108,13 @@ def _update_rate_and_congestion(self, ack, packet_out): # rate and congestion stats every N seconds if time.time() - self._previous_time_stamp > 0.1: # self._uplink_rate = self._amount_packets_up / (time.time() - self._previous_time_stamp) - self.signal_health['uplink_rate'] = self._amount_packets_up / (time.time() - self._previous_time_stamp) - self.signal_health['downlink_rate'] = self._amount_packets_down / \ + self.radio_link_statistics['uplink_rate'] = self._amount_packets_up / \ (time.time() - self._previous_time_stamp) - self.signal_health['uplink_congestion'] = 1.0 - self._amount_null_packets_up / self._amount_packets_up - self.signal_health['downlink_congestion'] = 1.0 - \ + self.radio_link_statistics['downlink_rate'] = self._amount_packets_down / \ + (time.time() - self._previous_time_stamp) + self.radio_link_statistics['uplink_congestion'] = 1.0 - \ + self._amount_null_packets_up / self._amount_packets_up + self.radio_link_statistics['downlink_congestion'] = 1.0 - \ self._amount_null_packets_down / self._amount_packets_down self._amount_packets_up = 0 diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 4ffd04ee3..a4d82ee19 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -52,7 +52,7 @@ from .crtpstack import CRTPPacket from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver -from cflib.crtp.signal_health import SignalHealth +from cflib.crtp.radio_link_statistics import RadioLinkStatistics from cflib.drivers.crazyradio import Crazyradio @@ -241,19 +241,19 @@ def __init__(self): self._radio = None self.uri = '' self.link_error_callback = None - self.signal_health_callback = None + self.radio_link_statistics_callback = None self.in_queue = None self.out_queue = None self._thread = None self.needs_resending = True - def connect(self, uri, signal_health_callback, link_error_callback): + def connect(self, uri, radio_link_statistics_callback, link_error_callback): """ Connect the link driver to a specified URI of the format: radio:////[250K,1M,2M] - The callback for signal health can be called at any moment from the - driver to report back the signal health. The callback from linkError + The callback for radio link statistics can be called at any moment from the + driver to report back the radio link statistics. The callback from linkError will be called when a error occurs with an error message. """ @@ -283,7 +283,7 @@ def connect(self, uri, signal_health_callback, link_error_callback): self._thread = _RadioDriverThread(self._radio, self.in_queue, self.out_queue, - signal_health_callback, + radio_link_statistics_callback, link_error_callback, self, rate_limit) @@ -381,7 +381,7 @@ def restart(self): self._thread = _RadioDriverThread(self._radio, self.in_queue, self.out_queue, - self.signal_health_callback, + self.radio_link_statistics_callback, self.link_error_callback, self) self._thread.start() @@ -401,7 +401,7 @@ def close(self): # Clear callbacks self.link_error_callback = None - self.signal_health_callback = None + self.radio_link_statistics_callback = None def _scan_radio_channels(self, radio: _SharedRadioInstance, start=0, stop=125): @@ -520,7 +520,7 @@ class _RadioDriverThread(threading.Thread): Crazyradio USB driver. """ def __init__(self, radio, inQueue, outQueue, - signal_health_callback, link_error_callback, link, rate_limit: Optional[int]): + radio_link_statistics_callback, link_error_callback, link, rate_limit: Optional[int]): """ Create the object """ threading.Thread.__init__(self) self._radio = radio @@ -528,7 +528,7 @@ def __init__(self, radio, inQueue, outQueue, self._out_queue = outQueue self._sp = False self._link_error_callback = link_error_callback - self._signal_health = SignalHealth(signal_health_callback) + self._radio_link_statistics = RadioLinkStatistics(radio_link_statistics_callback) self._retry_before_disconnect = _nr_of_retries self.rate_limit = rate_limit @@ -659,7 +659,7 @@ def run(self): # If no packet to send, send a null packet dataOut.append(0xFF) - self._signal_health.update(ackStatus, outPacket) + self._radio_link_statistics.update(ackStatus, outPacket) def set_retries_before_disconnect(nr_of_retries): diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index eb18fed27..349e58d5c 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -52,20 +52,19 @@ def __init__(self): self.cfusb = None self.uri = '' self.link_error_callback = None - self.signal_health_callback = None + self.radio_link_statistics_callback = None self.in_queue = None self.out_queue = None self._thread = None self.needs_resending = False - def connect(self, uri, signal_health_callback, link_error_callback): + def connect(self, uri, radio_link_statistics_callback, link_error_callback): """ Connect the link driver to a specified URI of the format: radio:////[250K,1M,2M] - The callback for signal health can be called at any moment from - the driver to report back the signal health. The callback from - linkError will be called when a error occurs with an error message. + The callback for radio link statistics should not be called from the usb driver + The callback from linkError will be called when a error occurs with an error message. """ # check if the URI is a radio URI @@ -99,7 +98,7 @@ def connect(self, uri, signal_health_callback, link_error_callback): # Launch the comm thread self._thread = _UsbReceiveThread(self.cfusb, self.in_queue, - signal_health_callback, + radio_link_statistics_callback, link_error_callback) self._thread.start() @@ -151,7 +150,7 @@ def restart(self): return self._thread = _UsbReceiveThread(self.cfusb, self.in_queue, - self.signal_health_callback, + self.radio_link_statistics_callback, self.link_error_callback) self._thread.start() @@ -207,7 +206,7 @@ class _UsbReceiveThread(threading.Thread): Radio link receiver thread used to read data from the Crazyradio USB driver. """ - def __init__(self, cfusb, inQueue, signal_health_callback, + def __init__(self, cfusb, inQueue, radio_link_statistics_callback, link_error_callback): """ Create the object """ threading.Thread.__init__(self) @@ -215,7 +214,7 @@ def __init__(self, cfusb, inQueue, signal_health_callback, self.in_queue = inQueue self.sp = False self.link_error_callback = link_error_callback - self.signal_health_callback = signal_health_callback + self.radio_link_statistics_callback = radio_link_statistics_callback def stop(self): """ Stop the thread """ diff --git a/sys_test/swarm_test_rig/test_response_time.py b/sys_test/swarm_test_rig/test_response_time.py index 582a9519f..40158239c 100644 --- a/sys_test/swarm_test_rig/test_response_time.py +++ b/sys_test/swarm_test_rig/test_response_time.py @@ -128,14 +128,14 @@ def _is_response_correct_seq_nr(self, response, seq_nr): return False def connect_link(self, uri): - link = cflib.crtp.get_link_driver(uri, self._signal_health_cb, + link = cflib.crtp.get_link_driver(uri, self._radio_link_statistics_cb, self._link_error_cb) self.assertIsNotNone(link) self.links.append(link) return link - def _signal_health_cb(self, signal_health): + def _radio_link_statistics_cb(self, radoi_link_statistics): pass def _link_error_cb(self, errmsg): From 8cec9b8d63ef34a41453314816637c94e1a06c90 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 24 Oct 2024 11:25:11 +0200 Subject: [PATCH 687/861] Switch to pyproject.toml for package configuration - Migrated from setup.py to pyproject.toml for better compliance with modern PEP - Identical package metadata, dependencies, and long description in the new configuration. - Enforce supported Python version --- pyproject.toml | 55 +++++++++++++++++++++++++++++++++++++++++++++++ setup.cfg | 2 -- setup.py | 55 ----------------------------------------------- tools/build/bdist | 2 +- 4 files changed, 56 insertions(+), 58 deletions(-) create mode 100644 pyproject.toml delete mode 100644 setup.cfg delete mode 100644 setup.py diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..486047402 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,55 @@ +[build-system] +requires = ["setuptools>=61.0", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "cflib" +version = "0.1.27" +description = "Crazyflie Python driver" +authors = [ + { name = "Bitcraze and contributors", email = "contact@bitcraze.io" }, +] + +readme = {file = "README.md", content-type = "text/markdown"} +license = { text = "GPLv3" } +keywords = ["driver", "crazyflie", "quadcopter"] + +classifiers = [ + "Development Status :: 4 - Beta", + "License :: OSI Approved :: GNU General Public License v3 (GPLv3)", + "Topic :: System :: Hardware :: Hardware Drivers", + + # Supported Python versions + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Programming Language :: Python :: 3.13", +] +requires-python = ">= 3.10" + +dependencies = [ + "pyusb>=1.0.0b2", + "libusb-package~=1.0", + "scipy~=1.7", + "numpy~=1.20", + "packaging~=24.0", +] + + +[project.urls] +Homepage = "bitcraze.io" +Documentation = "https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/" +Repository = "https://github.com/bitcraze/crazyflie-lib-python" +Issues = "https://github.com/bitcraze/crazyflie-lib-python/issues" + +[project.optional-dependencies] +dev = ["pre-commit"] + +[tool.setuptools] +include-package-data = true + +[tool.setuptools.packages] +find = { exclude = ["examples", "test"] } + +[tool.setuptools.package-data] +"cflib.resources.binaries" = ["cflib/resources/binaries/*.bin"] diff --git a/setup.cfg b/setup.cfg deleted file mode 100644 index aa76baec6..000000000 --- a/setup.cfg +++ /dev/null @@ -1,2 +0,0 @@ -[bdist_wheel] -universal=0 diff --git a/setup.py b/setup.py deleted file mode 100644 index 096f1f87e..000000000 --- a/setup.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python3 -from pathlib import Path - -from setuptools import find_packages -from setuptools import setup -# read the contents of README.md file fo use in pypi description -directory = Path(__file__).parent -long_description = (directory / 'README.md').read_text() - -package_data = { - 'cflib.resources.binaries': ['cflib/resources/binaries/*.bin'], -} - -setup( - name='cflib', - version='0.1.27', - packages=find_packages(exclude=['examples', 'test']), - - description='Crazyflie python driver', - url='https://github.com/bitcraze/crazyflie-lib-python', - - long_description=long_description, - long_description_content_type='text/markdown', - - author='Bitcraze and contributors', - author_email='contact@bitcraze.io', - license='GPLv3', - - classifiers=[ - 'Development Status :: 4 - Beta', - 'License :: OSI Approved :: GNU General Public License v3 (GPLv3)', - 'Topic :: System :: Hardware :: Hardware Drivers', - 'Programming Language :: Python :: 3' - ], - - keywords='driver crazyflie quadcopter', - - install_requires=[ - 'pyusb>=1.0.0b2', - 'libusb-package~=1.0', - 'scipy~=1.7', - 'numpy~=1.20', - 'packaging~=24.0', - ], - - # $ pip install -e .[dev] - extras_require={ - 'dev': [ - 'pre-commit' - ], - }, - - include_package_data=True, - package_data=package_data -) diff --git a/tools/build/bdist b/tools/build/bdist index c0bc8dc31..40a9acd04 100755 --- a/tools/build/bdist +++ b/tools/build/bdist @@ -9,7 +9,7 @@ try: script_dir = os.path.dirname(os.path.realpath(__file__)) root = _path.normpath(_path.join(script_dir, '../..')) - subprocess.check_call(['python3', 'setup.py', 'bdist_wheel'], cwd=root) + subprocess.check_call(['python3', '-m', 'build', '--wheel'], cwd=root) print('Wheel built') except subprocess.CalledProcessError as e: From b03d305f44b7b41ee987723bb5be30d096b39ab7 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 24 Oct 2024 14:22:59 +0200 Subject: [PATCH 688/861] Support updated web-builder with venv --- tools/build-docs/build-docs | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs index 05c97dcb6..7060ccb0d 100755 --- a/tools/build-docs/build-docs +++ b/tools/build-docs/build-docs @@ -11,9 +11,6 @@ tempDir=$scriptDir/../../.tmp mkdir -p $apiRefDir mkdir -p $tempDir -# This hack is needed to find --user installed modules -export PYTHONPATH=$PYTHONPATH:$scriptDir/.local/lib/python3.7/site-packages - [ -n "$API_REF_BASE" ] || { export API_REF_BASE=$(readlink -fn $tempDir) } @@ -27,8 +24,8 @@ export PYTHONPATH=$PYTHONPATH:$scriptDir/.local/lib/python3.7/site-packages # # Make a temporary install in the temp dir -HOME=$tempDir pip3 install --user --upgrade pip -HOME=$tempDir pip3 install --user -e $scriptDir/../../ +HOME=$tempDir pip3 install --upgrade pip +HOME=$tempDir pip3 install -e $scriptDir/../../ # Generate documentation HOME=$tempDir pdoc3 --force $cflibDir --output-dir $tempDir --template-dir $templateDir From fe7beba3031a2d72d6513dfa1fe2763f0a7420ee Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Fri, 25 Oct 2024 15:05:04 +0200 Subject: [PATCH 689/861] Use assertEqual instead of deprecated assertEquals --- test/utils/test_param_file_helper.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/utils/test_param_file_helper.py b/test/utils/test_param_file_helper.py index a06a8c953..4f8666cb2 100644 --- a/test/utils/test_param_file_helper.py +++ b/test/utils/test_param_file_helper.py @@ -116,5 +116,5 @@ def mock_wait(self, timeout=None): with patch.object(Event, 'wait', new=mock_wait): # Test and Assert self.assertTrue(helper.store_params_from_file('test/utils/fixtures/five_params.yaml')) - self.assertEquals(5, len(mock_Param.set_value.mock_calls)) - self.assertEquals(5, len(mock_Param.persistent_store.mock_calls)) + self.assertEqual(5, len(mock_Param.set_value.mock_calls)) + self.assertEqual(5, len(mock_Param.persistent_store.mock_calls)) From 31ff8e44ac5a45f27426547fa2c1c869b644f007 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 12 Nov 2024 12:51:08 +0100 Subject: [PATCH 690/861] Rename latency update callback, reduce ping interval to 0.1 seconds --- cflib/crazyflie/link_statistics.py | 8 ++++---- examples/link_quality/latency.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py index 1c536fdac..af23f3b56 100644 --- a/cflib/crazyflie/link_statistics.py +++ b/cflib/crazyflie/link_statistics.py @@ -97,7 +97,7 @@ def __init__(self, crazyflie): self._stop_event = Event() self._ping_thread_instance = None self.latency = 0 - self.latencyUpdated = Caller() + self.latency_updated = Caller() def start(self): """ @@ -123,7 +123,7 @@ def stop(self): self._ping_thread_instance.join() self._ping_thread_instance = None - def _ping_thread(self, interval: float = 1.0) -> None: + def _ping_thread(self, interval: float = 0.1) -> None: """ Background thread method that sends a ping to the Crazyflie at regular intervals. @@ -131,7 +131,7 @@ def _ping_thread(self, interval: float = 1.0) -> None: until the stop event is set. Args: - interval (float): The time (in seconds) to wait between ping requests. Default is 1 second. + interval (float): The time (in seconds) to wait between ping requests. Default is 0.1 seconds. """ while not self._stop_event.is_set(): self.ping() @@ -169,7 +169,7 @@ def _ping_response(self, packet): if received_header != PING_HEADER: return self.latency = self._calculate_p95_latency(received_timestamp) - self.latencyUpdated.call(self.latency) + self.latency_updated.call(self.latency) def _calculate_p95_latency(self, timestamp): """ diff --git a/examples/link_quality/latency.py b/examples/link_quality/latency.py index b73c92b72..21faf8aa0 100644 --- a/examples/link_quality/latency.py +++ b/examples/link_quality/latency.py @@ -45,7 +45,7 @@ def latency_callback(latency: float): # Add a callback to whenever we receive an updated latency estimate # # This could also be a Python lambda, something like: - cf.link_statistics.latency.latencyUpdated.add_callback(latency_callback) + 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: From 1585ee7b93c87aafefce3fa8f2f06227022512a2 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 18 Nov 2024 14:03:53 +0100 Subject: [PATCH 691/861] Update expected inputs in test-python-publish.yml --- .github/workflows/test-python-publish.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index 444e635f2..cb9dd7fb5 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -34,6 +34,6 @@ jobs: - name: Publish package to TestPyPI uses: pypa/gh-action-pypi-publish@release/v1 with: - username: __token__ + user: __token__ password: ${{ secrets.PYPI_TEST_TOKEN }} - repository_url: https://test.pypi.org/legacy/ + repository-url: https://test.pypi.org/legacy/ From 7323c09c06d07a70217110736c70c698ec453952 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 18 Nov 2024 14:19:10 +0100 Subject: [PATCH 692/861] Trusted publish to test pypi --- .github/workflows/test-python-publish.yml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index cb9dd7fb5..77f81444e 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -18,6 +18,11 @@ jobs: deploy: runs-on: ubuntu-latest + environment: + name: pypi-test + url: https://pypi.org/p/cflib + permissions: + id-token: write steps: - uses: actions/checkout@v4 @@ -34,6 +39,4 @@ jobs: - name: Publish package to TestPyPI uses: pypa/gh-action-pypi-publish@release/v1 with: - user: __token__ - password: ${{ secrets.PYPI_TEST_TOKEN }} repository-url: https://test.pypi.org/legacy/ From 8fe7f726465c2b9413a7dcb2962e63ff7c9ed036 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 18 Nov 2024 14:46:29 +0100 Subject: [PATCH 693/861] Update test-python-publish.yml --- .github/workflows/test-python-publish.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index 77f81444e..947a42101 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -40,3 +40,4 @@ jobs: uses: pypa/gh-action-pypi-publish@release/v1 with: repository-url: https://test.pypi.org/legacy/ + verbose: true From efcd52dfcd094933a12c5835d05a22d9e6f76016 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 18 Nov 2024 14:55:51 +0100 Subject: [PATCH 694/861] Update pyproject.toml --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 486047402..16573a991 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "cflib" -version = "0.1.27" +version = "0.1.27.dev0" description = "Crazyflie Python driver" authors = [ { name = "Bitcraze and contributors", email = "contact@bitcraze.io" }, From 1319daa95713c9b44040f52584b77784a0a76a8c Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 18 Nov 2024 14:58:27 +0100 Subject: [PATCH 695/861] Update homepage --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 16573a991..46dbea9e6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -37,7 +37,7 @@ dependencies = [ [project.urls] -Homepage = "bitcraze.io" +Homepage = "https://www.bitcraze.io" Documentation = "https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/" Repository = "https://github.com/bitcraze/crazyflie-lib-python" Issues = "https://github.com/bitcraze/crazyflie-lib-python/issues" From dc658aa611b1cadb9a8852ac3853389e3dda15a8 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 18 Nov 2024 15:06:20 +0100 Subject: [PATCH 696/861] Update version to 0.1.27.1.dev0 0.1.27.dev0 is considered < 1.1.27 breaking compatibility with latest cfclient release New version is considered pre-release of 0.1.27.1 --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 46dbea9e6..c6c3c4d37 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "cflib" -version = "0.1.27.dev0" +version = "0.1.27.1.dev0" description = "Crazyflie Python driver" authors = [ { name = "Bitcraze and contributors", email = "contact@bitcraze.io" }, From 6d8d974da592b3ef7431761bff5ecb799b62c69d Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 18 Nov 2024 15:13:41 +0100 Subject: [PATCH 697/861] Trusted publish to pypi --- .github/workflows/python-publish.yml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml index 0dc3d068c..d5f2232b1 100644 --- a/.github/workflows/python-publish.yml +++ b/.github/workflows/python-publish.yml @@ -18,6 +18,11 @@ jobs: deploy: runs-on: ubuntu-latest + environment: + name: pypi + url: https://pypi.org/p/cflib + permissions: + id-token: write steps: - uses: actions/checkout@v4 @@ -34,5 +39,4 @@ jobs: - name: Publish package uses: pypa/gh-action-pypi-publish@release/v1 with: - username: __token__ - password: ${{ secrets.PYPI_TOKEN }} + verbose: true From 608f5f0f43498366cedc8ff18e571e7c65fd5a9b Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 20 Nov 2024 11:18:14 +0100 Subject: [PATCH 698/861] Update dependencies, metadata --- pyproject.toml | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index c6c3c4d37..e0d5f8d00 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -15,9 +15,16 @@ license = { text = "GPLv3" } keywords = ["driver", "crazyflie", "quadcopter"] classifiers = [ - "Development Status :: 4 - Beta", + "Development Status :: 5 - Production/Stable", "License :: OSI Approved :: GNU General Public License v3 (GPLv3)", "Topic :: System :: Hardware :: Hardware Drivers", + "Topic :: Scientific/Engineering :: Robotics", + "Intended Audience :: Science/Research", + "Intended Audience :: Education", + "Intended Audience :: Developers", + "Operating System :: Linux", + "Operating System :: MacOS", + "Operating System :: Microsoft :: Windows", # Supported Python versions "Programming Language :: Python :: 3.10", @@ -28,14 +35,13 @@ classifiers = [ requires-python = ">= 3.10" dependencies = [ - "pyusb>=1.0.0b2", + "pyusb~=1.2", "libusb-package~=1.0", - "scipy~=1.7", - "numpy~=1.20", - "packaging~=24.0", + "scipy~=1.14", + "numpy~=1.26", + "packaging~=24.2", ] - [project.urls] Homepage = "https://www.bitcraze.io" Documentation = "https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/" From 52bff9ddc07d1b04c757d302ce3821b87999a2bc Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 20 Nov 2024 11:18:29 +0100 Subject: [PATCH 699/861] Include Python 3.13 installation instructions --- docs/installation/install.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/installation/install.md b/docs/installation/install.md index f8c696dda..a505d74e3 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -7,6 +7,7 @@ page_id: install This project requires Python 3.10+. +To install on Python 3.13, build tools and Python development headers are required. See below sections for more platform-specific requirements. ## Install from Source @@ -78,6 +79,8 @@ With linux, the crazyradio is easily recognized, but you have to setup UDEVpermi Look at the [Zadig crazyradio instructions](https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/usbwindows/) to install crazyradio on Windows +If you're using Python 3.13, you need to install [Visual Studio](https://visualstudio.microsoft.com/downloads/). During the installation process, you only need to select the Desktop Development with C++ workload in the Visual Studio Installer. + ### macOS If you are using python 3.12 on mac you need to install libusb using homebrew. ``` From 95ced817175552a9ad947eba35f4b615b1cadf4e Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 20 Nov 2024 11:25:05 +0100 Subject: [PATCH 700/861] Automatic versioning Identical to cfclient --- pyproject.toml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index e0d5f8d00..6677f6f41 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,10 +1,10 @@ [build-system] -requires = ["setuptools>=61.0", "wheel"] +requires = ["setuptools>=61.0", "wheel", "setuptools_scm"] build-backend = "setuptools.build_meta" [project] name = "cflib" -version = "0.1.27.1.dev0" +dynamic = ["version"] description = "Crazyflie Python driver" authors = [ { name = "Bitcraze and contributors", email = "contact@bitcraze.io" }, @@ -59,3 +59,6 @@ find = { exclude = ["examples", "test"] } [tool.setuptools.package-data] "cflib.resources.binaries" = ["cflib/resources/binaries/*.bin"] + +[tool.setuptools_scm] +version_scheme = "no-guess-dev" From 16528967435b08d46b2b4ba0ef0806af1331590b Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 20 Nov 2024 12:01:43 +0100 Subject: [PATCH 701/861] Support auto versioning in GitHub publishing workflows --- .github/workflows/python-publish.yml | 2 ++ .github/workflows/test-python-publish.yml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml index d5f2232b1..0e197504e 100644 --- a/.github/workflows/python-publish.yml +++ b/.github/workflows/python-publish.yml @@ -26,6 +26,8 @@ jobs: steps: - uses: actions/checkout@v4 + with: + fetch-depth: 0 - name: Set up Python uses: actions/setup-python@v4 with: diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml index 947a42101..2b4851bd3 100644 --- a/.github/workflows/test-python-publish.yml +++ b/.github/workflows/test-python-publish.yml @@ -26,6 +26,8 @@ jobs: steps: - uses: actions/checkout@v4 + with: + fetch-depth: 0 - name: Set up Python uses: actions/setup-python@v4 with: From 4fc747d76f20bd375f5027d3ffe9478b188a136a Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Wed, 20 Nov 2024 14:29:27 +0100 Subject: [PATCH 702/861] Run dependency check on all supported Python versions --- .github/workflows/dependency_check.yml | 45 +++++++------------------- 1 file changed, 12 insertions(+), 33 deletions(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index f2d09c69d..e18b680de 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -6,44 +6,23 @@ on: - cron: '0 2 * * *' jobs: - python-latest: + python-check: + strategy: + matrix: + python-version: [3.10, 3.11, 3.12, 3.13] runs-on: ubuntu-latest - container: - image: python:latest steps: - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 - - name: python version - run: python --version - - - name: install lib - run: pip install -e . - - python-python311: - runs-on: ubuntu-latest - container: - image: python:3.11 - - steps: - - uses: actions/checkout@v4 - - - name: python version - run: python --version - - - name: install lib - run: pip install -e . - - minimum-python310: - runs-on: ubuntu-latest - container: - image: python:3.10 - - steps: - - uses: actions/checkout@v4 + - name: Setup Python + uses: actions/setup-python@v4 + with: + python-version: ${{ matrix.python-version }} - - name: python version + - name: Verify Python version run: python --version - - name: install lib + - name: Install library run: pip install -e . From b2772f099dabb7fae5aa8e73fd6e284ab7712299 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Wed, 20 Nov 2024 14:35:11 +0100 Subject: [PATCH 703/861] Run dependency check weekly on Friday at 02:00 --- .github/workflows/dependency_check.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index e18b680de..8f9dbf3f0 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -3,7 +3,7 @@ name: Dependency check on: workflow_dispatch: schedule: - - cron: '0 2 * * *' + - cron: '0 2 * * 5' jobs: python-check: From 840d891c7214f2616b0805b691f4293a461f20f9 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Wed, 20 Nov 2024 14:48:03 +0100 Subject: [PATCH 704/861] Introduce nightly build and test On all supported OSes and Python versions --- .github/workflows/nightly.yml | 37 +++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 .github/workflows/nightly.yml diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml new file mode 100644 index 000000000..b18984b2a --- /dev/null +++ b/.github/workflows/nightly.yml @@ -0,0 +1,37 @@ +# Run check and build of the lib using the Bitcraze builder docker image +name: Nightly Build + +on: + workflow_dispatch: + schedule: + - cron: '0 2 * * *' + +jobs: + build-and-test: + strategy: + matrix: + os: [ubuntu-latest, macos-latest, windows-latest] + python-version: [3.10, 3.11, 3.12, 3.13] + + runs-on: ${{ matrix.os }} + + steps: + - name: Checkout repo + uses: actions/checkout@v4 + + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.python-version }} + + - name: Verify + run: ./tools/build/verify + + - name: Test + run: ./tools/build/test + + - name: Build + run: ./tools/build/build + + - name: Build docs + run: ./tools/build-docs/build-docs From a59ff760ee3e39c1f494454a2f5cc46f94f466c7 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Wed, 20 Nov 2024 14:50:58 +0100 Subject: [PATCH 705/861] Use self-hosted MacOS runner --- .github/workflows/nightly.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index b18984b2a..d26e3b8d5 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -10,7 +10,7 @@ jobs: build-and-test: strategy: matrix: - os: [ubuntu-latest, macos-latest, windows-latest] + os: [ubuntu-latest, lab-mac, windows-latest] python-version: [3.10, 3.11, 3.12, 3.13] runs-on: ${{ matrix.os }} From e5048d46f1e66d18df8e1f9b50ec7523b1ba1714 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Wed, 20 Nov 2024 14:52:47 +0100 Subject: [PATCH 706/861] Update nightly.yml --- .github/workflows/nightly.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index d26e3b8d5..5fc1c9b03 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -9,6 +9,7 @@ on: jobs: build-and-test: strategy: + fail-fast: false matrix: os: [ubuntu-latest, lab-mac, windows-latest] python-version: [3.10, 3.11, 3.12, 3.13] From 2866ce02e0523e7054902879abbeb5adabd0d26d Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 20 Nov 2024 15:09:08 +0100 Subject: [PATCH 707/861] Install required dependencies for nightly. Run same script as docker does. --- .github/workflows/nightly.yml | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index 5fc1c9b03..3cace8d8f 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -12,10 +12,10 @@ jobs: fail-fast: false matrix: os: [ubuntu-latest, lab-mac, windows-latest] - python-version: [3.10, 3.11, 3.12, 3.13] - + python-version: ["3.10", "3.11", "3.12", "3.13"] + runs-on: ${{ matrix.os }} - + steps: - name: Checkout repo uses: actions/checkout@v4 @@ -25,13 +25,12 @@ jobs: with: python-version: ${{ matrix.python-version }} - - name: Verify - run: ./tools/build/verify - - - name: Test - run: ./tools/build/test + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install build pre-commit - - name: Build + - name: Verify, test, and build run: ./tools/build/build - name: Build docs From 83e78ce68d3bfd14e44435249635e99ade18ed48 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 20 Nov 2024 15:20:55 +0100 Subject: [PATCH 708/861] First build to fetch dependencies --- .github/workflows/nightly.yml | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index 3cace8d8f..4b2ef6be9 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -30,8 +30,14 @@ jobs: python -m pip install --upgrade pip pip install build pre-commit - - name: Verify, test, and build - run: ./tools/build/build + - name: Build + run: ./tools/build/bdist + + - name: Test + run: ./tools/build/test + + - name: Verify + run: ./tools/build/verify - name: Build docs run: ./tools/build-docs/build-docs From c1e97cfa868150c46f047b8872d61a344e1c6bc6 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 21 Nov 2024 10:01:43 +0100 Subject: [PATCH 709/861] Nightly: build wheel, pip install it. First run pre-commit checks. Build docs once. --- .github/workflows/nightly.yml | 36 +++++++++++++++++++++++++---------- 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index 4b2ef6be9..5dc87a18d 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -27,17 +27,33 @@ jobs: - name: Install dependencies run: | - python -m pip install --upgrade pip - pip install build pre-commit + python3 -m pip install --upgrade pip build setuptools + python3 -m pip install pre-commit - - name: Build - run: ./tools/build/bdist + - name: Code quality checks + run: pre-commit run --all-files - - name: Test - run: ./tools/build/test + - name: Build wheel + run: python3 -m build --wheel + + - name: Install wheel + run: pip install ./dist/*.whl - - name: Verify - run: ./tools/build/verify + - name: Test + run: python3 -m unittest discover test/ - - name: Build docs - run: ./tools/build-docs/build-docs + build-docs: + runs-on: ubuntu-latest + steps: + - name: Checkout repo + uses: actions/checkout@v4 + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: '3.x' + - name: Install dependencies + run: | + python3 -m pip install --upgrade pip + python3 -m pip install pdoc3 + - name: Build docs + run: ./tools/build-docs/build-docs From bdffcc869da41b1fa60178aa6f27cd6069531521 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 21 Nov 2024 10:21:07 +0100 Subject: [PATCH 710/861] Install setuptools, yaml for generating docs --- .github/workflows/nightly.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index 5dc87a18d..7e90600e6 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -53,7 +53,7 @@ jobs: python-version: '3.x' - name: Install dependencies run: | - python3 -m pip install --upgrade pip - python3 -m pip install pdoc3 + python3 -m pip install --upgrade pip setuptools + python3 -m pip install pdoc3 pyyaml - name: Build docs run: ./tools/build-docs/build-docs From 5d0ccc783cf37ed1ceac9eff73b21469dfe84f38 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 21 Nov 2024 10:21:24 +0100 Subject: [PATCH 711/861] Find wheel on Windows --- .github/workflows/nightly.yml | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index 7e90600e6..fca136120 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -36,8 +36,22 @@ jobs: - name: Build wheel run: python3 -m build --wheel - - name: Install wheel - run: pip install ./dist/*.whl + - name: Install the built wheel + run: | + # Find the built wheel + WHEEL_FILE=$(ls dist/*.whl | head -n 1) + echo "Installing wheel: $WHEEL_FILE" + pip install "$WHEEL_FILE" + shell: bash + if: runner.os != 'Windows' + + - name: Install the built wheel (Windows) + run: | + for /f %%i in ('dir /b dist\*.whl') do set WHEEL_FILE=dist\%%i + echo Installing wheel: %WHEEL_FILE% + pip install %WHEEL_FILE% + shell: cmd + if: runner.os == 'Windows' - name: Test run: python3 -m unittest discover test/ From 14cf8b55b6fddfbeb3324533d7f824b43fe08a1f Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 21 Nov 2024 11:30:19 +0100 Subject: [PATCH 712/861] Use Brew Python on MacOs --- .github/workflows/nightly.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index fca136120..95dde3c57 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -21,10 +21,18 @@ jobs: uses: actions/checkout@v4 - name: Set up Python ${{ matrix.python-version }} + if: runner.os == 'Linux' || runner.os == 'Windows' uses: actions/setup-python@v5 with: python-version: ${{ matrix.python-version }} + - name: Set up Python ${{ matrix.python-version }} and venv on macOS + if: runner.os == 'macOS' + run: | + brew install python@${{ matrix.python-version }} + $(brew --prefix)/bin/python${{ matrix.python-version }} -m venv venv + echo "PATH=$(pwd)/venv/bin:$PATH" >> $GITHUB_ENV + - name: Install dependencies run: | python3 -m pip install --upgrade pip build setuptools From ec74aa48c4006fedee637fc9c612d66c829f282f Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Thu, 21 Nov 2024 15:29:13 +0100 Subject: [PATCH 713/861] Use Visual Studio for compiling wheels --- .github/workflows/nightly.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index 95dde3c57..cf3ba9f9c 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -33,6 +33,12 @@ jobs: $(brew --prefix)/bin/python${{ matrix.python-version }} -m venv venv echo "PATH=$(pwd)/venv/bin:$PATH" >> $GITHUB_ENV + - name: Set up MSVC environment (Windows) + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x64 + if: runner.os == 'Windows' + - name: Install dependencies run: | python3 -m pip install --upgrade pip build setuptools From 02cfb058e6c5dddeb036280cd4a8fc646e5b29ae Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Fri, 22 Nov 2024 11:31:54 +0100 Subject: [PATCH 714/861] Update dependency_check.yml --- .github/workflows/dependency_check.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml index 8f9dbf3f0..89d693aa1 100644 --- a/.github/workflows/dependency_check.yml +++ b/.github/workflows/dependency_check.yml @@ -9,7 +9,7 @@ jobs: python-check: strategy: matrix: - python-version: [3.10, 3.11, 3.12, 3.13] + python-version: ["3.10", "3.11", "3.12", "3.13"] runs-on: ubuntu-latest steps: From 580903f56c87161da810e102276d2451bb6748f7 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Fri, 22 Nov 2024 11:33:47 +0100 Subject: [PATCH 715/861] Remove now redundant dependency check This dependency check is now redundant with the nightly build --- .github/workflows/dependency_check.yml | 28 -------------------------- 1 file changed, 28 deletions(-) delete mode 100644 .github/workflows/dependency_check.yml diff --git a/.github/workflows/dependency_check.yml b/.github/workflows/dependency_check.yml deleted file mode 100644 index 89d693aa1..000000000 --- a/.github/workflows/dependency_check.yml +++ /dev/null @@ -1,28 +0,0 @@ -name: Dependency check - -on: - workflow_dispatch: - schedule: - - cron: '0 2 * * 5' - -jobs: - python-check: - strategy: - matrix: - python-version: ["3.10", "3.11", "3.12", "3.13"] - runs-on: ubuntu-latest - - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Setup Python - uses: actions/setup-python@v4 - with: - python-version: ${{ matrix.python-version }} - - - name: Verify Python version - run: python --version - - - name: Install library - run: pip install -e . From 1d4c5e83a225d7f46956b25bf69899ebd8849b72 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 28 Nov 2024 11:11:18 +0100 Subject: [PATCH 716/861] Remove deprecated call to setDaemon --- cflib/crazyflie/__init__.py | 2 +- cflib/crazyflie/param.py | 4 ++-- cflib/crtp/radiodriver.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index cb9a0eed4..06f5e2e6d 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -109,7 +109,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): rw_cache=rw_cache) self.incoming = _IncomingPacketHandler(self) - self.incoming.setDaemon(True) + self.incoming.daemon = True if self.link: self.incoming.start() diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 1dc199e8f..25ea37a0b 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -520,7 +520,7 @@ class _ExtendedTypeFetcher(Thread): def __init__(self, cf, toc): Thread.__init__(self) - self.setDaemon(True) + self.daemon = True self._lock = Lock() self._cf = cf @@ -599,7 +599,7 @@ class _ParamUpdater(Thread): def __init__(self, cf, useV2, updated_callback): """Initialize the thread""" Thread.__init__(self) - self.setDaemon(True) + self.daemon = True self.wait_lock = Lock() self.cf = cf self._useV2 = useV2 diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 7d22b6c52..2d805f7c1 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -156,7 +156,7 @@ def __init__(self, devid: int): self._lock = Semaphore(1) - self.setDaemon(True) + self.daemon = True self.start() From f9dd6f5e4f527ad648607f091a6b3e78ef1fbf19 Mon Sep 17 00:00:00 2001 From: Gadgeteer Date: Sun, 1 Dec 2024 12:38:42 +0000 Subject: [PATCH 717/861] Add serial reader Add serial reader to the log and remove N/A --- cflib/crtp/radiodriver.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 7d22b6c52..1bd4bb605 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -156,7 +156,7 @@ def __init__(self, devid: int): self._lock = Semaphore(1) - self.setDaemon(True) + self.daemon = True self.start() @@ -455,12 +455,15 @@ def scan_interface(self, address): except Exception as e: print(e) return [] - - # FIXME: implements serial number in the Crazyradio driver! - serial = 'N/A' + try: + serial = crazyradio.get_serials() + except Exception as e: + print(e) + serial = 'N/A' + logger.info('v%s dongle with serial %s found', self._radio.version, - serial) + serial[0]) found = [] if address is not None: From aed7950b18e764304a3cbeb4e365c4b197014404 Mon Sep 17 00:00:00 2001 From: Gageteering Date: Wed, 4 Dec 2024 10:24:10 +0000 Subject: [PATCH 718/861] pre-commit check --- cflib/crtp/radiodriver.py | 1 - 1 file changed, 1 deletion(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 1bd4bb605..52095ad04 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -460,7 +460,6 @@ def scan_interface(self, address): except Exception as e: print(e) serial = 'N/A' - logger.info('v%s dongle with serial %s found', self._radio.version, serial[0]) From 29146f622b6a3476634176797c0b1738d899ce34 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 4 Dec 2024 14:35:22 +0100 Subject: [PATCH 719/861] Manually arm Crazyflies in all examples Disarming is automatic after flight. --- examples/aideck/fpv.py | 5 +++++ examples/autonomy/autonomousSequence.py | 4 ++++ .../autonomy/autonomous_sequence_high_level.py | 4 ++++ .../autonomous_sequence_high_level_compressed.py | 4 ++++ examples/autonomy/circling_square_demo.py | 4 ++++ examples/autonomy/full_state_setpoint_demo.py | 4 ++++ examples/autonomy/motion_commander_demo.py | 4 ++++ examples/autonomy/position_commander_demo.py | 14 ++++++++++++++ examples/lighthouse/lighthouse_openvr_grab.py | 5 +++++ .../lighthouse/lighthouse_openvr_grab_color.py | 5 +++++ examples/lighthouse/lighthouse_openvr_multigrab.py | 6 ++++++ examples/mocap/mocap_hl_commander.py | 5 +++++ examples/mocap/qualisys_hl_commander.py | 5 +++++ examples/motors/multiramp.py | 4 ++++ examples/motors/ramp.py | 6 +++++- examples/multiranger/multiranger_pointcloud.py | 5 +++++ examples/multiranger/multiranger_push.py | 4 ++++ examples/multiranger/multiranger_wall_following.py | 4 ++++ examples/positioning/flowsequenceSync.py | 4 ++++ examples/positioning/initial_position.py | 4 ++++ examples/positioning/matrix_light_printer.py | 4 ++++ examples/step-by-step/sbs_swarm.py | 6 ++++++ examples/swarm/hl-commander-swarm.py | 6 ++++++ examples/swarm/swarmSequence.py | 6 ++++++ examples/swarm/swarmSequenceCircle.py | 4 ++++ examples/swarm/synchronizedSequence.py | 12 ++++++++++++ 26 files changed, 137 insertions(+), 1 deletion(-) diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 8a00f52a0..bea7924de 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -48,6 +48,7 @@ import struct import sys import threading +from time import time import numpy as np @@ -185,6 +186,10 @@ def __init__(self, URI): self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} + # Arm the Crazyflie + self.cf.platform.send_arming_request(True) + time.sleep(1.0) + self.hoverTimer = QtCore.QTimer() self.hoverTimer.timeout.connect(self.sendHoverCommand) self.hoverTimer.setInterval(100) diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index 48f3047c6..da6cb9a7c 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -127,6 +127,10 @@ def start_position_printing(scf): def run_sequence(scf, sequence): 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)) for i in range(50): diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 1880bb767..6bf737e4f 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -140,6 +140,10 @@ def upload_trajectory(cf, trajectory_id, trajectory): 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 diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index 7aea72cba..b28059a5b 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -137,6 +137,10 @@ def upload_trajectory(cf, trajectory_id, trajectory): 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 diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index 88f2ed495..6cc58ff45 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -162,6 +162,10 @@ def turn_off_leds(scf): 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 diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 9b6ea00c6..0eaac6a32 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -134,6 +134,10 @@ def run_sequence(scf): # 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], diff --git a/examples/autonomy/motion_commander_demo.py b/examples/autonomy/motion_commander_demo.py index 1f34e178a..da86b55b9 100644 --- a/examples/autonomy/motion_commander_demo.py +++ b/examples/autonomy/motion_commander_demo.py @@ -52,6 +52,10 @@ cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + # We take off when the commander is created with MotionCommander(scf) as mc: time.sleep(1) diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py index 6908138fc..b8597e9d8 100644 --- a/examples/autonomy/position_commander_demo.py +++ b/examples/autonomy/position_commander_demo.py @@ -29,6 +29,8 @@ Change the URI variable to your Crazyflie configuration. """ +from time import time + import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie @@ -41,6 +43,10 @@ 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, @@ -67,6 +73,10 @@ def slightly_more_complex_usage(): 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, @@ -80,6 +90,10 @@ def land_on_elevated_surface(): 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) diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py index 63ee8ec7e..8aff29538 100644 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ b/examples/lighthouse/lighthouse_openvr_grab.py @@ -171,6 +171,11 @@ def run_sequence(scf): 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 index 765b06be0..6e8abef55 100644 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ b/examples/lighthouse/lighthouse_openvr_grab_color.py @@ -203,6 +203,11 @@ def run_sequence(scf): 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 index d4d2377f2..9948e359c 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -199,6 +199,12 @@ def run_sequence(scf0, scf1): 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/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py index 2a73fb455..3d9f0ea13 100644 --- a/examples/mocap/mocap_hl_commander.py +++ b/examples/mocap/mocap_hl_commander.py @@ -233,6 +233,11 @@ def run_sequence(cf, trajectory_id, duration): 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/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py index b46d369c1..d2dcb301f 100644 --- a/examples/mocap/qualisys_hl_commander.py +++ b/examples/mocap/qualisys_hl_commander.py @@ -296,6 +296,11 @@ def run_sequence(cf, trajectory_id, duration): 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 index 5e89bb1e8..df77fec51 100644 --- a/examples/motors/multiramp.py +++ b/examples/motors/multiramp.py @@ -59,6 +59,10 @@ 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() diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index d17261d4a..a91d87bac 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -60,6 +60,10 @@ 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() @@ -98,7 +102,7 @@ def _ramp_motors(self): self._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) + time.sleep(1) self._cf.close_link() diff --git a/examples/multiranger/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py index 3c42a41f9..837c7f570 100644 --- a/examples/multiranger/multiranger_pointcloud.py +++ b/examples/multiranger/multiranger_pointcloud.py @@ -50,6 +50,7 @@ import logging import math import sys +from time import time import numpy as np from vispy import scene @@ -111,6 +112,10 @@ def __init__(self, URI): # 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() diff --git a/examples/multiranger/multiranger_push.py b/examples/multiranger/multiranger_push.py index 9905d6472..491f30038 100755 --- a/examples/multiranger/multiranger_push.py +++ b/examples/multiranger/multiranger_push.py @@ -74,6 +74,10 @@ def is_close(range): 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 diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index 710f9b6c1..d54d4a871 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -82,6 +82,10 @@ def handle_range_measurement(range): 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: diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py index 42d7a691f..24c0bd939 100644 --- a/examples/positioning/flowsequenceSync.py +++ b/examples/positioning/flowsequenceSync.py @@ -54,6 +54,10 @@ cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) + # 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) diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index d2119c635..b0c0274ce 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -118,6 +118,10 @@ def reset_estimator(scf): 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)) diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index 8aa37b447..4de1d7f02 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -110,5 +110,9 @@ def matrix_print(cf, pc, image_def): 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/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py index 7332950e9..3cdfa2d9f 100644 --- a/examples/step-by-step/sbs_swarm.py +++ b/examples/step-by-step/sbs_swarm.py @@ -44,6 +44,11 @@ def light_check(scf: SyncCrazyflie): time.sleep(2) +def arm(scf: SyncCrazyflie): + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + + def take_off(scf: SyncCrazyflie): commander = scf.cf.high_level_commander @@ -161,6 +166,7 @@ def run_sequence(scf: SyncCrazyflie, sequence): swarm.reset_estimators() print('Estimators have been reset') + swarm.parallel_safe(arm) swarm.parallel_safe(take_off) # swarm.parallel_safe(run_square_sequence) swarm.parallel_safe(run_sequence, args_dict=seq_args) diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index 346d21aeb..662d19317 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -44,6 +44,11 @@ def activate_mellinger_controller(scf, use_mellinger): 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) @@ -84,4 +89,5 @@ def run_shared_sequence(scf): 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/swarmSequence.py b/examples/swarm/swarmSequence.py index dea78169c..a3d5e9e2d 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -171,6 +171,11 @@ def wait_for_param_download(scf): 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 @@ -209,6 +214,7 @@ def run_sequence(scf, sequence): try: cf = scf.cf + arm(cf) take_off(cf, sequence[0]) for position in sequence: print('Setting position {}'.format(position)) diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index b80e176ad..90835e3ff 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -87,6 +87,10 @@ def poshold(cf, t, z): 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 diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index 17661a0e0..7d839df33 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -45,6 +45,7 @@ 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']) @@ -62,6 +63,10 @@ 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)), @@ -105,6 +110,11 @@ def activate_mellinger_controller(scf, use_mellinger): 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)) @@ -133,6 +143,8 @@ def crazyflie_control(scf): 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: From 2ccdb3d82aa25eeb6c562e900fccd44ad3602bc2 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Thu, 5 Dec 2024 13:12:07 +0100 Subject: [PATCH 720/861] Exclude python 3.13 on mac from unit tests --- .github/workflows/nightly.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index cf3ba9f9c..aedaf7095 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -13,6 +13,9 @@ jobs: matrix: os: [ubuntu-latest, lab-mac, windows-latest] python-version: ["3.10", "3.11", "3.12", "3.13"] + exclude: + - os: lab-mac + python-version: ["3.13"] runs-on: ${{ matrix.os }} From fb73c2114cb05d41b7b112b6600271ebe97d438e Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Thu, 5 Dec 2024 13:14:17 +0100 Subject: [PATCH 721/861] Add py 3.13 not supported for mac --- docs/installation/install.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/installation/install.md b/docs/installation/install.md index a505d74e3..274b8a90f 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -9,6 +9,8 @@ This project requires Python 3.10+. To install on Python 3.13, build tools and Python development headers are required. +***NOTE: Running with python 3.13 on macOS is not officially supported*** + See below sections for more platform-specific requirements. ## Install from Source ### Clone the repository From 4a681d1b9cb540dee4d658c76db7a914b99d05bd Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Thu, 5 Dec 2024 13:37:24 +0100 Subject: [PATCH 722/861] Python version in nighly exclude should be string --- .github/workflows/nightly.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index aedaf7095..aaf91788e 100644 --- a/.github/workflows/nightly.yml +++ b/.github/workflows/nightly.yml @@ -15,7 +15,7 @@ jobs: python-version: ["3.10", "3.11", "3.12", "3.13"] exclude: - os: lab-mac - python-version: ["3.13"] + python-version: "3.13" runs-on: ${{ matrix.os }} From fba61709aeb7536d5f214353ddc74fd78bf872de Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 5 Dec 2024 16:06:31 +0100 Subject: [PATCH 723/861] Move radio link statistics into (universal) link statistics object - Move radio link statistics into (universal) link statistics object - Stop updating radio link statistics from callbacks if stop is called on link statistics. User probably will expect this behavior. - Deprecation warning on calling link_quality_updated directly. Proxy call to link_statistics.link_quality_updated until removal. --- cflib/crazyflie/__init__.py | 36 ++++++++++------------------ cflib/crazyflie/link_statistics.py | 38 ++++++++++++++++++++++++++++++ 2 files changed, 51 insertions(+), 23 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index d8c4fa919..472019210 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -33,6 +33,7 @@ import datetime import logging import time +import warnings from collections import namedtuple from threading import current_thread from threading import Lock @@ -100,13 +101,6 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.packet_received = Caller() # Called for every packet sent self.packet_sent = Caller() - # Called when the link driver updates the link quality measurement - self.link_quality_updated = Caller() - self.uplink_rssi_updated = Caller() - self.uplink_rate_updated = Caller() - self.downlink_rate_updated = Caller() - self.uplink_congestion_updated = Caller() - self.downlink_congestion_updated = Caller() self.state = State.DISCONNECTED @@ -167,6 +161,17 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.disconnected.add_callback( lambda uri: self.link_statistics.stop()) + @property + def link_quality_updated(self): + # Issue a deprecation warning when the deprecated attribute is accessed + warnings.warn( + 'link_quality_updated is deprecated and will be removed soon. ' + 'Please use link_statistics.link_quality_updated directly and/or update your client.', + DeprecationWarning, + stacklevel=2 # To point to the caller's code + ) + return self.link_statistics.link_quality_updated + def _disconnected(self, link_uri): """ Callback when disconnected.""" self.connected_ts = None @@ -220,21 +225,6 @@ def _link_error_cb(self, errmsg): self.disconnected_link_error.call(self.link_uri, errmsg) self.state = State.DISCONNECTED - def _radio_link_statistics_cb(self, radio_link_statistics): - """Called from link driver to report radio link statistics""" - if 'link_quality' in radio_link_statistics: - self.link_quality_updated.call(radio_link_statistics['link_quality']) - if 'uplink_rssi' in radio_link_statistics: - self.uplink_rssi_updated.call(radio_link_statistics['uplink_rssi']) - if 'uplink_rate' in radio_link_statistics: - self.uplink_rate_updated.call(radio_link_statistics['uplink_rate']) - if 'downlink_rate' in radio_link_statistics: - self.downlink_rate_updated.call(radio_link_statistics['downlink_rate']) - if 'uplink_congestion' in radio_link_statistics: - self.uplink_congestion_updated.call(radio_link_statistics['uplink_congestion']) - if 'downlink_congestion' in radio_link_statistics: - self.downlink_congestion_updated.call(radio_link_statistics['downlink_congestion']) - def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. @@ -256,7 +246,7 @@ def open_link(self, link_uri): self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver( - link_uri, self._radio_link_statistics_cb, self._link_error_cb) + link_uri, self.link_statistics.radio_link_statistics_callback, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py index af23f3b56..e577659b4 100644 --- a/cflib/crazyflie/link_statistics.py +++ b/cflib/crazyflie/link_statistics.py @@ -60,20 +60,58 @@ class LinkStatistics: def __init__(self, crazyflie): self._cf = crazyflie + # Flag to track if the statistics are active + self._is_active = False + + # Universal statistics self.latency = Latency(self._cf) + # Proxy for latency callback + self.latency_updated = self.latency.latency_updated + + # Callers for radio link statistics + self.link_quality_updated = Caller() + self.uplink_rssi_updated = Caller() + self.uplink_rate_updated = Caller() + self.downlink_rate_updated = Caller() + self.uplink_congestion_updated = Caller() + self.downlink_congestion_updated = Caller() + def start(self): """ Start collecting all statistics. """ + self._is_active = True self.latency.start() def stop(self): """ Stop collecting all statistics. """ + self._is_active = False self.latency.stop() + def radio_link_statistics_callback(self, radio_link_statistics): + """ + This callback is called by the RadioLinkStatistics class after it + processes the data provided by the radio driver. + """ + if not self._is_active: + return # Skip processing if link statistics are stopped + + if 'link_quality' in radio_link_statistics: + self.link_quality_updated.call(radio_link_statistics['link_quality']) + if 'uplink_rssi' in radio_link_statistics: + self.uplink_rssi_updated.call(radio_link_statistics['uplink_rssi']) + if 'uplink_rate' in radio_link_statistics: + self.uplink_rate_updated.call(radio_link_statistics['uplink_rate']) + if 'downlink_rate' in radio_link_statistics: + self.downlink_rate_updated.call(radio_link_statistics['downlink_rate']) + if 'uplink_congestion' in radio_link_statistics: + self.uplink_congestion_updated.call(radio_link_statistics['uplink_congestion']) + if 'downlink_congestion' in radio_link_statistics: + self.downlink_congestion_updated.call(radio_link_statistics['downlink_congestion']) + class Latency: """ From 965f3d2fca69962e21b9dbf53166c2590c784fdd Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 5 Dec 2024 16:08:14 +0100 Subject: [PATCH 724/861] Enable deprecation warnings lib-wide These generally should be visible and fixed. + fix deprecated setDaemon warnings --- cflib/__init__.py | 4 ++++ cflib/crazyflie/__init__.py | 2 +- cflib/crazyflie/param.py | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/cflib/__init__.py b/cflib/__init__.py index 8c44f9d14..adff0ca58 100644 --- a/cflib/__init__.py +++ b/cflib/__init__.py @@ -54,6 +54,10 @@ cf.close_link() ``` """ +import warnings + +warnings.simplefilter('always', DeprecationWarning) # Enbable DeprecationWarnings + __pdoc__ = {} __pdoc__['cflib.crtp.cflinkcppdriver'] = False __pdoc__['cflib.cpx.transports'] = False diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 472019210..cc4868e50 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -109,7 +109,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): rw_cache=rw_cache) self.incoming = _IncomingPacketHandler(self) - self.incoming.setDaemon(True) + self.incoming.daemon = True if self.link: self.incoming.start() diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 1dc199e8f..beb69d2ec 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -599,7 +599,7 @@ class _ParamUpdater(Thread): def __init__(self, cf, useV2, updated_callback): """Initialize the thread""" Thread.__init__(self) - self.setDaemon(True) + self.daemon = True self.wait_lock = Lock() self.cf = cf self._useV2 = useV2 From d1ea364e88a1666d306906b6e8bc18b9fa0ce74c Mon Sep 17 00:00:00 2001 From: Hannes Verschore Date: Mon, 16 Dec 2024 23:32:55 +0100 Subject: [PATCH 725/861] Fix restart method in radiodriver.py. Restart doesn't work anymore, since _RadioDriverThread expects a rate_limit. Fixing this by saving the rate_limit and reusing it here. --- cflib/crtp/radiodriver.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 52095ad04..f46a9588b 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -260,6 +260,7 @@ def connect(self, uri, link_quality_callback, link_error_callback): devid, channel, datarate, address, rate_limit = self.parse_uri(uri) self.uri = uri + self.rate_limit = rate_limit if self._radio is None: self._radio = RadioManager.open(devid) @@ -383,7 +384,8 @@ def restart(self): self.out_queue, self.link_quality_callback, self.link_error_callback, - self) + self, + self.rate_limit) self._thread.start() def close(self): From 6eea1ed0e7d0d02aef4e44cc1676b6034efeeebf Mon Sep 17 00:00:00 2001 From: Tove Date: Wed, 8 Jan 2025 13:41:18 +0100 Subject: [PATCH 726/861] Initialize rate_limit Initialize rate_limit to None, for good practice --- cflib/crtp/radiodriver.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index f46a9588b..7e8ad3a5a 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -246,6 +246,7 @@ def __init__(self): self.out_queue = None self._thread = None self.needs_resending = True + self.rate_limit = None def connect(self, uri, link_quality_callback, link_error_callback): """ From 6ed0286e594c0d9b8151ebf46ea28dc8af2c7b43 Mon Sep 17 00:00:00 2001 From: Aris Date: Fri, 17 Jan 2025 13:19:30 +0100 Subject: [PATCH 727/861] Aris/Clarify_Same_Channel_In_Swarm_Examples --- examples/swarm/hl-commander-swarm.py | 1 + examples/swarm/swarmSequence.py | 1 + examples/swarm/swarmSequenceCircle.py | 1 + examples/swarm/synchronizedSequence.py | 1 + 4 files changed, 4 insertions(+) diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index 662d19317..a3f4ddf27 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -82,6 +82,7 @@ def run_shared_sequence(scf): '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__': diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py index a3d5e9e2d..cd4ee640d 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -52,6 +52,7 @@ 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' diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index 90835e3ff..90ecd7bbb 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -44,6 +44,7 @@ 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' diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index 7d839df33..f6483f4b4 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -59,6 +59,7 @@ '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 = [ From ba5f7f788483de97dd3eedbf21f12c7943716850 Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 21 Jan 2025 10:48:09 +0100 Subject: [PATCH 728/861] Added inline comments under all listed URIs in the sbs swarm interface and added an extra sentence on the first script description --- docs/user-guides/sbs_swarm_interface.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index 9ec4f3d56..a7559ae02 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -31,6 +31,7 @@ uris = { 'radio://0/20/2M/E7E7E7E703', 'radio://0/20/2M/E7E7E7E704', # 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__': @@ -39,7 +40,7 @@ if __name__ == '__main__': with Swarm(uris, factory=factory) as swarm: ``` -This will import all the necessary modules and open the necessary links for communication with all the Crazyflies of the swarm. `Swarm` is a wrapper class which facilitates the execution of functions given by the user for each copter and can execute them in parallel or sequentially. Each Crazyflie is treated as a `SyncCrazyflie` instance and as the first argument in swarm wide actions. There is no need to worry about threads since they are handled internally. To reduce connection time, the factory is chosen to be instance of the `CachedCfFactory` class that will cache the Crazyflie objects in the `./cache` directory. +This will import all the necessary modules and open the necessary links for communication with all the Crazyflies of the swarm. Note that the URIs in a swarm using the same radio must also be on the same channel. `Swarm` is a wrapper class which facilitates the execution of functions given by the user for each copter and can execute them in parallel or sequentially. Each Crazyflie is treated as a `SyncCrazyflie` instance and as the first argument in swarm wide actions. There is no need to worry about threads since they are handled internally. To reduce connection time, the factory is chosen to be instance of the `CachedCfFactory` class that will cache the Crazyflie objects in the `./cache` directory. The radio addresses of the copters are defined in the `uris` list and you can add more if you want. @@ -97,6 +98,7 @@ uris = { 'radio://0/20/2M/E7E7E7E703', 'radio://0/20/2M/E7E7E7E704', # 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__': @@ -189,6 +191,7 @@ uris = { 'radio://0/20/2M/E7E7E7E703', 'radio://0/20/2M/E7E7E7E704', # 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__': @@ -277,6 +280,7 @@ uris = { 'radio://0/20/2M/E7E7E7E703', 'radio://0/20/2M/E7E7E7E704', # 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__': From c0b09ae7a5c4be8e1d28bc25661d6284b299280f Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 22 Jan 2025 16:43:49 +0100 Subject: [PATCH 729/861] Added comment in sbs_swarm.py --- examples/step-by-step/sbs_swarm.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py index 3cdfa2d9f..b562ac58b 100644 --- a/examples/step-by-step/sbs_swarm.py +++ b/examples/step-by-step/sbs_swarm.py @@ -90,6 +90,7 @@ def run_square_sequence(scf: SyncCrazyflie): 'radio://0/20/2M/E7E7E7E703', 'radio://0/20/2M/E7E7E7E704', # 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 ] # The layout of the positions (1m apart from each other): From c654a1ba8bfa4d89f704d80a012c189223f821c8 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 28 Jan 2025 11:02:20 +0100 Subject: [PATCH 730/861] Increase sleep times during bootloading to wait for increased boot time Boot time was increased from 1 to 10 seconds in NRF firmware to prevent GAP8 freezes --- cflib/bootloader/__init__.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index eb2c482a2..1c5a597fc 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -278,7 +278,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo # Reset to firmware mode self.reset_to_firmware() self.close() - time.sleep(3) + time.sleep(12) # Flash all decks and reboot after each deck current_index = 0 @@ -289,7 +289,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self.progress_cb('Deck updated! Restarting...', int(100)) if current_index != -1: PowerSwitch(self.clink).reboot_to_fw() - time.sleep(3) + time.sleep(12) # Put the crazyflie back in Bootloader mode to exit the function in the same state we entered it self.start_bootloader(warm_boot=True, cf=cf) @@ -601,7 +601,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis self.progress_cb(f'Updating deck {deck.name}', 0) # Test and wait for the deck to be started - timeout_time = time.time() + 5 + timeout_time = time.time() + 12 while not deck.is_started: if time.time() > timeout_time: raise RuntimeError(f'Deck {deck.name} did not start') @@ -628,7 +628,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis continue # Wait for bootloader to be ready - timeout_time = time.time() + 5 + timeout_time = time.time() + 12 while not deck.is_bootloader_active: if time.time() > timeout_time: raise RuntimeError(f'Deck {deck.name} did not enter bootloader mode') From 32c1b1e5abb82f32238002a85597bd7f22d35826 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 29 Jan 2025 11:48:59 +0100 Subject: [PATCH 731/861] Only wait longer for restart if AI-deck is attached, reduce wait time Since nrf-firmware will now only delay boot if AI-deck is attached, and boot times have been reduced --- cflib/bootloader/__init__.py | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 1c5a597fc..3d62311e9 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -278,7 +278,11 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo # Reset to firmware mode self.reset_to_firmware() self.close() - time.sleep(12) + + if any(deck.target == 'bcAI:gap8' for deck in deck_targets): + time.sleep(7) + else: + time.sleep(2) # Flash all decks and reboot after each deck current_index = 0 @@ -289,7 +293,10 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self.progress_cb('Deck updated! Restarting...', int(100)) if current_index != -1: PowerSwitch(self.clink).reboot_to_fw() - time.sleep(12) + if any(deck.target == 'bcAI:gap8' for deck in deck_targets): + time.sleep(7) + else: + time.sleep(2) # Put the crazyflie back in Bootloader mode to exit the function in the same state we entered it self.start_bootloader(warm_boot=True, cf=cf) @@ -601,7 +608,10 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis self.progress_cb(f'Updating deck {deck.name}', 0) # Test and wait for the deck to be started - timeout_time = time.time() + 12 + if any(deck.name == 'bcAI:gap8' for deck in decks.values()): + timeout_time = time.time() + 9 + else: + timeout_time = time.time() + 4 while not deck.is_started: if time.time() > timeout_time: raise RuntimeError(f'Deck {deck.name} did not start') @@ -628,7 +638,10 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis continue # Wait for bootloader to be ready - timeout_time = time.time() + 12 + if any(deck.name == 'bcAI:gap8' for deck in decks.values()): + timeout_time = time.time() + 9 + else: + timeout_time = time.time() + 4 while not deck.is_bootloader_active: if time.time() > timeout_time: raise RuntimeError(f'Deck {deck.name} did not enter bootloader mode') From d56f5c846485ff4fbce81186e0887b909080371d Mon Sep 17 00:00:00 2001 From: Aris Date: Fri, 7 Feb 2025 10:37:26 +0100 Subject: [PATCH 732/861] Added arming in sbs motion commander --- docs/user-guides/sbs_motion_commander.md | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 79876107e..883a7b6d8 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -55,6 +55,9 @@ Since this tutorial won't be a table top tutorial like last time, but an actual We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...SyncCrazyflie(...)` in `__main__` ```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) scf.cf.param.add_update_callback(group="deck", name="bcFlow2", cb=param_deck_flow) @@ -120,6 +123,9 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -133,6 +139,9 @@ So now we are going to start up the SyncCrazyflie and start a function in the `_ ```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) if not deck_attached_event.wait(timeout=5): print('No flow deck detected!') @@ -216,6 +225,9 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -305,6 +317,9 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -408,6 +423,9 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -521,6 +539,9 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -648,6 +669,9 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) From 1d846e2e4d7ced2dc1fc7878f3ccf8a9176b6efc Mon Sep 17 00:00:00 2001 From: Aris Date: Mon, 10 Feb 2025 14:54:02 +0100 Subject: [PATCH 733/861] Added warning about usage --- cflib/crazyflie/swarm.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index 9bf8cd6d8..eeb562b77 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -130,6 +130,10 @@ def get_estimated_positions(self): """ Return a `dict`, keyed by URI and with the SwarmPosition namedtuples as value, with the estimated (x, y, z) of each Crazyflie in the swarm. + + This function is very costly in resources and is not recommended to be + used in a loop. To continuously get the position of the Crazyflies, use the + start_position_printing() function in the autonomousSequence.py example. """ self.parallel_safe(self.__get_estimated_position) return self._positions From e2a07fae3df6711af0a56e88c0557abb0d4331d6 Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 11 Feb 2025 12:00:16 +0100 Subject: [PATCH 734/861] Removed reset estimator - swarm is used --- examples/autonomy/circling_square_demo.py | 51 ----------------------- 1 file changed, 51 deletions(-) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index 6cc58ff45..f2f735638 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -36,13 +36,11 @@ import cflib.crtp from cflib.crazyflie.high_level_commander import HighLevelCommander -from cflib.crazyflie.log import LogConfig 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 -from cflib.crazyflie.syncLogger import SyncLogger URI1 = 'radio://0/60/2M/E7E7E7E710' URI2 = 'radio://0/60/2M/E7E7E7E711' @@ -72,55 +70,6 @@ def rotate_beizer_node(xl, yl, alpha): return x_rot, y_rot -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(cf): - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def activate_high_level_commander(cf): cf.param.set_value('commander.enHighLevel', '1') From 12b02db6a2ca4ab30aa189e83a3ced523cf6f05c Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 11 Feb 2025 16:04:13 +0100 Subject: [PATCH 735/861] Added ResetEstimator util --- cflib/utils/ResetEstimator.py | 56 +++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 cflib/utils/ResetEstimator.py diff --git a/cflib/utils/ResetEstimator.py b/cflib/utils/ResetEstimator.py new file mode 100644 index 000000000..b933f6203 --- /dev/null +++ b/cflib/utils/ResetEstimator.py @@ -0,0 +1,56 @@ +import time + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.syncLogger import SyncLogger + + +def reset_estimator(crazyflie): + if isinstance(crazyflie, SyncCrazyflie): + cf = crazyflie.cf + else: + cf = crazyflie + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + _wait_for_position_estimator(cf) + + +def _wait_for_position_estimator(cf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(cf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + print('reset estimator success!') + break From 4b70359536cddd793978a3277d72d659f19ce0fe Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 11 Feb 2025 16:06:27 +0100 Subject: [PATCH 736/861] Converted reset estimator in autonomy examples --- examples/autonomy/autonomousSequence.py | 54 +----------------- .../autonomous_sequence_high_level.py | 52 +----------------- ...tonomous_sequence_high_level_compressed.py | 52 +----------------- examples/autonomy/full_state_setpoint_demo.py | 55 +------------------ 4 files changed, 6 insertions(+), 207 deletions(-) diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index da6cb9a7c..00ab4a2f8 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -36,8 +36,8 @@ 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.utils import uri_helper +from cflib.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -56,56 +56,6 @@ ] -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def position_callback(timestamp, data, logconf): x = data['kalman.stateX'] y = data['kalman.stateY'] @@ -154,5 +104,5 @@ def run_sequence(scf, sequence): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: reset_estimator(scf) - # start_position_printing(scf) + start_position_printing(scf) run_sequence(scf, sequence) diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 6bf737e4f..e7219ee41 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -33,12 +33,11 @@ import cflib.crtp from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper +from cflib.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -62,55 +61,6 @@ ] -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(cf): - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def activate_mellinger_controller(cf): cf.param.set_value('stabilizer.controller', '2') diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index b28059a5b..2628739af 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -34,13 +34,12 @@ import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.high_level_commander import HighLevelCommander -from cflib.crazyflie.log import LogConfig 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.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper +from cflib.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -58,55 +57,6 @@ ] -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(cf): - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def activate_mellinger_controller(cf): cf.param.set_value('stabilizer.controller', '2') diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 0eaac6a32..57885653d 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -30,12 +30,11 @@ 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.utils import uri_helper - +from cflib.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/65/2M/E7E7E7E7F2') +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') def quaternion_from_euler(roll: float, pitch: float, yaw: float): @@ -52,56 +51,6 @@ def quaternion_from_euler(roll: float, pitch: float, yaw: float): return Rotation.from_euler(seq='xyz', angles=(roll, pitch, yaw), degrees=False).as_quat() -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def position_callback(timestamp, data, logconf): x = data['kalman.stateX'] y = data['kalman.stateY'] From b50ebfb0d676d2252f1f84b8cc125f5a5530bf56 Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 11 Feb 2025 16:07:44 +0100 Subject: [PATCH 737/861] Converted reset estimator in lighthouse examples --- examples/lighthouse/lighthouse_openvr_grab.py | 54 +------------------ .../lighthouse_openvr_grab_color.py | 54 +------------------ .../lighthouse/lighthouse_openvr_multigrab.py | 52 +----------------- 3 files changed, 5 insertions(+), 155 deletions(-) diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py index 8aff29538..7b48a5143 100644 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ b/examples/lighthouse/lighthouse_openvr_grab.py @@ -11,10 +11,10 @@ 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.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' +uri = 'radio://0/80/2M/E7E7E7E7E7' print('Opening') vr = openvr.init(openvr.VRApplication_Other) @@ -37,56 +37,6 @@ sys.exit(1) -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def position_callback(timestamp, data, logconf): x = data['kalman.stateX'] y = data['kalman.stateY'] diff --git a/examples/lighthouse/lighthouse_openvr_grab_color.py b/examples/lighthouse/lighthouse_openvr_grab_color.py index 6e8abef55..12dede16f 100644 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ b/examples/lighthouse/lighthouse_openvr_grab_color.py @@ -15,10 +15,10 @@ 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.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' +uri = 'radio://0/80/2M/E7E7E7E7E7' print('Openning') vr = openvr.init(openvr.VRApplication_Other) @@ -41,56 +41,6 @@ sys.exit(1) -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print('{} {} {}'. - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def position_callback(timestamp, data, logconf): x = data['kalman.stateX'] y = data['kalman.stateY'] diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index 9948e359c..a0a57a463 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -12,7 +12,7 @@ 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.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to uri0 = 'radio://0/80/2M/E7E7E7E701' @@ -39,56 +39,6 @@ sys.exit(1) -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def position_callback(timestamp, data, logconf): x = data['kalman.stateX'] y = data['kalman.stateY'] From 0fa68406be08b256a7d86dac4f7987e5b654e009 Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 11 Feb 2025 16:08:59 +0100 Subject: [PATCH 738/861] Converted reset estimator in mocap examples --- examples/mocap/mocap_hl_commander.py | 53 +------------------------ examples/mocap/qualisys_hl_commander.py | 53 +------------------------ 2 files changed, 2 insertions(+), 104 deletions(-) diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py index 3d9f0ea13..673396981 100644 --- a/examples/mocap/mocap_hl_commander.py +++ b/examples/mocap/mocap_hl_commander.py @@ -35,12 +35,11 @@ import cflib.crtp from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper +from cflib.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -105,47 +104,6 @@ def run(self): self.on_pose([pos[0], pos[1], pos[2], obj.rotation]) -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - def send_extpose_quat(cf, x, y, z, quat): """ Send the current Crazyflie X, Y, Z position and attitude as a quaternion. @@ -157,15 +115,6 @@ def send_extpose_quat(cf, x, y, z, quat): cf.extpos.send_extpos(x, y, z) -def reset_estimator(cf): - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - # time.sleep(1) - wait_for_position_estimator(cf) - - def adjust_orientation_sensitivity(cf): cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev) diff --git a/examples/mocap/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py index d2dcb301f..237b64aa7 100644 --- a/examples/mocap/qualisys_hl_commander.py +++ b/examples/mocap/qualisys_hl_commander.py @@ -38,12 +38,11 @@ import cflib.crtp from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper +from cflib.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -154,47 +153,6 @@ async def _close(self): self.connection.disconnect() -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - def _sqrt(a): """ There might be rounding errors making 'a' slightly negative. @@ -219,15 +177,6 @@ def send_extpose_rot_matrix(cf, x, y, z, rot): cf.extpos.send_extpos(x, y, z) -def reset_estimator(cf): - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - # time.sleep(1) - wait_for_position_estimator(cf) - - def adjust_orientation_sensitivity(cf): cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev) From 76f29c6d9500ee678f102d028a283ab8f206ae8a Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 11 Feb 2025 16:09:44 +0100 Subject: [PATCH 739/861] Converted reset estimator in positioning examples --- examples/positioning/flowsequenceSync.py | 7 ++-- examples/positioning/initial_position.py | 53 +----------------------- 2 files changed, 4 insertions(+), 56 deletions(-) diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py index 24c0bd939..6ac262aba 100644 --- a/examples/positioning/flowsequenceSync.py +++ b/examples/positioning/flowsequenceSync.py @@ -35,6 +35,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper +from cflib.utils.ResetEstimator import reset_estimator URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -49,10 +50,8 @@ with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - time.sleep(2) + reset_estimator(scf) + time.sleep(1) # Arm the Crazyflie cf.platform.send_arming_request(True) diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index b0c0274ce..3e293a824 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -38,10 +38,9 @@ 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.utils import uri_helper +from cflib.utils.ResetEstimator import reset_estimator # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') @@ -56,47 +55,6 @@ ] -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') - - log_config = LogConfig(name='Kalman Variance', period_in_ms=500) - log_config.add_variable('kalman.varPX', 'float') - log_config.add_variable('kalman.varPY', 'float') - log_config.add_variable('kalman.varPZ', 'float') - - var_y_history = [1000] * 10 - var_x_history = [1000] * 10 - var_z_history = [1000] * 10 - - threshold = 0.001 - - with SyncLogger(scf, log_config) as logger: - for log_entry in logger: - data = log_entry[1] - - var_x_history.append(data['kalman.varPX']) - var_x_history.pop(0) - var_y_history.append(data['kalman.varPY']) - var_y_history.pop(0) - var_z_history.append(data['kalman.varPZ']) - var_z_history.pop(0) - - min_x = min(var_x_history) - max_x = max(var_x_history) - min_y = min(var_y_history) - max_y = max(var_y_history) - min_z = min(var_z_history) - max_z = max(var_z_history) - - # print("{} {} {}". - # format(max_x - min_x, max_y - min_y, max_z - min_z)) - - if (max_x - min_x) < threshold and ( - max_y - min_y) < threshold and ( - max_z - min_z) < threshold: - break - - 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) @@ -106,15 +64,6 @@ def set_initial_position(scf, x, y, z, yaw_deg): scf.cf.param.set_value('kalman.initialYaw', yaw_radians) -def reset_estimator(scf): - cf = scf.cf - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - - wait_for_position_estimator(cf) - - def run_sequence(scf, sequence, base_x, base_y, base_z, yaw): cf = scf.cf From 3f0c5a0d9e36270f65b2b007e82f3ff4ede0350a Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 11 Feb 2025 17:07:10 +0100 Subject: [PATCH 740/861] Use correct yaw convention in motion commander Requires reverting commit f6c0e737cfd061b72efcb125af7d545e06597807 in `crazyflie-firmware`, i.e., requires no longer inverting received yaw rates upon arrival in `crazyflie-firmware` --- cflib/positioning/motion_commander.py | 8 ++++---- examples/multiranger/multiranger_wall_following.py | 4 +--- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/cflib/positioning/motion_commander.py b/cflib/positioning/motion_commander.py index 35debdd0f..28f258a2e 100644 --- a/cflib/positioning/motion_commander.py +++ b/cflib/positioning/motion_commander.py @@ -347,7 +347,7 @@ def start_turn_left(self, rate=RATE): :param rate: The angular rate (degrees/second) :return: """ - self._set_vel_setpoint(0.0, 0.0, 0.0, -rate) + self._set_vel_setpoint(0.0, 0.0, 0.0, rate) def start_turn_right(self, rate=RATE): """ @@ -356,7 +356,7 @@ def start_turn_right(self, rate=RATE): :param rate: The angular rate (degrees/second) :return: """ - self._set_vel_setpoint(0.0, 0.0, 0.0, rate) + self._set_vel_setpoint(0.0, 0.0, 0.0, -rate) def start_circle_left(self, radius_m, velocity=VELOCITY): """ @@ -369,7 +369,7 @@ def start_circle_left(self, radius_m, velocity=VELOCITY): circumference = 2 * radius_m * math.pi rate = 360.0 * velocity / circumference - self._set_vel_setpoint(velocity, 0.0, 0.0, -rate) + self._set_vel_setpoint(velocity, 0.0, 0.0, rate) def start_circle_right(self, radius_m, velocity=VELOCITY): """ @@ -382,7 +382,7 @@ def start_circle_right(self, radius_m, velocity=VELOCITY): circumference = 2 * radius_m * math.pi rate = 360.0 * velocity / circumference - self._set_vel_setpoint(velocity, 0.0, 0.0, rate) + self._set_vel_setpoint(velocity, 0.0, 0.0, -rate) def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw=0.0): """ diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index d54d4a871..7e06a59c3 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -122,9 +122,7 @@ def handle_range_measurement(range): 'yaw_rate', yaw_rate, 'state_wf', state_wf) # convert yaw_rate from rad to deg - # the negative sign is because of this ticket: - # https://github.com/bitcraze/crazyflie-lib-python/issues/389 - yaw_rate_deg = -1 * degrees(yaw_rate) + yaw_rate_deg = degrees(yaw_rate) motion_commander.start_linear_motion( velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg) From 1b6ebc364d23b46aa1d30b0ef61c60e8f033a29a Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 12 Feb 2025 10:42:23 +0100 Subject: [PATCH 741/861] Correct yaw convention in motion commander tests --- test/positioning/test_motion_commander.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/test/positioning/test_motion_commander.py b/test/positioning/test_motion_commander.py index 879ec8e6f..a8b9855dd 100644 --- a/test/positioning/test_motion_commander.py +++ b/test/positioning/test_motion_commander.py @@ -255,7 +255,7 @@ def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock): # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(0.0, 0.0, 0.0, rate), + call(0.0, 0.0, 0.0, -rate), ]) def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock): @@ -270,7 +270,7 @@ def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock): # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(0.0, 0.0, 0.0, -rate), + call(0.0, 0.0, 0.0, rate), ]) def test_that_it_starts_circle_right( @@ -289,7 +289,7 @@ def test_that_it_starts_circle_right( # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(velocity, 0.0, 0.0, expected_rate), + call(velocity, 0.0, 0.0, -expected_rate), ]) def test_that_it_starts_circle_left( @@ -308,7 +308,7 @@ def test_that_it_starts_circle_left( # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(velocity, 0.0, 0.0, -expected_rate), + call(velocity, 0.0, 0.0, expected_rate), ]) def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock): @@ -325,7 +325,7 @@ def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock): # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(0.0, 0.0, 0.0, rate), + call(0.0, 0.0, 0.0, -rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) @@ -344,7 +344,7 @@ def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock): # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(0.0, 0.0, 0.0, -rate), + call(0.0, 0.0, 0.0, rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) @@ -368,7 +368,7 @@ def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock): # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(velocity, 0.0, 0.0, rate), + call(velocity, 0.0, 0.0, -rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) @@ -392,7 +392,7 @@ def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock): # Assert thread_mock.set_vel_setpoint.assert_has_calls([ - call(velocity, 0.0, 0.0, -rate), + call(velocity, 0.0, 0.0, rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) From 44dcf1fc1ccda78da4c3daa56bf4c9a2daf040c9 Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 12 Feb 2025 14:06:44 +0100 Subject: [PATCH 742/861] Added asynchronized swarm example with motion commander --- examples/swarm/asynchronizedSwarm.py | 154 +++++++++++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 examples/swarm/asynchronizedSwarm.py diff --git a/examples/swarm/asynchronizedSwarm.py b/examples/swarm/asynchronizedSwarm.py new file mode 100644 index 000000000..de090b87c --- /dev/null +++ b/examples/swarm/asynchronizedSwarm.py @@ -0,0 +1,154 @@ +# -*- 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, timestamp, data, logconf): + global pos1, pos2 + if uri == 'radio://0/80/2M/E7E7E7E7E7': + 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 == 'radio://0/80/2M/E7E7E7E7E8': + 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, timestamp, data, logconf)) + log_conf1.start() + + +def async_flight(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + end_time = time.time() + 12 + while time.time() < end_time: + if scf.__dict__['_link_uri'] == 'radio://0/80/2M/E7E7E7E7E7': + if time.time() < end_time - 7: + mc.start_up(DEFAULT_VELOCITY) + elif time.time() < end_time - 5: + mc.stop() + elif time.time() < end_time: + mc.start_down(DEFAULT_VELOCITY) + else: + mc.stop() + + elif scf.__dict__['_link_uri'] == 'radio://0/80/2M/E7E7E7E7E8': + if time.time() < end_time-10: + mc.start_left(DEFAULT_VELOCITY) + elif time.time() < end_time-8: + mc.start_right(DEFAULT_VELOCITY) + elif time.time() < end_time-6: + mc.start_left(DEFAULT_VELOCITY) + elif time.time() < end_time-4: + mc.start_right(DEFAULT_VELOCITY) + elif time.time() < end_time-2: + mc.start_left(DEFAULT_VELOCITY) + elif time.time() < end_time: + 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(async_flight) + time.sleep(1) From c25aa4423867810d4f6ec6a459533c7f8e71d7a5 Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 12 Feb 2025 14:15:51 +0100 Subject: [PATCH 743/861] Renamed file to reset_estimator.py --- cflib/utils/{ResetEstimator.py => reset_estimator.py} | 0 examples/autonomy/autonomousSequence.py | 2 +- examples/autonomy/autonomous_sequence_high_level.py | 2 +- examples/autonomy/autonomous_sequence_high_level_compressed.py | 2 +- examples/autonomy/full_state_setpoint_demo.py | 2 +- examples/lighthouse/lighthouse_openvr_grab.py | 2 +- examples/lighthouse/lighthouse_openvr_grab_color.py | 2 +- examples/lighthouse/lighthouse_openvr_multigrab.py | 2 +- examples/mocap/mocap_hl_commander.py | 2 +- examples/mocap/qualisys_hl_commander.py | 2 +- examples/positioning/flowsequenceSync.py | 2 +- examples/positioning/initial_position.py | 2 +- 12 files changed, 11 insertions(+), 11 deletions(-) rename cflib/utils/{ResetEstimator.py => reset_estimator.py} (100%) diff --git a/cflib/utils/ResetEstimator.py b/cflib/utils/reset_estimator.py similarity index 100% rename from cflib/utils/ResetEstimator.py rename to cflib/utils/reset_estimator.py diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index 00ab4a2f8..d1b3bc5a5 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -37,7 +37,7 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +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') diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index e7219ee41..afa995fae 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -37,7 +37,7 @@ from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +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') diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index 2628739af..9dff82cf5 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -39,7 +39,7 @@ from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +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') diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 57885653d..9f81f9de3 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -31,7 +31,7 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +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') diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py index 7b48a5143..632ebc9d7 100644 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ b/examples/lighthouse/lighthouse_openvr_grab.py @@ -11,7 +11,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils.ResetEstimator import reset_estimator +from cflib.utils.reset_estimator import reset_estimator # URI to the Crazyflie to connect to uri = 'radio://0/80/2M/E7E7E7E7E7' diff --git a/examples/lighthouse/lighthouse_openvr_grab_color.py b/examples/lighthouse/lighthouse_openvr_grab_color.py index 12dede16f..64b5a5812 100644 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ b/examples/lighthouse/lighthouse_openvr_grab_color.py @@ -15,7 +15,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils.ResetEstimator import reset_estimator +from cflib.utils.reset_estimator import reset_estimator # URI to the Crazyflie to connect to uri = 'radio://0/80/2M/E7E7E7E7E7' diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index a0a57a463..2c3134ff3 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -12,7 +12,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils.ResetEstimator import reset_estimator +from cflib.utils.reset_estimator import reset_estimator # URI to the Crazyflie to connect to uri0 = 'radio://0/80/2M/E7E7E7E701' diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py index 673396981..162a8fd0c 100644 --- a/examples/mocap/mocap_hl_commander.py +++ b/examples/mocap/mocap_hl_commander.py @@ -39,7 +39,7 @@ from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +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') diff --git a/examples/mocap/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py index 237b64aa7..3470a5c00 100644 --- a/examples/mocap/qualisys_hl_commander.py +++ b/examples/mocap/qualisys_hl_commander.py @@ -42,7 +42,7 @@ from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +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') diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py index 6ac262aba..4617c41ee 100644 --- a/examples/positioning/flowsequenceSync.py +++ b/examples/positioning/flowsequenceSync.py @@ -35,7 +35,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +from cflib.utils.reset_estimator import reset_estimator URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index 3e293a824..79ff83634 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -40,7 +40,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.utils import uri_helper -from cflib.utils.ResetEstimator import reset_estimator +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') From 86df7156a64b3973d80d1a3fd2f0d38126898ed9 Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 12 Feb 2025 14:30:23 +0100 Subject: [PATCH 744/861] Fixed reset_estimator confirmation message --- cflib/utils/reset_estimator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/utils/reset_estimator.py b/cflib/utils/reset_estimator.py index b933f6203..e22debbd1 100644 --- a/cflib/utils/reset_estimator.py +++ b/cflib/utils/reset_estimator.py @@ -18,7 +18,7 @@ def reset_estimator(crazyflie): def _wait_for_position_estimator(cf): - print('Waiting for estimator to find position...') + print('Waiting for estimator to find position...', end='\r') log_config = LogConfig(name='Kalman Variance', period_in_ms=500) log_config.add_variable('kalman.varPX', 'float') @@ -52,5 +52,5 @@ def _wait_for_position_estimator(cf): if (max_x - min_x) < threshold and ( max_y - min_y) < threshold and ( max_z - min_z) < threshold: - print('reset estimator success!') + print('Waiting for estimator to find position...success!') break From 119ce20b90977a9152ae550bd71f2c612805bd72 Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 12 Feb 2025 14:38:27 +0100 Subject: [PATCH 745/861] Added swarm.reset_estimator in autonomy/swarm example --- examples/autonomy/circling_square_demo.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index f2f735638..e1871bca5 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -183,6 +183,7 @@ def upload_trajectories(scf, alpha, r): 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) From 03251bb6e8183eb9289ec988412e9add8bd26288 Mon Sep 17 00:00:00 2001 From: Aris Date: Mon, 10 Feb 2025 15:14:40 +0100 Subject: [PATCH 746/861] Added go_to() documentation --- cflib/crazyflie/high_level_commander.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 98ab846d7..06e014f85 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -137,7 +137,20 @@ def stop(self, group_mask=ALL_GROUPS): def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, group_mask=ALL_GROUPS): """ - Go to an absolute or relative position + Go to an absolute or relative position. + + The path is designed to transition smoothly from the current + state to the target position, gradually decelerating at the + goal with minimal overshoot. When the system is at hover, the + path will be a straight line, but if there is any initial + velocity, the path will be a smooth curve. + + The trajectory is derived by solving for a unique 7th-degree + polynomial that satisfies the initial conditions of position, + velocity, and acceleration, and ends at the goal with zero + velocity and acceleration. Additionally, the jerk (derivative + of acceleration) is constrained to be zero at both the starting + and ending points. :param x: X (m) :param y: Y (m) From 405d32e0e88f70e3aae6dd94bca19904867a9f0a Mon Sep 17 00:00:00 2001 From: Aris Date: Mon, 10 Feb 2025 15:17:23 +0100 Subject: [PATCH 747/861] Added hl_commander documentation --- cflib/crazyflie/high_level_commander.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 06e014f85..bb3cb1420 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -23,7 +23,13 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . """ -Used for sending high level setpoints to the Crazyflie +Used for sending high level setpoints to the Crazyflie. + +The high level commander generates setpoints from within the firmware +based on a predefined trajectory. This was merged as part of the +Crazyswarm project of the USC ACT lab. The high level commander uses a +planner to generate smooth trajectories based on actions like "take off", +"go to" or "land" with 7th order polynomials. """ import math import struct From 1a10819b75dbf39c089c738e4bf956d042fed8bb Mon Sep 17 00:00:00 2001 From: Aris Date: Mon, 10 Feb 2025 15:22:01 +0100 Subject: [PATCH 748/861] Added warning about overlapping commands --- cflib/crazyflie/high_level_commander.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index bb3cb1420..e52ad520e 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -158,6 +158,10 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, of acceleration) is constrained to be zero at both the starting and ending points. + Warning! Avoid overlapping go_to commands. When a command is sent to a + Crazyflie when another one is currently executed, the generated polynomial + can take unexpected routes and have high peaks. + :param x: X (m) :param y: Y (m) :param z: Z (m) From 0c63fde01d1470fd98f2abfa033e06f7a855f506 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 13 Feb 2025 12:16:00 +0100 Subject: [PATCH 749/861] Send correct yaw sign based on the Crazyflie's CRTP version; maintain backward compatibility The lib now checks the Crazyflie's CRTP version before sending velocity, zdistance, or hover setpoints. If the Crazyflie is outdated, it sends the legacy type with the original (negated) yaw to maintain compatibility. If the Crazyflie supports the new format, it sends the updated type with the correct yaw sign. This ensures a smooth transition when updating either the lib or Crazyflie separately. The legacy type will be deprecated in a future update. --- cflib/crazyflie/commander.py | 46 +++++++++++++++++++++++++++++------- 1 file changed, 37 insertions(+), 9 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 4f78da782..1529e82da 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -26,6 +26,7 @@ Used for sending control setpoints to the Crazyflie """ import struct +import warnings from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort @@ -38,11 +39,14 @@ META_COMMAND_CHANNEL = 1 TYPE_STOP = 0 -TYPE_VELOCITY_WORLD = 1 -TYPE_ZDISTANCE = 2 -TYPE_HOVER = 5 +TYPE_VELOCITY_WORLD_LEGACY = 1 +TYPE_ZDISTANCE_LEGACY = 2 +TYPE_HOVER_LEGACY = 5 TYPE_FULL_STATE = 6 TYPE_POSITION = 7 +TYPE_VELOCITY_WORLD = 8 +TYPE_ZDISTANCE = 9 +TYPE_HOVER = 10 TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0 @@ -122,8 +126,16 @@ def send_velocity_world_setpoint(self, vx, vy, vz, yawrate): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.channel = SET_SETPOINT_CHANNEL - pk.data = struct.pack(' Date: Mon, 17 Feb 2025 11:18:59 +0100 Subject: [PATCH 750/861] Added swarm sharing data example --- examples/swarm/leader-follower.py | 208 ++++++++++++++++++++++++++++++ 1 file changed, 208 insertions(+) create mode 100644 examples/swarm/leader-follower.py diff --git a/examples/swarm/leader-follower.py b/examples/swarm/leader-follower.py new file mode 100644 index 000000000..7a4852afe --- /dev/null +++ b/examples/swarm/leader-follower.py @@ -0,0 +1,208 @@ +# -*- 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 yaw1 + global x1, y1, z1, x2, y2, z2, 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) From 4edb8c3f654a5b011b47b498c028a666120b4f6c Mon Sep 17 00:00:00 2001 From: Aris Date: Mon, 17 Feb 2025 12:03:25 +0100 Subject: [PATCH 751/861] Fixed minor issues in asynchronizedSwarm example --- examples/swarm/asynchronizedSwarm.py | 35 +++++++++++++++------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/examples/swarm/asynchronizedSwarm.py b/examples/swarm/asynchronizedSwarm.py index de090b87c..02fd941a4 100644 --- a/examples/swarm/asynchronizedSwarm.py +++ b/examples/swarm/asynchronizedSwarm.py @@ -73,14 +73,14 @@ def arm(scf): time.sleep(1.0) -def position_callback(uri, timestamp, data, logconf): +def position_callback(uri, data): global pos1, pos2 - if uri == 'radio://0/80/2M/E7E7E7E7E7': + 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 == 'radio://0/80/2M/E7E7E7E7E8': + elif uri == URI2: pos2[0] = data['stateEstimate.x'] pos2[1] = data['stateEstimate.y'] pos2[2] = data['stateEstimate.z'] @@ -93,38 +93,41 @@ def start_position_printing(scf): 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, timestamp, data, logconf)) + 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'] == 'radio://0/80/2M/E7E7E7E7E7': - if time.time() < end_time - 7: + + if scf.__dict__['_link_uri'] == URI1: + if time.time() - start_time < 5: mc.start_up(DEFAULT_VELOCITY) - elif time.time() < end_time - 5: + elif time.time() - start_time < 7: mc.stop() - elif time.time() < end_time: + elif time.time() - start_time < 12: mc.start_down(DEFAULT_VELOCITY) else: mc.stop() - elif scf.__dict__['_link_uri'] == 'radio://0/80/2M/E7E7E7E7E8': - if time.time() < end_time-10: + elif scf.__dict__['_link_uri'] == URI2: + if time.time() - start_time < 2: mc.start_left(DEFAULT_VELOCITY) - elif time.time() < end_time-8: + elif time.time() - start_time < 4: mc.start_right(DEFAULT_VELOCITY) - elif time.time() < end_time-6: + elif time.time() - start_time < 6: mc.start_left(DEFAULT_VELOCITY) - elif time.time() < end_time-4: + elif time.time() - start_time < 8: mc.start_right(DEFAULT_VELOCITY) - elif time.time() < end_time-2: + elif time.time() - start_time < 10: mc.start_left(DEFAULT_VELOCITY) - elif time.time() < end_time: + elif time.time() - start_time < 12: mc.start_right(DEFAULT_VELOCITY) else: mc.stop() From 2a7dd8010ae29f3ddae9c40abb9545f3d65f3703 Mon Sep 17 00:00:00 2001 From: Aris Date: Mon, 17 Feb 2025 14:22:37 +0100 Subject: [PATCH 752/861] Added arm option --- examples/swarm/leader-follower.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/swarm/leader-follower.py b/examples/swarm/leader-follower.py index 7a4852afe..2505822e7 100644 --- a/examples/swarm/leader-follower.py +++ b/examples/swarm/leader-follower.py @@ -194,7 +194,7 @@ def leader_follower(scf): swarm.reset_estimators() - # swarm.parallel_safe(arm) + swarm.parallel_safe(arm) print('Waiting for parameters to be downloaded...') swarm.parallel_safe(wait_for_param_download) From 843c81af574da41b32dd79d57f4429c5281d7931 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 17 Feb 2025 16:02:34 +0100 Subject: [PATCH 753/861] Use correct Linux Trove classifier --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 6677f6f41..e71f57823 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -22,7 +22,7 @@ classifiers = [ "Intended Audience :: Science/Research", "Intended Audience :: Education", "Intended Audience :: Developers", - "Operating System :: Linux", + "Operating System :: POSIX :: Linux", "Operating System :: MacOS", "Operating System :: Microsoft :: Windows", From 042e3f5598fdb847007f28a491243e6b304e9b71 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Mon, 17 Feb 2025 16:09:57 +0100 Subject: [PATCH 754/861] Remove robotics topic classifier --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index e71f57823..283dedb81 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -18,7 +18,7 @@ classifiers = [ "Development Status :: 5 - Production/Stable", "License :: OSI Approved :: GNU General Public License v3 (GPLv3)", "Topic :: System :: Hardware :: Hardware Drivers", - "Topic :: Scientific/Engineering :: Robotics", + "Topic :: Scientific/Engineering", "Intended Audience :: Science/Research", "Intended Audience :: Education", "Intended Audience :: Developers", From eb659d36a820438c91c53cf31411ee90fe158392 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 18 Feb 2025 11:52:36 +0100 Subject: [PATCH 755/861] Account for AI-deck boot delay in reset to firmware --- cflib/bootloader/__init__.py | 13 +++++++------ cflib/bootloader/cloader.py | 4 ++-- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 3d62311e9..e7d67dac9 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -276,12 +276,13 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self.progress_cb('Restarting firmware to update decks.', int(0)) # Reset to firmware mode - self.reset_to_firmware() - self.close() - if any(deck.target == 'bcAI:gap8' for deck in deck_targets): + self.reset_to_firmware(boot_delay=5.0) + self.close() time.sleep(7) else: + self.reset_to_firmware() + self.close() time.sleep(2) # Flash all decks and reboot after each deck @@ -389,12 +390,12 @@ def _flash_flash(self, artifacts: List[FlashArtifact], targets: List[Target]): for (i, artifact) in enumerate(artifacts): self._internal_flash(artifact, i + 1, len(artifacts)) - def reset_to_firmware(self) -> bool: + def reset_to_firmware(self, boot_delay: float = 0.0) -> bool: status = False if self._cload.protocol_version == BootVersion.CF2_PROTO_VER: - status = self._cload.reset_to_firmware(TargetTypes.NRF51) + status = self._cload.reset_to_firmware(TargetTypes.NRF51, boot_delay) else: - status = self._cload.reset_to_firmware(TargetTypes.STM32) + status = self._cload.reset_to_firmware(TargetTypes.STM32, boot_delay) if status: self.close() diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index 53419da54..ea54675db 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -117,7 +117,7 @@ def reset_to_bootloader(self, target_id: int) -> bool: return False - def reset_to_firmware(self, target_id: int) -> bool: + def reset_to_firmware(self, target_id: int, boot_delay: float = 0.0) -> bool: """ Reset to firmware The parameter target_id corresponds to the device to reset. @@ -138,7 +138,7 @@ def reset_to_firmware(self, target_id: int) -> bool: continue pk = CRTPPacket(0xff, [target_id, 0xf0, 0x01]) self.link.send_packet(pk) - time.sleep(1) + time.sleep(1 + boot_delay) return True time.sleep(0.1) From 3c0f8e4d5cc811c8d4905f1e1e979473e9a5425d Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Tue, 18 Feb 2025 12:08:57 +0100 Subject: [PATCH 756/861] Fix deprecation warnings for params --- cflib/crazyflie/param.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 25ea37a0b..b4f827c4a 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -333,7 +333,7 @@ def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ - if not self._initialized.isSet(): + if not self._initialized.is_set(): if self.cf.is_called_by_incoming_handler_thread(): raise Exception('Can not set parameter from callback until fully connected.') if not self._initialized.wait(timeout=60): @@ -371,7 +371,7 @@ def get_value(self, complete_name, timeout=60): Read a value for the supplied parameter. This can block for a period of time if the parameter values have not been fetched yet. """ - if not self._initialized.isSet(): + if not self._initialized.is_set(): if self.cf.is_called_by_incoming_handler_thread(): raise Exception('Can not get parameter from callback until fully connected.') if not self._initialized.wait(timeout=60): From 354a3fcaedc2b9a6c6d1a0c71eb383858ca4d426 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 18 Feb 2025 14:43:01 +0100 Subject: [PATCH 757/861] Increase waiting for boot delay time for AI-deck to 6 seconds --- cflib/bootloader/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index e7d67dac9..1d5ade295 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -277,7 +277,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo # Reset to firmware mode if any(deck.target == 'bcAI:gap8' for deck in deck_targets): - self.reset_to_firmware(boot_delay=5.0) + self.reset_to_firmware(boot_delay=6.0) self.close() time.sleep(7) else: From b7ead1937609995ea6ab7f9a3d87fd2d269eac73 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 18 Feb 2025 15:24:16 +0100 Subject: [PATCH 758/861] Always wait 5 seconds for reset to firmware mode when triggered from bootloader mode. --- cflib/bootloader/__init__.py | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 1d5ade295..89ebcfdf3 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -276,14 +276,9 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self.progress_cb('Restarting firmware to update decks.', int(0)) # Reset to firmware mode - if any(deck.target == 'bcAI:gap8' for deck in deck_targets): - self.reset_to_firmware(boot_delay=6.0) - self.close() - time.sleep(7) - else: - self.reset_to_firmware() - self.close() - time.sleep(2) + self.reset_to_firmware(boot_delay=5.0) + self.close() + time.sleep(2) # Flash all decks and reboot after each deck current_index = 0 From e17a5068cbd6f20abbd8e57849dcbdb44e0bab5e Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 19 Feb 2025 13:12:12 +0100 Subject: [PATCH 759/861] Fixed some example issues --- examples/autonomy/autonomousSequence.py | 20 ++++++++++++++++++-- examples/autonomy/position_commander_demo.py | 2 +- examples/positioning/flowsequenceSync.py | 2 +- examples/swarm/swarmSequence.py | 3 ++- 4 files changed, 22 insertions(+), 5 deletions(-) diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index d1b3bc5a5..ed7445a49 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -27,8 +27,8 @@ one every 5 seconds. This example is intended to work with the Loco Positioning System in TWR TOA -mode. It aims at documenting how to set the Crazyflie in position control mode -and how to send setpoints. +mode and with the Lighthouse Positioning System. It aims at documenting how +to set the Crazyflie in position control mode and how to send setpoints. """ import time @@ -56,6 +56,19 @@ ] +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(f'take off at {position[2]}') + + for i in range(steps): + cf.commander.send_velocity_world_setpoint(0, 0, vz, 0) + time.sleep(sleep_time) + + def position_callback(timestamp, data, logconf): x = data['kalman.stateX'] y = data['kalman.stateY'] @@ -81,6 +94,9 @@ def run_sequence(scf, sequence): cf.platform.send_arming_request(True) time.sleep(1.0) + take_off(cf, sequence[0]) + time.sleep(1.0) + for position in sequence: print('Setting position {}'.format(position)) for i in range(50): diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py index b8597e9d8..1b0f93796 100644 --- a/examples/autonomy/position_commander_demo.py +++ b/examples/autonomy/position_commander_demo.py @@ -29,7 +29,7 @@ Change the URI variable to your Crazyflie configuration. """ -from time import time +import time import cflib.crtp from cflib.crazyflie import Crazyflie diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py index 4617c41ee..4f62db71b 100644 --- a/examples/positioning/flowsequenceSync.py +++ b/examples/positioning/flowsequenceSync.py @@ -24,7 +24,7 @@ """ 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 (and designed for) the flow deck. +tested with the flow deck and the lighthouse positioning system. Change the URI variable to your Crazyflie configuration. """ diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py index cd4ee640d..132dfb81c 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -215,7 +215,6 @@ def run_sequence(scf, sequence): try: cf = scf.cf - arm(cf) take_off(cf, sequence[0]) for position in sequence: print('Setting position {}'.format(position)) @@ -249,4 +248,6 @@ def run_sequence(scf, sequence): print('Waiting for parameters to be downloaded...') swarm.parallel(wait_for_param_download) + swarm.parallel(arm) + swarm.parallel(run_sequence, args_dict=seq_args) From f4c6cfd53e7ce68806da9b00b070b3aad97b68bc Mon Sep 17 00:00:00 2001 From: Nagesh Eranki Date: Wed, 5 Mar 2025 17:32:01 -0600 Subject: [PATCH 760/861] Update sbs_swarm_interface.md Clean up a few typos in the example code. deactivateBitMask should be deactivate_led_bit_mask and so on. --- docs/user-guides/sbs_swarm_interface.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index a7559ae02..ebe7a909f 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -58,9 +58,9 @@ def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) def light_check(scf): - activateBitMask(scf) + activate_led_bit_mask(scf) time.sleep(2) - deactivateBitMask(scf) + deactivate_led_bit_mask(scf) ``` `light_check` will light up a copter red for 2 seconds and then return them to normal. @@ -162,9 +162,9 @@ def deactivate_led_bit_mask(scf): scf.cf.param.set_value('led.bitmask', 0) def light_check(scf): - activateBitMask(scf) + activate_led_bit_mask(scf) time.sleep(2) - deactivateBitMask(scf) + deactivate_led_bit_mask(scf) time.sleep(2) def take_off(scf): From f19691e1b3d8fd374b42b55eaa084bd44c3a5b95 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 18 Mar 2025 17:04:32 +0100 Subject: [PATCH 761/861] Increase ESP flashing timeout by 5s to match GAP8. When flashing the ESP, we also reboot the GAP8, requiring the same delay. However, since flashing the ESP firmware does not necessarily mean we are flashing the GAP8, we must explicitly check if the ESP is among our targets to apply the timeout correctly. This ensures the GAP8 is never in an incorrect state. This fixes #526. Also adds a missing boot delay in the reset-to-firmware call, originally intended in b7ead1937609995ea6ab7f9a3d87fd2d269eac73. --- cflib/bootloader/__init__.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index 89ebcfdf3..e21512d44 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -289,7 +289,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self.progress_cb('Deck updated! Restarting...', int(100)) if current_index != -1: PowerSwitch(self.clink).reboot_to_fw() - if any(deck.target == 'bcAI:gap8' for deck in deck_targets): + if any(deck.target in ['bcAI:gap8', 'bcAI:esp'] for deck in deck_targets): time.sleep(7) else: time.sleep(2) @@ -337,7 +337,7 @@ def flash_full(self, cf: Optional[Crazyflie] = None, if filename is not None: self.flash(filename, targets, cf, enable_console_log=enable_console_log) - self.reset_to_firmware() + self.reset_to_firmware(boot_delay=5.0) def _get_flash_artifacts_from_zip(self, filename): if not zipfile.is_zipfile(filename): @@ -604,7 +604,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis self.progress_cb(f'Updating deck {deck.name}', 0) # Test and wait for the deck to be started - if any(deck.name == 'bcAI:gap8' for deck in decks.values()): + if any(deck.name in ['bcAI:gap8', 'bcAI:esp'] for deck in decks.values()): timeout_time = time.time() + 9 else: timeout_time = time.time() + 4 @@ -634,7 +634,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis continue # Wait for bootloader to be ready - if any(deck.name == 'bcAI:gap8' for deck in decks.values()): + if any(deck.name in ['bcAI:gap8', 'bcAI:esp'] for deck in decks.values()): timeout_time = time.time() + 9 else: timeout_time = time.time() + 4 From 236a34405c8275f220469e28f71ab67f01f1911b Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 19 Mar 2025 14:23:58 +0100 Subject: [PATCH 762/861] Refactor boot delay handling into `_get_boot_delay()` method Replaced the previous ad-hoc boot delay checks with a dedicated `_get_boot_delay()` method. This function reads deck memory before flashing and determines if an AI deck is attached, returning the appropriate boot delay. This improves clarity, reduces the risk of missing a required delay, and ensures consistency in handling delayed boots. The method is now called within `flash_full`, ensuring the correct boot delay is used in all standard flashing operations. Responsibility for handling the delay remains with the caller in other cases, making the function more flexible while maintaining internal consistency. --- cflib/bootloader/__init__.py | 70 +++++++++++++++++++++++++++--------- 1 file changed, 53 insertions(+), 17 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index e21512d44..e6f648f3f 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -178,7 +178,48 @@ def _get_provided_nrf51_bl_version(self, flash_artifacts: List[FlashArtifact]): return provided_nrf_bl_version - def flash(self, filename: str, targets: List[Target], cf=None, enable_console_log: Optional[bool] = False): + def _get_boot_delay(self, cf: Optional[Crazyflie] = None) -> float: + """ + Determines the boot delay for the Crazyflie. + This method calculates the boot delay based on the presence of specific decks. + If the AI deck is attached, a longer boot delay is used. + @return: The boot delay in seconds. Returns -1 if no deck memory is found. + @rtype: float + @raises RuntimeError: If there is a failure in reading the decks. + """ + + if cf is not None and cf.link: + cf.close_link() + + try: + with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: + deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) + deck_mems_count = len(deck_mems) + if deck_mems_count == 0: + return -1 + + mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0]) + try: + decks = mgr.query_decks() + except RuntimeError as e: + if self.progress_cb: + message = f'Failed to read decks: {str(e)}' + self.progress_cb(message, 0) + logger.error(message) + time.sleep(2) + raise RuntimeError(message) + + if any(deck.name in ['bcAI:gap8', 'bcAI:esp'] for deck in decks.values()): + return 5.0 + except Exception as e: + # If we fail to connect to the Crazyflie in firmware mode, we assume the AI-deck is attached + print(f'Failed to connect to Crazyflie in firmware mode: {str(e)}. Setting boot delay to 5.0 seconds') + return 5.0 # AI-deck may be attached + + return 0.0 + + def flash(self, filename: str, targets: List[Target], cf=None, enable_console_log: Optional[bool] = False, + boot_delay: Optional[float] = 0.0): # Separate flash targets from decks platform = self._get_platform_id() flash_targets = [t for t in targets if t.platform == platform] @@ -276,7 +317,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self.progress_cb('Restarting firmware to update decks.', int(0)) # Reset to firmware mode - self.reset_to_firmware(boot_delay=5.0) + self.reset_to_firmware(boot_delay=boot_delay) self.close() time.sleep(2) @@ -284,15 +325,13 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo current_index = 0 while current_index != -1: current_index = self._flash_deck_incrementally(deck_artifacts, deck_targets, current_index, - enable_console_log=enable_console_log) + enable_console_log=enable_console_log, + boot_delay=boot_delay) if self.progress_cb: self.progress_cb('Deck updated! Restarting...', int(100)) if current_index != -1: PowerSwitch(self.clink).reboot_to_fw() - if any(deck.target in ['bcAI:gap8', 'bcAI:esp'] for deck in deck_targets): - time.sleep(7) - else: - time.sleep(2) + time.sleep(2.0 + boot_delay) # Put the crazyflie back in Bootloader mode to exit the function in the same state we entered it self.start_bootloader(warm_boot=True, cf=cf) @@ -321,6 +360,9 @@ def flash_full(self, cf: Optional[Crazyflie] = None, Flash .zip or bin .file to list of targets. Reset to firmware when done. """ + # Get the required boot delay + boot_delay = self._get_boot_delay(cf=cf) + if progress_cb is not None: self.progress_cb = progress_cb if terminate_flash_cb is not None: @@ -336,7 +378,7 @@ def flash_full(self, cf: Optional[Crazyflie] = None, info_cb(self.protocol_version, connected) if filename is not None: - self.flash(filename, targets, cf, enable_console_log=enable_console_log) + self.flash(filename, targets, cf, enable_console_log=enable_console_log, boot_delay=boot_delay) self.reset_to_firmware(boot_delay=5.0) def _get_flash_artifacts_from_zip(self, filename): @@ -549,7 +591,7 @@ def console_callback(self, text: str): print(text, end='') def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: List[Target], start_index: int, - enable_console_log: Optional[bool] = False): + enable_console_log: Optional[bool] = False, boot_delay=0.0): flash_all_targets = len(targets) == 0 if self.progress_cb: self.progress_cb('Identifying deck to be updated', 0) @@ -604,10 +646,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis self.progress_cb(f'Updating deck {deck.name}', 0) # Test and wait for the deck to be started - if any(deck.name in ['bcAI:gap8', 'bcAI:esp'] for deck in decks.values()): - timeout_time = time.time() + 9 - else: - timeout_time = time.time() + 4 + timeout_time = time.time() + 4. + boot_delay while not deck.is_started: if time.time() > timeout_time: raise RuntimeError(f'Deck {deck.name} did not start') @@ -634,10 +673,7 @@ def _flash_deck_incrementally(self, artifacts: List[FlashArtifact], targets: Lis continue # Wait for bootloader to be ready - if any(deck.name in ['bcAI:gap8', 'bcAI:esp'] for deck in decks.values()): - timeout_time = time.time() + 9 - else: - timeout_time = time.time() + 4 + timeout_time = time.time() + 4. + boot_delay while not deck.is_bootloader_active: if time.time() > timeout_time: raise RuntimeError(f'Deck {deck.name} did not enter bootloader mode') From 2c7527494d0928bd50de77f6e85e857341ace07a Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 19 Mar 2025 15:53:01 +0100 Subject: [PATCH 763/861] Suppress connection errors when checking Crazyflie state in boot delay logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Refactor boot delay logic to avoid errors when determining the Crazyflie’s state. Since we do not explicitly check whether the Crazyflie is connected, disconnected, or already in bootloader mode before attempting a connection, we now simply try cf.link first and fall back to SyncCrazyflie if needed. Failures in these attempts are handled silently, preventing misleading error messages when the Crazyflie is already in bootloader mode. If both attempts fail, we assume bootloader mode and apply a 5-second delay. --- cflib/bootloader/__init__.py | 69 +++++++++++++++++------------------- 1 file changed, 32 insertions(+), 37 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index e6f648f3f..ff8b9ba36 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -178,45 +178,40 @@ def _get_provided_nrf51_bl_version(self, flash_artifacts: List[FlashArtifact]): return provided_nrf_bl_version - def _get_boot_delay(self, cf: Optional[Crazyflie] = None) -> float: - """ - Determines the boot delay for the Crazyflie. - This method calculates the boot delay based on the presence of specific decks. - If the AI deck is attached, a longer boot delay is used. - @return: The boot delay in seconds. Returns -1 if no deck memory is found. - @rtype: float - @raises RuntimeError: If there is a failure in reading the decks. - """ - - if cf is not None and cf.link: - cf.close_link() + def _get_boot_delay(self, cf: Optional[Crazyflie] = None): + """Determines the boot delay based on AI-deck presence or failure to connect.""" + try: + # First try using an existing link + if cf.link: + return 5.0 if self._has_ai_deck(cf) else 0.0 + except Exception: + # Failed to connect using existing link, try opening a new link + pass try: - with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: - deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) - deck_mems_count = len(deck_mems) - if deck_mems_count == 0: - return -1 - - mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0]) - try: - decks = mgr.query_decks() - except RuntimeError as e: - if self.progress_cb: - message = f'Failed to read decks: {str(e)}' - self.progress_cb(message, 0) - logger.error(message) - time.sleep(2) - raise RuntimeError(message) - - if any(deck.name in ['bcAI:gap8', 'bcAI:esp'] for deck in decks.values()): - return 5.0 - except Exception as e: - # If we fail to connect to the Crazyflie in firmware mode, we assume the AI-deck is attached - print(f'Failed to connect to Crazyflie in firmware mode: {str(e)}. Setting boot delay to 5.0 seconds') - return 5.0 # AI-deck may be attached - - return 0.0 + with SyncCrazyflie(cf.uri, cf=Crazyflie()) as scf: + return 5.0 if self._has_ai_deck(scf.cf) else 0.0 + except Exception: + # If we fail to connect using SyncCrazyflie, assume the Crazyflie may be in bootloader mode + return 5.0 + + def _has_ai_deck(self, cf): + deck_mems = cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) + if not deck_mems: + raise RuntimeError('Failed to read memory: No deck memory found') + + mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0]) + try: + decks = mgr.query_decks() + except RuntimeError as e: + if self.progress_cb: + message = f'Failed to read decks: {str(e)}' + self.progress_cb(message, 0) + print(message) + time.sleep(2) + raise RuntimeError(message) + + return any(deck.name in ['bcAI:gap8', 'bcAI:esp'] for deck in decks.values()) def flash(self, filename: str, targets: List[Target], cf=None, enable_console_log: Optional[bool] = False, boot_delay: Optional[float] = 0.0): From 01e4fd3b6b2fe162e1d2d5b0deb9a647063c8136 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 19 Mar 2025 16:15:34 +0100 Subject: [PATCH 764/861] Send continuous zero setpoints until landing in ramp examples In ramp examples where motors are gradually ramped, the supervisor assumes the drone is flying and requires continuous updated setpoints. Previously, a single zero setpoint was sent before sleeping, which leads to supervisor intervention due to missing regular setpoints. Now, zero setpoints are continuously sent for 3 seconds, and the drone is considered landed, preventing locking. Currently, this does not directly check with the supervisor if the drone has landed. Investigating a way to do this revealed that it is not straightforward. In the client, we check the supervisor states through a bitfield check, but there is no direct and easy way to access this information from the library. I will open a new issue to propose adding supervisor state reading functionality to the library, making it easier to access this information directly. --- examples/motors/multiramp.py | 9 +++++---- examples/motors/ramp.py | 9 +++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/examples/motors/multiramp.py b/examples/motors/multiramp.py index df77fec51..3adfc8cdf 100644 --- a/examples/motors/multiramp.py +++ b/examples/motors/multiramp.py @@ -101,10 +101,11 @@ def _ramp_motors(self): if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult - self._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) + 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) self._cf.close_link() diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index a91d87bac..c95f5cdea 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -99,10 +99,11 @@ def _ramp_motors(self): if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult - self._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(1) + 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) self._cf.close_link() From 5fb3f592d2754dcb51e227dec66d69c483d8f8c8 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 19 Mar 2025 16:35:16 +0100 Subject: [PATCH 765/861] Restore comment about sleeping before closing the link MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Re-added a slightly modified comment explaining that sleeping before closing the link ensures the last packet is sent, as the message queue is not flushed before closing. While it’s somewhat arbitrary that this information is only found in this example, it’s still valuable to have it documented somewhere. In practice, we don’t care as much about this delay anymore since the new method’s delay is more focused on the setpoint sending loop rather than guaranteeing message delivery, but the comment is still worth keeping. --- examples/motors/multiramp.py | 3 +++ examples/motors/ramp.py | 3 +++ 2 files changed, 6 insertions(+) diff --git a/examples/motors/multiramp.py b/examples/motors/multiramp.py index 3adfc8cdf..8c3e9340f 100644 --- a/examples/motors/multiramp.py +++ b/examples/motors/multiramp.py @@ -106,6 +106,9 @@ def _ramp_motors(self): # 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() diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index c95f5cdea..b0f78831f 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -104,6 +104,9 @@ def _ramp_motors(self): # 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() From 185d5179b25102976bc34a6339d3ab2e4b655343 Mon Sep 17 00:00:00 2001 From: Aris Date: Mon, 24 Mar 2025 13:58:52 +0100 Subject: [PATCH 766/861] Solved arming issue on sbs motion commander --- docs/user-guides/sbs_motion_commander.md | 50 +++++++++---------- examples/step-by-step/sbs_motion_commander.py | 4 ++ 2 files changed, 29 insertions(+), 25 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 883a7b6d8..e17100d2c 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -55,9 +55,6 @@ Since this tutorial won't be a table top tutorial like last time, but an actual We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...SyncCrazyflie(...)` in `__main__` ```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) scf.cf.param.add_update_callback(group="deck", name="bcFlow2", cb=param_deck_flow) @@ -123,9 +120,6 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -139,14 +133,15 @@ So now we are going to start up the SyncCrazyflie and start a function in the `_ ```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) if not deck_attached_event.wait(timeout=5): print('No flow deck detected!') sys.exit(1) + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + take_off_simple(scf) ``` See that we are now using `deck_attached_event.wait()`? If this returns false, the function will not be called and the crazyflie will not take off. @@ -225,9 +220,6 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -237,6 +229,10 @@ if __name__ == '__main__': print('No flow deck detected!') sys.exit(1) + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + take_off_simple(scf) ``` @@ -317,9 +313,6 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -329,6 +322,10 @@ if __name__ == '__main__': print('No flow deck detected!') sys.exit(1) + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + move_linear_simple(scf) ``` @@ -423,9 +420,6 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -441,6 +435,10 @@ if __name__ == '__main__': print('No flow deck detected!') sys.exit(1) + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + logconf.start() move_linear_simple(scf) @@ -539,9 +537,6 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -557,6 +552,10 @@ if __name__ == '__main__': print('No flow deck detected!') sys.exit(1) + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + logconf.start() move_box_limit(scf) logconf.stop() @@ -669,9 +668,6 @@ if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) @@ -687,6 +683,10 @@ if __name__ == '__main__': print('No flow deck detected!') sys.exit(1) + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + logconf.start() move_box_limit(scf) logconf.stop() @@ -697,4 +697,4 @@ You're done! The full code of this tutorial can be found in the example/step-by- ## What is next ? -Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. +Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the [motion_commander_demo.py](https://github.com/bitcraze/crazyflie-lib-python/blob/master/examples/step-by-step/sbs_motion_commander.py) in the example folder of the cflib if you would like to see what the commander can do. diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index 376d0b0d1..a477455c1 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -125,6 +125,10 @@ def param_deck_flow(_, value_str): print('No flow deck detected!') sys.exit(1) + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + logconf.start() take_off_simple(scf) From b42a989d9f127f4772521f7c1ba809233914a8bc Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 2 Apr 2025 17:10:25 +0200 Subject: [PATCH 767/861] Removed Lists from being global variables --- examples/step-by-step/sbs_motion_commander.py | 1 - examples/swarm/asynchronizedSwarm.py | 1 - examples/swarm/leader-follower.py | 3 +-- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index a477455c1..b557cce2d 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -91,7 +91,6 @@ def take_off_simple(scf): def log_pos_callback(timestamp, data, logconf): print(data) - global position_estimate position_estimate[0] = data['stateEstimate.x'] position_estimate[1] = data['stateEstimate.y'] diff --git a/examples/swarm/asynchronizedSwarm.py b/examples/swarm/asynchronizedSwarm.py index 02fd941a4..e80ab96a1 100644 --- a/examples/swarm/asynchronizedSwarm.py +++ b/examples/swarm/asynchronizedSwarm.py @@ -74,7 +74,6 @@ def arm(scf): def position_callback(uri, data): - global pos1, pos2 if uri == URI1: pos1[0] = data['stateEstimate.x'] pos1[1] = data['stateEstimate.y'] diff --git a/examples/swarm/leader-follower.py b/examples/swarm/leader-follower.py index 2505822e7..552133af3 100644 --- a/examples/swarm/leader-follower.py +++ b/examples/swarm/leader-follower.py @@ -106,8 +106,7 @@ def pos_to_vel(x1, y1, x2, y2, dist): def position_callback(uri, data): - global yaw1 - global x1, y1, z1, x2, y2, z2, d + global d if uri == URI1: # Follower x1.append(data['stateEstimate.x']) y1.append(data['stateEstimate.y']) From 8bbcee72d05bc05854afba30324c1907713a4800 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 2 Apr 2025 16:51:46 +0200 Subject: [PATCH 768/861] Update the flake8 version. it has been removed from the original repo. We where using a very very old version so we where missing some features like global variable checking --- .pre-commit-config.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b34f05004..92355ecdf 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -14,8 +14,11 @@ repos: - id: detect-private-key - id: double-quote-string-fixer - id: end-of-file-fixer - - id: flake8 - id: requirements-txt-fixer +- repo: https://github.com/PyCQA/flake8 + rev: 16f5f28a384f0781bebb37a08aa45e65b9526c50 + hooks: + - id: flake8 - repo: https://github.com/asottile/reorder_python_imports rev: ab609b9b982729dfc287b4e75963c0c4de254a31 hooks: From d57702a8e765ce04876f5b4ff27c4da6faae86d6 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 12:26:16 +0200 Subject: [PATCH 769/861] Fixing some typing errors --- .../lighthouse_initial_estimator.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 5d784b448..738aa6795 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -40,8 +40,8 @@ class LighthouseInitialEstimator: OUTLIER_DETECTION_ERROR = 0.5 @classmethod - def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> tuple( - LhBsCfPoses, list[LhCfPoseSample]): + def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> tuple[ + LhBsCfPoses, list[LhCfPoseSample]]: """ Make a rough estimate of the poses of all base stations and CF poses found in the samples. @@ -83,7 +83,7 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A @classmethod def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike - ) -> dict[tuple(int, int), npt.NDArray]: + ) -> dict[tuple[int, int], npt.NDArray]: """ Find the pose of all base stations, in the reference frame of other base stations. @@ -101,7 +101,7 @@ def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions (2, 1) contains the position of base station 1, in the base station 2 reference frame. """ - position_permutations: dict[tuple(int, int), list[list[npt.ArrayLike]]] = {} + position_permutations: dict[tuple[int, int], list[list[npt.ArrayLike]]] = {} for sample in matched_samples: solutions: dict[int, tuple[Pose, Pose]] = {} for bs, angles in sample.angles_calibrated.items(): @@ -151,8 +151,8 @@ def _add_solution_permutations(cls, solutions: dict[int, tuple[Pose, Pose]], @classmethod def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike, - bs_positions: dict[tuple(int, int), npt.NDArray]) -> tuple(list[dict[int, Pose]], - list[LhCfPoseSample]): + bs_positions: dict[tuple[int, int], npt.NDArray]) -> tuple[list[dict[int, Pose]], + list[LhCfPoseSample]]: """ Estimate the base station poses in the Crazyflie reference frames, for each sample. @@ -223,15 +223,15 @@ def _choose_solutions(cls, solutions_1: tuple[Pose, Pose], solutions_2: tuple[Po return best1, best2 @classmethod - def _find_most_likely_positions(cls, position_permutations: dict[tuple(int, int), - list[list[npt.ArrayLike]]]) -> dict[tuple(int, int), npt.NDArray]: + def _find_most_likely_positions(cls, position_permutations: dict[tuple[int, int], + list[list[npt.ArrayLike]]]) -> dict[tuple[int, int], npt.NDArray]: """ Find the most likely base station positions from all the possible permutations. Sort the permutations into buckets based on how close they are to the solutions in the first sample. Solutions - that are "too" far away and distcarded. The bucket with the most samples in, is considerred the best. + that are "too" far away and discarded. The bucket with the most samples in, is considered the best. """ - result: dict[tuple(int, int), npt.NDArray] = {} + result: dict[tuple[int, int], npt.NDArray] = {} for pair, position_lists in position_permutations.items(): # Use first as reference to map the others to @@ -367,7 +367,7 @@ def q_average(Q, W=None): return Pose.from_quat(R_quat=average_quaternion, t_vec=average_pos) @classmethod - def _estimate_cf_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: list[Pose]) -> list[Pose]: + def _estimate_cf_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: dict[int, Pose]) -> list[Pose]: """ Find the pose of the Crazyflie in all samples, based on the base station poses. """ From 19d90d5de2e31c0257fa538f767566035144c93d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 13:13:25 +0200 Subject: [PATCH 770/861] Spelling --- cflib/localization/lighthouse_geometry_solver.py | 2 +- cflib/localization/lighthouse_sample_matcher.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index a58082e02..949d70b50 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -78,7 +78,7 @@ def __init__(self) -> None: # Information about errors in the solution self.error_info = {} - # Indicates if the solution coverged (True). + # Indicates if the solution converged (True). # If it did not converge, the solution is probably not good enough to use self.success = False diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py index a7bd06e19..afe26fb8f 100644 --- a/cflib/localization/lighthouse_sample_matcher.py +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -29,7 +29,7 @@ class LighthouseSampleMatcher: """Utility class to match samples of measurements from multiple lighthouse base stations. Assuming that the Crazyflie was moving when the measurements were recorded, - samples that were meassured aproximately at the same position are aggregated into + samples that were measured approximately at the same position are aggregated into a list of LhCfPoseSample. Matching is done using the timestamp and a maximum time span. """ From a1fe154f60f481671122729a102fc3488cba731f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 13:16:07 +0200 Subject: [PATCH 771/861] Introduced LhException type --- cflib/localization/lighthouse_initial_estimator.py | 5 +++-- cflib/localization/lighthouse_types.py | 4 ++++ test/localization/test_lighthouse_initial_estimator.py | 5 +++-- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 738aa6795..d8a320c4e 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -28,6 +28,7 @@ from cflib.localization.lighthouse_types import LhBsCfPoses from cflib.localization.lighthouse_types import LhCfPoseSample from cflib.localization.lighthouse_types import Pose +from cflib.localization.lighthouse_types import LhException class LighthouseInitialEstimator: @@ -70,7 +71,7 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A break if reference_bs_pose is None: - raise Exception('Too little data, no reference') + raise LhException('Too little data, no reference') bs_poses: dict[int, Pose] = {bs_id: reference_bs_pose} # Calculate the pose of the remaining base stations, based on the pose of the first CF @@ -342,7 +343,7 @@ def _estimate_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], b break if len(to_find) == remaining: - raise RuntimeError('Can not link positions between all base stations') + raise LhException('Can not link positions between all base stations') remaining = len(to_find) diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index ec8d34c5f..885720b0c 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -182,3 +182,7 @@ class LhDeck4SensorPositions: (_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0)]) diagonal_distance = np.sqrt(_sensor_distance_length ** 2 + _sensor_distance_length ** 2) + + +class LhException(RuntimeError): + """Base exception for lighthouse exceptions""" diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py index f0fd7cb8d..259dadf43 100644 --- a/test/localization/test_lighthouse_initial_estimator.py +++ b/test/localization/test_lighthouse_initial_estimator.py @@ -28,6 +28,7 @@ from cflib.localization.lighthouse_types import LhCfPoseSample from cflib.localization.lighthouse_types import LhDeck4SensorPositions from cflib.localization.lighthouse_types import Pose +from cflib.localization.lighthouse_types import LhException class TestLighthouseInitialEstimator(LighthouseTestBase): @@ -44,7 +45,7 @@ def test_that_one_bs_pose_raises_exception(self): # Test # Assert - with self.assertRaises(Exception): + with self.assertRaises(LhException): LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) def test_that_two_bs_poses_in_same_sample_are_found(self): @@ -174,5 +175,5 @@ def test_that_raises_for_isolated_bs(self): # Test # Assert - with self.assertRaises(Exception): + with self.assertRaises(LhException): LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) From 43cd6bb126ae1206e237e50a5558ec5ce0c2d298 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 13:19:58 +0200 Subject: [PATCH 772/861] Introduced named tuples for base station pair ids and poses --- .../lighthouse_initial_estimator.py | 44 +++++++++++-------- 1 file changed, 26 insertions(+), 18 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index d8a320c4e..3b1def8f5 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -23,6 +23,7 @@ import numpy as np import numpy.typing as npt +from collections import namedtuple from .ippe_cf import IppeCf from cflib.localization.lighthouse_types import LhBsCfPoses @@ -31,6 +32,13 @@ from cflib.localization.lighthouse_types import LhException +# A type representing the ids of a pair of base stations +BsPairIds = namedtuple('BsPairIds', ['bs1', 'bs2']) + +# A type representing the poses of a pair of base stations +BsPairPoses = namedtuple('BsPairPoses', ['bs1', 'bs2']) + + class LighthouseInitialEstimator: """ Make initial estimates of base station and CF poses using IPPE (analytical solution). @@ -84,7 +92,7 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A @classmethod def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike - ) -> dict[tuple[int, int], npt.NDArray]: + ) -> dict[BsPairIds, npt.NDArray]: """ Find the pose of all base stations, in the reference frame of other base stations. @@ -102,9 +110,9 @@ def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions (2, 1) contains the position of base station 1, in the base station 2 reference frame. """ - position_permutations: dict[tuple[int, int], list[list[npt.ArrayLike]]] = {} + position_permutations: dict[BsPairIds, list[list[npt.ArrayLike]]] = {} for sample in matched_samples: - solutions: dict[int, tuple[Pose, Pose]] = {} + solutions: dict[int, BsPairPoses] = {} for bs, angles in sample.angles_calibrated.items(): projections = angles.projection_pair_list() estimates_ref_bs = IppeCf.solve(sensor_positions, projections) @@ -116,8 +124,8 @@ def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions return cls._find_most_likely_positions(position_permutations) @classmethod - def _add_solution_permutations(cls, solutions: dict[int, tuple[Pose, Pose]], - position_permutations: dict[tuple[int, int], list[list[npt.ArrayLike]]]): + def _add_solution_permutations(cls, solutions: dict[int, BsPairPoses], + position_permutations: dict[BsPairIds, list[list[npt.ArrayLike]]]): """ Add the possible permutations of base station positions for a sample to a collection of aggregated positions. The aggregated collection contains base station positions in the reference frame of other base stations. @@ -144,7 +152,7 @@ def _add_solution_permutations(cls, solutions: dict[int, tuple[Pose, Pose]], pose3 = solution_i[1].inv_rotate_translate_pose(solution_j[0]) pose4 = solution_i[1].inv_rotate_translate_pose(solution_j[1]) - pair = (id_i, id_j) + pair = BsPairIds(id_i, id_j) if pair not in position_permutations: position_permutations[pair] = [] position_permutations[pair].append([pose1.translation, pose2.translation, @@ -152,8 +160,8 @@ def _add_solution_permutations(cls, solutions: dict[int, tuple[Pose, Pose]], @classmethod def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike, - bs_positions: dict[tuple[int, int], npt.NDArray]) -> tuple[list[dict[int, Pose]], - list[LhCfPoseSample]]: + bs_positions: dict[BsPairIds, npt.NDArray]) -> tuple[list[dict[int, Pose]], + list[LhCfPoseSample]]: """ Estimate the base station poses in the Crazyflie reference frames, for each sample. @@ -171,7 +179,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position cleaned_matched_samples: list[LhCfPoseSample] = [] for sample in matched_samples: - solutions: dict[int, tuple[Pose, Pose]] = {} + solutions: dict[int, BsPairPoses] = {} for bs, angles in sample.angles_calibrated.items(): projections = angles.projection_pair_list() estimates_ref_bs = IppeCf.solve(sensor_positions, projections) @@ -183,7 +191,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position first = ids[0] for other in ids[1:]: - pair = (first, other) + pair = BsPairIds(first, other) expected = bs_positions[pair] firstPose, otherPose = cls._choose_solutions(solutions[first], solutions[other], expected) @@ -201,8 +209,8 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position return result, cleaned_matched_samples @classmethod - def _choose_solutions(cls, solutions_1: tuple[Pose, Pose], solutions_2: tuple[Pose, Pose], - expected: npt.ArrayLike) -> tuple[Pose, Pose]: + def _choose_solutions(cls, solutions_1: BsPairPoses, solutions_2: BsPairPoses, + expected: npt.ArrayLike) -> BsPairPoses: """Pick the base pose solutions for a pair of base stations, based on the position in expected""" min_dist = 100000.0 @@ -224,15 +232,15 @@ def _choose_solutions(cls, solutions_1: tuple[Pose, Pose], solutions_2: tuple[Po return best1, best2 @classmethod - def _find_most_likely_positions(cls, position_permutations: dict[tuple[int, int], - list[list[npt.ArrayLike]]]) -> dict[tuple[int, int], npt.NDArray]: + def _find_most_likely_positions(cls, position_permutations: dict[BsPairIds, + list[list[npt.ArrayLike]]]) -> dict[BsPairIds, npt.NDArray]: """ Find the most likely base station positions from all the possible permutations. Sort the permutations into buckets based on how close they are to the solutions in the first sample. Solutions that are "too" far away and discarded. The bucket with the most samples in, is considered the best. """ - result: dict[tuple[int, int], npt.NDArray] = {} + result: dict[BsPairIds, npt.NDArray] = {} for pair, position_lists in position_permutations.items(): # Use first as reference to map the others to @@ -279,7 +287,7 @@ def _find_best_position_bucket(cls, buckets: list[list[npt.ArrayLike]]) -> npt.N return pos @classmethod - def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf.Solution]) -> tuple[Pose, Pose]: + def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf.Solution]) -> BsPairPoses: """ Convert the two ippe solutions from the base station reference frame to the CF reference frame """ @@ -289,12 +297,12 @@ def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf. rot_2 = estimates_ref_bs[1].R.transpose() t_2 = np.dot(rot_2, -estimates_ref_bs[1].t) - return Pose(rot_1, t_1), Pose(rot_2, t_2) + return BsPairPoses(Pose(rot_1, t_1), Pose(rot_2, t_2)) @classmethod def _estimate_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: dict[int, Pose]) -> None: """ - Based on one base station pose, estimate the other base staion poses. + Based on one base station pose, estimate the other base station poses. The process is iterative and runs until all poses are found. Assume we know the pose of base station 0, and we have information of base station pairs (0, 2) and (2, 3), from this we can first derive the pose of 2 and after From 92ba8d162dca5b2b51c5d455ab8f84b4bb58aae3 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 13:35:15 +0200 Subject: [PATCH 773/861] cleaned up _angles_to_poses() --- .../lighthouse_initial_estimator.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 3b1def8f5..09707edb2 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -189,20 +189,21 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position poses: dict[int, Pose] = {} ids = sorted(solutions.keys()) first = ids[0] + is_sample_valid = True for other in ids[1:]: - pair = BsPairIds(first, other) - expected = bs_positions[pair] + pair_ids = BsPairIds(first, other) + expected = bs_positions[pair_ids] - firstPose, otherPose = cls._choose_solutions(solutions[first], solutions[other], expected) - if firstPose is not None: - poses[first] = firstPose - poses[other] = otherPose + first_pose, other_pose = cls._choose_solutions(solutions[first], solutions[other], expected) + if first_pose is not None: + poses[first] = first_pose + poses[other] = other_pose else: - poses = None + is_sample_valid = False break - if poses is not None: + if is_sample_valid: result.append(poses) cleaned_matched_samples.append(sample) From f20a22dbc8d8ef4c41e9019db5a839c90205b623 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 13:37:45 +0200 Subject: [PATCH 774/861] Updated deprecated import --- test/localization/lighthouse_test_base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/localization/lighthouse_test_base.py b/test/localization/lighthouse_test_base.py index 403bcb87a..361db3f12 100644 --- a/test/localization/lighthouse_test_base.py +++ b/test/localization/lighthouse_test_base.py @@ -23,7 +23,7 @@ import numpy as np import numpy.typing as npt -from scipy.spatial.transform.rotation import Rotation +from scipy.spatial.transform import Rotation from cflib.localization.lighthouse_types import Pose From a1d24c8606b5db58fc0407b06315759006ba0f5e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 13:54:42 +0200 Subject: [PATCH 775/861] Cleaned up _choose_solutions() --- .../lighthouse_initial_estimator.py | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 09707edb2..03f90d93f 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -195,10 +195,10 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position pair_ids = BsPairIds(first, other) expected = bs_positions[pair_ids] - first_pose, other_pose = cls._choose_solutions(solutions[first], solutions[other], expected) - if first_pose is not None: - poses[first] = first_pose - poses[other] = other_pose + success, pair_poses = cls._choose_solutions(solutions[first], solutions[other], expected) + if success: + poses[pair_ids.bs1] = pair_poses.bs1 + poses[pair_ids.bs2] = pair_poses.bs2 else: is_sample_valid = False break @@ -211,12 +211,12 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position @classmethod def _choose_solutions(cls, solutions_1: BsPairPoses, solutions_2: BsPairPoses, - expected: npt.ArrayLike) -> BsPairPoses: + expected: npt.ArrayLike) -> tuple[bool, BsPairPoses]: """Pick the base pose solutions for a pair of base stations, based on the position in expected""" min_dist = 100000.0 - best1 = None - best2 = None + best = BsPairPoses(Pose(), Pose()) + success = True for solution_1 in solutions_1: for solution_2 in solutions_2: @@ -224,13 +224,12 @@ def _choose_solutions(cls, solutions_1: BsPairPoses, solutions_2: BsPairPoses, dist = np.linalg.norm(expected - pose_second_bs_ref_fr_first.translation) if dist < min_dist: min_dist = dist - best1 = solution_1 - best2 = solution_2 + best = BsPairPoses(solution_1, solution_2) if min_dist > cls.OUTLIER_DETECTION_ERROR: - return None, None + success = False - return best1, best2 + return success, best @classmethod def _find_most_likely_positions(cls, position_permutations: dict[BsPairIds, From 3a6308df9b565fe38c8064aa8a0cf331a2c28d17 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 14:12:34 +0200 Subject: [PATCH 776/861] Use typed named tuples --- .../localization/lighthouse_initial_estimator.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 03f90d93f..8d30de53b 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -21,9 +21,9 @@ # along with this program. If not, see . from __future__ import annotations +from typing import NamedTuple import numpy as np import numpy.typing as npt -from collections import namedtuple from .ippe_cf import IppeCf from cflib.localization.lighthouse_types import LhBsCfPoses @@ -32,11 +32,16 @@ from cflib.localization.lighthouse_types import LhException -# A type representing the ids of a pair of base stations -BsPairIds = namedtuple('BsPairIds', ['bs1', 'bs2']) +class BsPairIds(NamedTuple): + """A type representing the ids of a pair of base stations""" + bs1: int + bs2: int -# A type representing the poses of a pair of base stations -BsPairPoses = namedtuple('BsPairPoses', ['bs1', 'bs2']) + +class BsPairPoses(NamedTuple): + """A type representing the poses of a pair of base stations""" + bs1: Pose + bs2: Pose class LighthouseInitialEstimator: From fafc97b8a1aeb65102bfb481622fb9957b7bea87 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 15:44:57 +0200 Subject: [PATCH 777/861] Cleaned up array types --- .../lighthouse_initial_estimator.py | 42 ++++++++++--------- cflib/localization/lighthouse_types.py | 2 +- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 8d30de53b..087f85bfe 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -32,6 +32,9 @@ from cflib.localization.lighthouse_types import LhException +ArrayFloat = npt.NDArray[np.float_] + + class BsPairIds(NamedTuple): """A type representing the ids of a pair of base stations""" bs1: int @@ -54,7 +57,7 @@ class LighthouseInitialEstimator: OUTLIER_DETECTION_ERROR = 0.5 @classmethod - def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) -> tuple[ + def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat) -> tuple[ LhBsCfPoses, list[LhCfPoseSample]]: """ Make a rough estimate of the poses of all base stations and CF poses found in the samples. @@ -96,8 +99,8 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.A return LhBsCfPoses(bs_poses, cf_poses), cleaned_matched_samples @classmethod - def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike - ) -> dict[BsPairIds, npt.NDArray]: + def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat + ) -> dict[BsPairIds, ArrayFloat]: """ Find the pose of all base stations, in the reference frame of other base stations. @@ -115,7 +118,7 @@ def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions (2, 1) contains the position of base station 1, in the base station 2 reference frame. """ - position_permutations: dict[BsPairIds, list[list[npt.ArrayLike]]] = {} + position_permutations: dict[BsPairIds, list[list[ArrayFloat]]] = {} for sample in matched_samples: solutions: dict[int, BsPairPoses] = {} for bs, angles in sample.angles_calibrated.items(): @@ -130,17 +133,17 @@ def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions @classmethod def _add_solution_permutations(cls, solutions: dict[int, BsPairPoses], - position_permutations: dict[BsPairIds, list[list[npt.ArrayLike]]]): + position_permutations: dict[BsPairIds, list[list[ArrayFloat]]]): """ Add the possible permutations of base station positions for a sample to a collection of aggregated positions. The aggregated collection contains base station positions in the reference frame of other base stations. :param solutions: All possible positions of the base stations, in the reference frame of the Crazyflie in one sample - :param position_permutations: Aggregated possible solutions. A dictionary with base staion pairs as keys, mapped - to lists of lists of possible positions. For instance, the entry for (2, 1) would - contain a list of lists with 4 positions each, for where base station 1 might be - located in the base station 2 reference frame. + :param position_permutations: Aggregated possible solutions. A dictionary with base staTion pairs as keys, + mapped to lists of lists of possible positions. For instance, the entry for (2, 1) + would contain a list of lists with 4 positions each, for where base station 1 + might be located in the base station 2 reference frame. """ ids = sorted(solutions.keys()) @@ -164,9 +167,9 @@ def _add_solution_permutations(cls, solutions: dict[int, BsPairPoses], pose3.translation, pose4.translation]) @classmethod - def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike, - bs_positions: dict[BsPairIds, npt.NDArray]) -> tuple[list[dict[int, Pose]], - list[LhCfPoseSample]]: + def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat, + bs_positions: dict[BsPairIds, ArrayFloat]) -> tuple[list[dict[int, Pose]], + list[LhCfPoseSample]]: """ Estimate the base station poses in the Crazyflie reference frames, for each sample. @@ -216,7 +219,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position @classmethod def _choose_solutions(cls, solutions_1: BsPairPoses, solutions_2: BsPairPoses, - expected: npt.ArrayLike) -> tuple[bool, BsPairPoses]: + expected: ArrayFloat) -> tuple[bool, BsPairPoses]: """Pick the base pose solutions for a pair of base stations, based on the position in expected""" min_dist = 100000.0 @@ -238,19 +241,19 @@ def _choose_solutions(cls, solutions_1: BsPairPoses, solutions_2: BsPairPoses, @classmethod def _find_most_likely_positions(cls, position_permutations: dict[BsPairIds, - list[list[npt.ArrayLike]]]) -> dict[BsPairIds, npt.NDArray]: + list[list[ArrayFloat]]]) -> dict[BsPairIds, ArrayFloat]: """ Find the most likely base station positions from all the possible permutations. Sort the permutations into buckets based on how close they are to the solutions in the first sample. Solutions that are "too" far away and discarded. The bucket with the most samples in, is considered the best. """ - result: dict[BsPairIds, npt.NDArray] = {} + result: dict[BsPairIds, ArrayFloat] = {} for pair, position_lists in position_permutations.items(): # Use first as reference to map the others to bucket_ref_positions = position_lists[0] - buckets: list[list[npt.NDArray]] = [[], [], [], []] + buckets: list[list[ArrayFloat]] = [[], [], [], []] cls._map_positions_to_ref(bucket_ref_positions, position_lists, buckets) best_pos = cls._find_best_position_bucket(buckets) @@ -259,8 +262,9 @@ def _find_most_likely_positions(cls, position_permutations: dict[BsPairIds, return result @classmethod - def _map_positions_to_ref(cls, bucket_ref_positions: list[npt.ArrayLike], position_lists: list[list[npt.ArrayLike]], - buckets: list[list[npt.ArrayLike]]) -> None: + def _map_positions_to_ref(cls, bucket_ref_positions: list[ArrayFloat], + position_lists: list[list[ArrayFloat]], + buckets: list[list[ArrayFloat]]) -> None: """ Sort solution into buckets based on the distance to the reference position. If no bucket is close enough, the solution is discarded. @@ -276,7 +280,7 @@ def _map_positions_to_ref(cls, bucket_ref_positions: list[npt.ArrayLike], positi break @classmethod - def _find_best_position_bucket(cls, buckets: list[list[npt.ArrayLike]]) -> npt.NDArray: + def _find_best_position_bucket(cls, buckets: list[list[ArrayFloat]]) -> ArrayFloat: """ Find the bucket with the most solutions in, this is considered to be the correct solution. The final result is the mean of the solution in the bucket. diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py index 885720b0c..941bc5e74 100644 --- a/cflib/localization/lighthouse_types.py +++ b/cflib/localization/lighthouse_types.py @@ -175,7 +175,7 @@ class LhDeck4SensorPositions: _sensor_distance_length = 0.03 # Sensor positions in the Crazyflie reference frame - positions = np.float32([ + positions = np.array([ (-_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0), (-_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0), (_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0), From 1a4bf439fb2f37ce8df9645ff131d79daf06164a Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 8 Apr 2025 16:51:41 +0200 Subject: [PATCH 778/861] Fixed errors --- cflib/localization/lighthouse_initial_estimator.py | 3 ++- test/localization/test_lighthouse_initial_estimator.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 087f85bfe..690c1936d 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -22,14 +22,15 @@ from __future__ import annotations from typing import NamedTuple + import numpy as np import numpy.typing as npt from .ippe_cf import IppeCf from cflib.localization.lighthouse_types import LhBsCfPoses from cflib.localization.lighthouse_types import LhCfPoseSample -from cflib.localization.lighthouse_types import Pose from cflib.localization.lighthouse_types import LhException +from cflib.localization.lighthouse_types import Pose ArrayFloat = npt.NDArray[np.float_] diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py index 259dadf43..b011558b5 100644 --- a/test/localization/test_lighthouse_initial_estimator.py +++ b/test/localization/test_lighthouse_initial_estimator.py @@ -27,8 +27,8 @@ from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator from cflib.localization.lighthouse_types import LhCfPoseSample from cflib.localization.lighthouse_types import LhDeck4SensorPositions -from cflib.localization.lighthouse_types import Pose from cflib.localization.lighthouse_types import LhException +from cflib.localization.lighthouse_types import Pose class TestLighthouseInitialEstimator(LighthouseTestBase): From 200a095810c6b0a84811832ec15c7323e7fc2b6d Mon Sep 17 00:00:00 2001 From: Aris Date: Wed, 9 Apr 2025 13:43:53 +0200 Subject: [PATCH 779/861] Fixed minor typos --- examples/swarm/asynchronizedSwarm.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/swarm/asynchronizedSwarm.py b/examples/swarm/asynchronizedSwarm.py index e80ab96a1..08fdd4cfe 100644 --- a/examples/swarm/asynchronizedSwarm.py +++ b/examples/swarm/asynchronizedSwarm.py @@ -78,12 +78,12 @@ def position_callback(uri, data): 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]}') + 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]}') + print(f'Uri2 position: x={pos2[0]}, y={pos2[1]}, z={pos2[2]}') def start_position_printing(scf): From 696ac5ffbb37a2c2c77e2903f05147d0531d328e Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Thu, 10 Apr 2025 13:45:45 +0200 Subject: [PATCH 780/861] Fix casing error --- cflib/localization/lighthouse_initial_estimator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 690c1936d..1853d415a 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -141,7 +141,7 @@ def _add_solution_permutations(cls, solutions: dict[int, BsPairPoses], :param solutions: All possible positions of the base stations, in the reference frame of the Crazyflie in one sample - :param position_permutations: Aggregated possible solutions. A dictionary with base staTion pairs as keys, + :param position_permutations: Aggregated possible solutions. A dictionary with base station pairs as keys, mapped to lists of lists of possible positions. For instance, the entry for (2, 1) would contain a list of lists with 4 positions each, for where base station 1 might be located in the base station 2 reference frame. From aea9aa193ab8331e29f2b4969b67e4d906697ecb Mon Sep 17 00:00:00 2001 From: Aris Date: Tue, 1 Apr 2025 16:20:34 +0200 Subject: [PATCH 781/861] Added send_setpoint_manual command --- cflib/crazyflie/commander.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index 1529e82da..82377dc1a 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -47,6 +47,7 @@ TYPE_VELOCITY_WORLD = 8 TYPE_ZDISTANCE = 9 TYPE_HOVER = 10 +TYPE_MANUAL = 11 TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0 @@ -232,3 +233,26 @@ def send_position_setpoint(self, x, y, z, yaw): pk.data = struct.pack(' 100 or thrust_percentage < 0: + raise ValueError('Thrust percentage must be between 0 and 100') + + thrust = 10001 + 0.01 * thrust_percentage * (60000 - 10001) + thrust_16 = struct.unpack(' Date: Mon, 14 Apr 2025 12:23:14 +0200 Subject: [PATCH 782/861] =?UTF-8?q?Fix=20rotvec=20comparison=20to=20handle?= =?UTF-8?q?=20180=C2=B0=20rotation=20ambiguity?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replaced direct rotvec comparison with relative rotation check to avoid sign ambiguity in 180° rotations. Fixes unit test failures on Windows on Python 3.10–3.12. --- test/localization/lighthouse_test_base.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/test/localization/lighthouse_test_base.py b/test/localization/lighthouse_test_base.py index 361db3f12..f117202b8 100644 --- a/test/localization/lighthouse_test_base.py +++ b/test/localization/lighthouse_test_base.py @@ -47,13 +47,13 @@ def assertPosesAlmostEqual(self, expected: Pose, actual: Pose, places: int = 5): self.assertAlmostEqual(0.0, np.linalg.norm(translation_diff), places, f'Translation different, expected: {expected.translation}, actual: {actual.translation}') - def un_ambiguize(rot_vec): - quat = Rotation.from_rotvec(rot_vec).as_quat() - return Rotation.from_quat(quat).as_rotvec() + _expected_rot = Rotation.from_rotvec(expected.rot_vec) + _actual_rot = Rotation.from_rotvec(actual.rot_vec) - _expected_rot_vec = un_ambiguize(expected.rot_vec) - _actual_rot_vec = un_ambiguize(actual.rot_vec) + # Compute the rotation needed to go from expected to actual. + # This avoids sign ambiguity in rotvecs (e.g., π vs. -π) by comparing actual orientation difference. + _relative_rot = _expected_rot.inv() * _actual_rot - rotation_diff = _expected_rot_vec - _actual_rot_vec - self.assertAlmostEqual(0.0, np.linalg.norm(rotation_diff), places, - f'Rotation different, expected: {expected.rot_vec}, actual: {actual.rot_vec}') + angle_diff = _relative_rot.magnitude() + self.assertAlmostEqual(0.0, angle_diff, places, + f'Rotation different, expected: {expected.rot_vec}, actual: {actual.rot_vec}') From a029f4f5f3aca679649693ef72e81a75abc5f4ee Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 14 Apr 2025 13:12:09 +0200 Subject: [PATCH 783/861] autopep8 compliance --- test/localization/lighthouse_test_base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/localization/lighthouse_test_base.py b/test/localization/lighthouse_test_base.py index f117202b8..557cd9437 100644 --- a/test/localization/lighthouse_test_base.py +++ b/test/localization/lighthouse_test_base.py @@ -56,4 +56,4 @@ def assertPosesAlmostEqual(self, expected: Pose, actual: Pose, places: int = 5): angle_diff = _relative_rot.magnitude() self.assertAlmostEqual(0.0, angle_diff, places, - f'Rotation different, expected: {expected.rot_vec}, actual: {actual.rot_vec}') + f'Rotation different, expected: {expected.rot_vec}, actual: {actual.rot_vec}') From 81b5e35a5b0331acebfd1bf7b71f92edfa4eb5ee Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 4 Jun 2025 14:24:19 +0200 Subject: [PATCH 784/861] Name threads for easier debugging --- cflib/crazyflie/link_statistics.py | 2 +- cflib/crazyflie/param.py | 4 ++-- cflib/crtp/radiodriver.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py index e577659b4..61c2c48fe 100644 --- a/cflib/crazyflie/link_statistics.py +++ b/cflib/crazyflie/link_statistics.py @@ -146,7 +146,7 @@ def start(self): """ if self._ping_thread_instance is None or not self._ping_thread_instance.is_alive(): self._stop_event.clear() - self._ping_thread_instance = Thread(target=self._ping_thread) + self._ping_thread_instance = Thread(target=self._ping_thread, name='ping_thread') self._ping_thread_instance.start() def stop(self): diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index b4f827c4a..5e7548253 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -519,7 +519,7 @@ def new_packet_cb(pk): class _ExtendedTypeFetcher(Thread): def __init__(self, cf, toc): - Thread.__init__(self) + Thread.__init__(self, name='ExtendedTypeFetcherThread') self.daemon = True self._lock = Lock() @@ -598,7 +598,7 @@ class _ParamUpdater(Thread): def __init__(self, cf, useV2, updated_callback): """Initialize the thread""" - Thread.__init__(self) + Thread.__init__(self, name='ParamUpdaterThread') self.daemon = True self.wait_lock = Lock() self.cf = cf diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index 00bf3d56b..b2b280ddd 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -527,7 +527,7 @@ class _RadioDriverThread(threading.Thread): def __init__(self, radio, inQueue, outQueue, radio_link_statistics_callback, link_error_callback, link, rate_limit: Optional[int]): """ Create the object """ - threading.Thread.__init__(self) + threading.Thread.__init__(self, name='RadioDriverThread') self._radio = radio self._in_queue = inQueue self._out_queue = outQueue From 0af73a7435895316d4a08ec8dc91c904a066cc7f Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 4 Jun 2025 14:45:31 +0200 Subject: [PATCH 785/861] properly close param thread and extendedtypefetcher thread The threads where never actually exited since they would hang waiting for next message in the queue for ever. To fix this we send a None message to unlock the queue. The _should_close flag (was created but not used before) should be used to actually close the thread --- cflib/crazyflie/param.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 5e7548253..8fbc547d8 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -295,7 +295,9 @@ def _connection_requested(self, uri): def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" - self.param_updater.close() + if self.param_updater is not None: + self.param_updater.close() + self.param_updater = None # Do not clear self.is_updated here as we might get spurious parameter updates later @@ -307,6 +309,9 @@ def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ + if self.param_updater is None: + self.param_updater = _ParamUpdater(self.cf, self._useV2, self._param_updated) + self.param_updater.start() self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) @@ -573,6 +578,9 @@ def _close(self): self.request_queue.get(block=False) except Empty: pass + self.request_queue.put(None) # Make sure we exit the run loop + self._should_close = True + self._cf.remove_port_callback(CRTPPort.PARAM, self._new_packet_cb) # Then force an unlock of the mutex if we are waiting for a packet # we didn't get back due to a disconnect for example. @@ -584,6 +592,8 @@ def _close(self): def run(self): while not self._should_close: pk = self.request_queue.get() # Wait for request update + if pk is None: + continue self._lock.acquire() if self._cf.link: self._req_param = struct.unpack(' Date: Wed, 4 Jun 2025 15:05:04 +0200 Subject: [PATCH 786/861] Stop modifying packet in callback The param callback modifyed the original packet. The owner of the packet is not the param thread but the caller, and its better not to modify it. The caller might call several callbacks after and they should get the original --- cflib/crazyflie/param.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 8fbc547d8..bfc4e254b 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -29,6 +29,7 @@ the parameters that can be written/read. """ +import copy import errno import logging import struct @@ -652,6 +653,7 @@ def _new_packet_cb(self, pk): if self._useV2: release_pattern = pk.data[:2] if pk.channel == READ_CHANNEL: + pk = copy.deepcopy(pk) # Dont modify the original packet pk.data = pk.data[:2] + pk.data[3:] else: release_pattern = pk.data[:1] From dabbb56ccaa96a5d3d97e869b8a2e918c129b600 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Tue, 10 Jun 2025 11:50:04 +0200 Subject: [PATCH 787/861] Add logging for more info --- cflib/crazyflie/param.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index bfc4e254b..3f8248a0b 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -295,6 +295,7 @@ def _connection_requested(self, uri): self._initialized.clear() def _disconnected(self, uri): + logger.info('Disconnected, cleaning up threads') """Disconnected callback from Crazyflie API""" if self.param_updater is not None: self.param_updater.close() From 300ef5c74c130a861be7c7d7ef75a40aed9496d6 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 11 Jun 2025 09:03:07 +0200 Subject: [PATCH 788/861] Incoming packet thread should also be daemon --- cflib/crazyflie/__init__.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index cc4868e50..db6ba79bd 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -393,7 +393,8 @@ class _IncomingPacketHandler(Thread): """Handles incoming packets and sends the data to the correct receivers""" def __init__(self, cf): - Thread.__init__(self) + Thread.__init__(self, name='IncomingPacketHandlerThread') + self.daemon = True self.cf = cf self.cb = [] From 4866badbb61c04c4188a0f7d7ebba28c028c0128 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 11 Jun 2025 09:04:27 +0200 Subject: [PATCH 789/861] Radiodriver should be daemon --- cflib/crtp/radiodriver.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index b2b280ddd..3f0894fb6 100644 --- a/cflib/crtp/radiodriver.py +++ b/cflib/crtp/radiodriver.py @@ -542,6 +542,7 @@ def __init__(self, radio, inQueue, outQueue, self._has_safelink = False self._link = link + self.daemon = True def stop(self): """ Stop the thread """ From 998a6a748c0f206b9c46b5fb6d36c4c9369a04b3 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 11 Jun 2025 13:57:52 +0200 Subject: [PATCH 790/861] Start the ParamUpdater thread only when opening a connection --- cflib/crazyflie/param.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 3f8248a0b..605b543d8 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -155,9 +155,6 @@ def __init__(self, crazyflie): self.all_update_callback = Caller() self.param_updater = None - self.param_updater = _ParamUpdater(self.cf, self._useV2, self._param_updated) - self.param_updater.start() - self.cf.disconnected.add_callback(self._disconnected) self.cf.connection_requested.add_callback(self._connection_requested) @@ -289,6 +286,8 @@ def refresh_done(): def _connection_requested(self, uri): # Reset the internal state on connect to make sure we have a clean state + self.param_updater = _ParamUpdater(self.cf, self._useV2, self._param_updated) + self.param_updater.start() self.is_updated = False self.toc = Toc() self.values = {} @@ -312,8 +311,7 @@ def request_param_update(self, complete_name): Request an update of the value for the supplied parameter. """ if self.param_updater is None: - self.param_updater = _ParamUpdater(self.cf, self._useV2, self._param_updated) - self.param_updater.start() + raise Exception('Param updater not initialized, did you call open_connection?') self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) From 7ae9973f3bcbd0ec74e8bf7cf1a2e4f6e233417e Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Fri, 13 Jun 2025 09:45:35 +0200 Subject: [PATCH 791/861] Upgrade to numpy 2.3.0 --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 283dedb81..acb65b98b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -38,7 +38,7 @@ dependencies = [ "pyusb~=1.2", "libusb-package~=1.0", "scipy~=1.14", - "numpy~=1.26", + "numpy~=2.3", "packaging~=24.2", ] From ceebfc03537ddde142325af3315fd175a3fa6189 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Fri, 13 Jun 2025 09:51:55 +0200 Subject: [PATCH 792/861] float_ is now float64 in later numpy versions --- cflib/localization/lighthouse_initial_estimator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 1853d415a..722e66d1e 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -33,7 +33,7 @@ from cflib.localization.lighthouse_types import Pose -ArrayFloat = npt.NDArray[np.float_] +ArrayFloat = npt.NDArray[np.float64] class BsPairIds(NamedTuple): From dc585966e251a83dc0dc071bbb138d4271316609 Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Fri, 13 Jun 2025 15:35:07 +0200 Subject: [PATCH 793/861] NaN is now nan in numpy2.x --- examples/radio/radio-test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/radio/radio-test.py b/examples/radio/radio-test.py index f96e947e6..4fe51a3e6 100644 --- a/examples/radio/radio-test.py +++ b/examples/radio/radio-test.py @@ -86,8 +86,8 @@ rssi_avg = np.mean(temp) std = np.std(temp) else: - rssi_avg = np.NaN - std = np.NaN + rssi_avg = np.nan + std = np.nan rssi.append(rssi_avg) ack.append(ack_rate) From 3a35d22026c2ed8251b821e4f5b10e67091f811f Mon Sep 17 00:00:00 2001 From: Tove Rumar Date: Wed, 18 Jun 2025 13:25:24 +0200 Subject: [PATCH 794/861] Downgrade to 2.2 to support python 3.10 --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index acb65b98b..bca3da757 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -38,7 +38,7 @@ dependencies = [ "pyusb~=1.2", "libusb-package~=1.0", "scipy~=1.14", - "numpy~=2.3", + "numpy~=2.2", "packaging~=24.2", ] From 41a09bf0a08decc183e30920c31dc035b992fd40 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 26 Jun 2025 15:25:21 +0200 Subject: [PATCH 795/861] Add support for trajectory high-level command with relative_yaw Chooses between legacy and new command format based on drone CRTP version, ensuring backward compatibility while enabling relative_yaw flag. --- cflib/crazyflie/high_level_commander.py | 30 ++++++++++++++++++------- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index e52ad520e..271e86240 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -55,6 +55,7 @@ class HighLevelCommander(): COMMAND_LAND_2 = 8 COMMAND_SPIRAL = 11 COMMAND_GO_TO_2 = 12 + COMMAND_START_TRAJECTORY_2 = 13 ALL_GROUPS = 0 @@ -231,7 +232,7 @@ def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=Fa duration_s)) def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, - reversed=False, group_mask=ALL_GROUPS): + relative_yaw=False, reversed=False, group_mask=ALL_GROUPS): """ starts executing a specified trajectory @@ -247,13 +248,26 @@ def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, :param group_mask: Mask for which CFs this should apply to :return: """ - self._send_packet(struct.pack(' Date: Thu, 26 Jun 2025 17:14:09 +0200 Subject: [PATCH 796/861] Refactor: Clarify naming of relative flag to indicate it applies to position Updated the naming of the relative flag used in trajectory commands to make it explicit that it applies to position. This improves clarity and aligns with the newly introduced relative yaw flag. --- cflib/crazyflie/high_level_commander.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 271e86240..a9904af64 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -231,7 +231,7 @@ def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=Fa ascent, duration_s)) - def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, + def start_trajectory(self, trajectory_id, time_scale=1.0, relative_position=False, relative_yaw=False, reversed=False, group_mask=ALL_GROUPS): """ starts executing a specified trajectory @@ -241,8 +241,10 @@ def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, :param time_scale: Time factor; 1.0 = original speed; >1.0: slower; <1.0: faster - :param relative: Set to True, if trajectory should be shifted to + :param relative_position: Set to True, if trajectory should be shifted to current setpoint + :param relative_yaw: Set to True, if trajectory should be aligned to + current yaw :param reversed: Set to True, if trajectory should be executed in reverse :param group_mask: Mask for which CFs this should apply to @@ -255,7 +257,7 @@ def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, self._send_packet(struct.pack(' Date: Thu, 26 Jun 2025 18:22:36 +0200 Subject: [PATCH 797/861] Add --relative-yaw flag to demonstrate relative yaw functionality Enables running the trajectory with relative_yaw=True, rotating it based on the prior yaw command. --- .../autonomous_sequence_high_level.py | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index afa995fae..556da5a42 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2018 Bitcraze AB +# 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 @@ -28,6 +28,7 @@ It aims at documenting how to set the Crazyflie in position control mode and how to send setpoints using the high level commander. """ +import argparse import sys import time @@ -87,17 +88,17 @@ def upload_trajectory(cf, trajectory_id, trajectory): return total_duration -def run_sequence(cf, trajectory_id, duration): +def run_sequence(cf, trajectory_id, duration, relative_yaw=False): commander = cf.high_level_commander # Arm the Crazyflie cf.platform.send_arming_request(True) time.sleep(1.0) - commander.takeoff(1.0, 2.0) + takeoff_yaw = 3.14 / 2 if relative_yaw else 0.0 + commander.takeoff(1.0, 2.0, yaw=takeoff_yaw) time.sleep(3.0) - relative = True - commander.start_trajectory(trajectory_id, 1.0, relative) + commander.start_trajectory(trajectory_id, 1.0, relative=True, relative_yaw=relative_yaw) time.sleep(duration) commander.land(0.0, 2.0) time.sleep(2) @@ -105,6 +106,16 @@ def run_sequence(cf, trajectory_id, duration): if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Crazyflie trajectory demo') + parser.add_argument('--relative-yaw', action='store_true', + help=( + 'Enable relative_yaw mode when running the trajectory. This demonstrates ' + 'how the trajectory can be rotated based on the orientation from the prior command.' + ) + ) + + args = parser.parse_args() + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -115,4 +126,4 @@ def run_sequence(cf, trajectory_id, duration): duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) reset_estimator(cf) - run_sequence(cf, trajectory_id, duration) + run_sequence(cf, trajectory_id, duration, relative_yaw=args.relative_yaw) From 30a9604b918e63d5900ddf333298b6e2d999849f Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Fri, 27 Jun 2025 08:37:42 +0200 Subject: [PATCH 798/861] Fix: Update example to use renamed relative position flag --- examples/autonomy/autonomous_sequence_high_level.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 556da5a42..f4f33bd85 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -98,7 +98,7 @@ def run_sequence(cf, trajectory_id, duration, relative_yaw=False): takeoff_yaw = 3.14 / 2 if relative_yaw else 0.0 commander.takeoff(1.0, 2.0, yaw=takeoff_yaw) time.sleep(3.0) - commander.start_trajectory(trajectory_id, 1.0, relative=True, relative_yaw=relative_yaw) + commander.start_trajectory(trajectory_id, 1.0, relative_position=True, relative_yaw=relative_yaw) time.sleep(duration) commander.land(0.0, 2.0) time.sleep(2) From bd83603bace86e088feba6b8c369283c95f8e0bd Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 1 Jul 2025 11:44:48 +0200 Subject: [PATCH 799/861] Added status messages for swarm.reset_estimators --- cflib/crazyflie/swarm.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cflib/crazyflie/swarm.py b/cflib/crazyflie/swarm.py index eeb562b77..632fc44cf 100644 --- a/cflib/crazyflie/swarm.py +++ b/cflib/crazyflie/swarm.py @@ -185,7 +185,9 @@ def reset_estimators(self): Reset estimator on all members of the swarm and wait for a stable positions. Blocks until position estimators finds a position. """ + print('Waiting for estimators to find positions...', end='\r') self.parallel_safe(self.__reset_estimator) + print('Waiting for estimators to find positions...success!') def sequential(self, func, args_dict=None): """ From e997732d647e74e56146b33607deae1866eb3b69 Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 1 Jul 2025 15:04:21 +0200 Subject: [PATCH 800/861] Updated the table in lh geo solver --- .../lighthouse_geometry_solver.py | 64 +++++++++---------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py index 949d70b50..ef33b49e0 100644 --- a/cflib/localization/lighthouse_geometry_solver.py +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -99,38 +99,38 @@ class LighthouseGeometrySolver: An examples matrix: - bs0_pose, bs1_pose, bs2_pose, bs3_pose, cf1_pose, cf2_pose, ... - - cf0/bs2/sens0/ang0 X - cf0/bs2/sens0/ang1 X - cf0/bs2/sens1/ang0 X - cf0/bs2/sens1/ang1 X - ... - cf0/bs3/sens0/ang0 X - cf0/bs3/sens0/ang1 X - cf0/bs3/sens1/ang0 X - cf0/bs3/sens1/ang1 X - ... - cf1/bs1/sens0/ang0 X X - cf1/bs1/sens0/ang1 X X - cf1/bs1/sens1/ang0 X X - cf1/bs1/sens1/ang1 X X - ... - cf1/bs2/sens0/ang0 X X - cf1/bs2/sens0/ang1 X X - cf1/bs2/sens1/ang0 X X - cf1/bs2/sens1/ang1 X X - ... - cf2/bs1/sens0/ang0 X X - cf2/bs1/sens0/ang1 X X - cf2/bs1/sens1/ang0 X X - cf2/bs1/sens1/ang1 X X - ... - cf2/bs3/sens0/ang0 X X - cf2/bs3/sens0/ang1 X X - cf2/bs3/sens1/ang0 X X - cf2/bs3/sens1/ang1 X X - ... + | | bs0_pose | bs1_pose | bs2_pose | bs3_pose | cf1_pose | cf2_pose | + |-------------------------|----------|----------|----------|----------|----------|----------| + | cf0/bs2/sens0/ang0 | | | X | | | | + | cf0/bs2/sens0/ang1 | | | X | | | | + | cf0/bs2/sens1/ang0 | | | X | | | | + | cf0/bs2/sens1/ang1 | | | X | | | | + | | | | | | | | + | cf0/bs3/sens0/ang0 | | | | X | | | + | cf0/bs3/sens0/ang1 | | | | X | | | + | cf0/bs3/sens1/ang0 | | | | X | | | + | cf0/bs3/sens1/ang1 | | | | X | | | + | | | | | | | | + | cf1/bs1/sens0/ang0 | | X | | | X | | + | cf1/bs1/sens0/ang1 | | X | | | X | | + | cf1/bs1/sens1/ang0 | | X | | | X | | + | cf1/bs1/sens1/ang1 | | X | | | X | | + | | | | | | | | + | cf1/bs2/sens0/ang0 | | | X | | X | | + | cf1/bs2/sens0/ang1 | | | X | | X | | + | cf1/bs2/sens1/ang0 | | | X | | X | | + | cf1/bs2/sens1/ang1 | | | X | | X | | + | | | | | | | | + | cf2/bs1/sens0/ang0 | | X | | | | X | + | cf2/bs1/sens0/ang1 | | X | | | | X | + | cf2/bs1/sens1/ang0 | | X | | | | X | + | cf2/bs1/sens1/ang1 | | X | | | | X | + | | | | | | | | + | cf2/bs3/sens0/ang0 | | | | X | | X | + | cf2/bs3/sens0/ang1 | | | | X | | X | + | cf2/bs3/sens1/ang0 | | | | X | | X | + | cf2/bs3/sens1/ang1 | | | | X | | X | + """ @classmethod From ae7b3cdadda84bc2de82030b4e4528a6b9fdca27 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 18 Aug 2025 17:19:39 +0200 Subject: [PATCH 801/861] Close RadioDriverThread upon failed reset to bootload --- cflib/bootloader/cloader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/cloader.py b/cflib/bootloader/cloader.py index ea54675db..52e6b2da5 100644 --- a/cflib/bootloader/cloader.py +++ b/cflib/bootloader/cloader.py @@ -114,7 +114,7 @@ def reset_to_bootloader(self, target_id: int) -> bool: self.link = cflib.crtp.get_link_driver(f'radio://0/0/2M/{address}?safelink=0') time.sleep(0.5) return True - + self.link.close() return False def reset_to_firmware(self, target_id: int, boot_delay: float = 0.0) -> bool: From 0c813f528e868680b39723724d79e2b6a7513f6a Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 19 Aug 2025 14:28:51 +0200 Subject: [PATCH 802/861] Properly close link upon failed open link attempt with SyncCrazyflie --- cflib/crazyflie/syncCrazyflie.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index 6a8e82647..9aacc5bd4 100644 --- a/cflib/crazyflie/syncCrazyflie.py +++ b/cflib/crazyflie/syncCrazyflie.py @@ -92,6 +92,7 @@ def open_link(self): if not self._is_link_open: self._remove_callbacks() self._params_updated_event.clear() + self.cf.close_link() raise Exception(self._error_message) def wait_for_params(self): From 70e6f7d8af8224f266ea0b363bbf11d3c68813c2 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 19 Aug 2025 14:29:46 +0200 Subject: [PATCH 803/861] Close IncomingPacketHandlerThread upon link closure --- cflib/crazyflie/__init__.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index db6ba79bd..af61f9c08 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -32,10 +32,10 @@ """ import datetime import logging -import time import warnings from collections import namedtuple from threading import current_thread +from threading import Event from threading import Lock from threading import Thread from threading import Timer @@ -284,6 +284,14 @@ def close_link(self): if (self.link is not None): self.link.close() self.link = None + if self.incoming: + callbacks = list(self.incoming.cb) + if self.incoming.is_alive(): + self.incoming.stop() + self.incoming.join() + self.incoming = _IncomingPacketHandler(self) + self.incoming.cb = callbacks + self.incoming.daemon = True self._answer_patterns = {} self.disconnected.call(self.link_uri) self.state = State.DISCONNECTED @@ -397,6 +405,7 @@ def __init__(self, cf): self.daemon = True self.cf = cf self.cb = [] + self._stop_event = Event() def add_port_callback(self, port, cb): """Add a callback for data that comes on a specific port""" @@ -431,10 +440,14 @@ def remove_header_callback(self, cb, port, channel, port_mask=0xFF, port_callback.callback == cb: self.cb.remove(port_callback) + def stop(self): + """Signal the thread to stop.""" + self._stop_event.set() + def run(self): - while True: + while not self._stop_event.is_set(): if self.cf.link is None: - time.sleep(1) + self._stop_event.wait(1) continue pk = self.cf.link.receive_packet(1) From d9e6758822fa20fa7cd06b0356d0762718a3794c Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 20 Aug 2025 10:26:26 +0200 Subject: [PATCH 804/861] Add timeout to join IncomingPacketHandler thread on link closure --- cflib/crazyflie/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index af61f9c08..5e09b906d 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -288,7 +288,7 @@ def close_link(self): callbacks = list(self.incoming.cb) if self.incoming.is_alive(): self.incoming.stop() - self.incoming.join() + self.incoming.join(timeout=1) self.incoming = _IncomingPacketHandler(self) self.incoming.cb = callbacks self.incoming.daemon = True From ef6d5bf46a32d489a8a8af4d93da923a8670d697 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Sun, 24 Aug 2025 14:16:55 +0300 Subject: [PATCH 805/861] Fix typo in install.md --- docs/installation/install.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 274b8a90f..9ad605839 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -37,7 +37,7 @@ The following should be executed in the root of the crazyflie-lib-python file tr #### Virtualenv This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) -with package dependencies. If you don't want to use virualenv and don't mind installing cflib dependencies system-wide +with package dependencies. If you don't want to use virtualenv and don't mind installing cflib dependencies system-wide you can skip this section. * Install virtualenv: `pip install virtualenv` From f1e54f1b888c14d24067e6657723a6b5267cd860 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Sun, 24 Aug 2025 14:23:01 +0300 Subject: [PATCH 806/861] Fix typo in sbs_motion_commander.md --- docs/user-guides/sbs_motion_commander.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index e17100d2c..303f49536 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -267,9 +267,9 @@ Now we are going to add a turn into it. Replace the content under motion command time.sleep(1) ``` -Try to run the script again. Now you can see the crazyflie take off, go forward, turn 180 degrees and go forward again to its initial position. The `mc.back()` needed to be replaced with the forward since the motion commander sends the velocity setpoints in the __body fixed coordinated__ system. This means that the commands forward will go forward to wherever the current heading (the front) of the crazyflie points to. +Try to run the script again. Now you can see the crazyflie take off, go forward, turn 180 degrees and go forward again to its initial position. The `mc.back()` needed to be replaced with the forward since the motion commander sends the velocity setpoints in the __body fixed coordinate__ system. This means that the commands forward will go forward to wherever the current heading (the front) of the crazyflie points to. -Double check if your code code is still correct: +Double check if your code is still correct: ```python import logging From e2d7d2463d6421dfd96c55fbcdccb278eaf38ad8 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Mon, 25 Aug 2025 23:37:06 +0300 Subject: [PATCH 807/861] Fix typo in test name --- test/crazyflie/test_syncLogger.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index 43fd2b2cf..5a81b8571 100644 --- a/test/crazyflie/test_syncLogger.py +++ b/test/crazyflie/test_syncLogger.py @@ -229,7 +229,7 @@ def test_construction_with_sync_crazyflie_instance(self): # Assert # Nothing here. Just not expecting an exception - def test_getting_data_without_conection_raises_exception(self): + def test_getting_data_without_connection_raises_exception(self): # Fixture # Test From 01ca9ce96d2068a48ea4d46a80b498de3f5792e8 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Mon, 25 Aug 2025 23:57:41 +0300 Subject: [PATCH 808/861] Fix typos in mem --- cflib/crazyflie/mem/__init__.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 00d07da11..c9874d531 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -566,7 +566,7 @@ def _handle_chan_write(self, cmd, payload): # Find the write request if id in self._write_requests: self._write_requests_lock.acquire() - do_call_sucess_cb = False + do_call_success_cb = False do_call_fail_cb = False wreq = self._write_requests[id][0] if status == 0: @@ -574,7 +574,7 @@ def _handle_chan_write(self, cmd, payload): # self._write_requests.pop(id, None) # Remove the first item self._write_requests[id].pop(0) - do_call_sucess_cb = True + do_call_success_cb = True # Get a new one to start (if there are any) if len(self._write_requests[id]) > 0: @@ -591,9 +591,9 @@ def _handle_chan_write(self, cmd, payload): self._write_requests_lock.release() - # Call callbacks after the lock has been released to alow for new writes + # Call callbacks after the lock has been released to allow for new writes # to be initiated from the callback. - if do_call_sucess_cb: + if do_call_success_cb: self.mem_write_cb.call(wreq.mem, wreq.addr) if do_call_fail_cb: self.mem_write_failed_cb.call(wreq.mem, wreq.addr) From eb90d3508230cc69b9d2e195f9488c853c21863f Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Mon, 25 Aug 2025 23:58:29 +0300 Subject: [PATCH 809/861] Fix typos in lighthouse_memory.py --- cflib/crazyflie/mem/lighthouse_memory.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem/lighthouse_memory.py b/cflib/crazyflie/mem/lighthouse_memory.py index e7fdd9703..a7078159b 100644 --- a/cflib/crazyflie/mem/lighthouse_memory.py +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -428,13 +428,13 @@ def _write_next_object(self): self._write_fcn(id, data, self._data_written, write_failed_cb=self._write_failed) else: tmp_cb = self._write_done_cb - is_sucess = not self._write_failed_for_one_or_more_objects + is_success = not self._write_failed_for_one_or_more_objects self._objects_to_write = None self._write_done_cb = None self._write_failed_for_one_or_more_objects = False - tmp_cb(is_sucess) + tmp_cb(is_success) def _data_written(self, mem, addr): self._write_next_object() From 8413aa39fabd28d78ca99fc8a23c3de6cfbebdff Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Mon, 25 Aug 2025 23:59:06 +0300 Subject: [PATCH 810/861] Fix typos in memory_tester.py --- cflib/crazyflie/mem/memory_tester.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/crazyflie/mem/memory_tester.py b/cflib/crazyflie/mem/memory_tester.py index a685838d7..fbdd6097b 100644 --- a/cflib/crazyflie/mem/memory_tester.py +++ b/cflib/crazyflie/mem/memory_tester.py @@ -35,7 +35,7 @@ class MemoryTester(MemoryElement): 1. To verify reading: * Call read_data() * Wait for the callback to be called - * Verify that readValidationSucess is True + * Verify that readValidationSuccess is True 2. To verify writing: * Set the parameter 'memTst.resetW' in the CF @@ -53,7 +53,7 @@ def __init__(self, id, type, size, mem_handler): self._update_finished_cb = None self._write_finished_cb = None - self.readValidationSucess = True + self.readValidationSuccess = True def new_data(self, mem, start_address, data): """Callback for when new memory data has been fetched""" @@ -64,7 +64,7 @@ def new_data(self, mem, start_address, data): if (actualValue != expectedValue): address = start_address + i - self.readValidationSucess = False + self.readValidationSuccess = False logger.error( 'Error in data - expected: {}, actual: {}, address:{}', expectedValue, actualValue, address) From 17a5bb57faeea39d8fec980adc63b3677d941f65 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Mon, 25 Aug 2025 23:59:49 +0300 Subject: [PATCH 811/861] Fix typo in _ippe.py --- cflib/localization/_ippe.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/localization/_ippe.py b/cflib/localization/_ippe.py index 71ffc5137..d09cae277 100644 --- a/cflib/localization/_ippe.py +++ b/cflib/localization/_ippe.py @@ -377,7 +377,7 @@ def normalise2dpts(pts): T: The 3x3 transformation matrix, newpts = T*pts """ if pts.shape[0] != 3: - print('Input shoud be 3') + print('Input should be 3') finiteind = np.nonzero(abs(pts[2, :]) > np.spacing(1)) From ad9c5b3527d92abc9442ab851c2ab2b5af2602d6 Mon Sep 17 00:00:00 2001 From: Valeriy Van Date: Tue, 26 Aug 2025 00:00:26 +0300 Subject: [PATCH 812/861] Fix typos in lighthouse_config_manager.py --- cflib/localization/lighthouse_config_manager.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py index d5204ea4b..49bf86683 100644 --- a/cflib/localization/lighthouse_config_manager.py +++ b/cflib/localization/lighthouse_config_manager.py @@ -124,7 +124,7 @@ def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, system_ The callback is called when done. If geos or calibs is None, no data will be written for that data type. If geos or calibs is a dictionary, the values for the base stations in the dictionary will - transfered to the Crazyflie, data for all other base stations will be invalidated. + transferred to the Crazyflie, data for all other base stations will be invalidated. """ if self._data_stored_cb is not None: raise Exception('Write already in prgress') @@ -189,8 +189,8 @@ def _next(self): if tmp_callback is not None: tmp_callback(not self._write_failed_for_one_or_more_objects) - def _upload_done(self, sucess): - if not sucess: + def _upload_done(self, success): + if not success: self._write_failed_for_one_or_more_objects = True self._next() From 7431a8c01425f40f210135887a247034492c14bc Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Tue, 2 Sep 2025 14:19:45 -0400 Subject: [PATCH 813/861] Add multicf_mocap_hl_and_motion_commander.py example --- .../multicf_mocap_hl_and_motion_commander.py | 124 ++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100644 examples/mocap/multicf_mocap_hl_and_motion_commander.py diff --git a/examples/mocap/multicf_mocap_hl_and_motion_commander.py b/examples/mocap/multicf_mocap_hl_and_motion_commander.py new file mode 100644 index 000000000..759bcc998 --- /dev/null +++ b/examples/mocap/multicf_mocap_hl_and_motion_commander.py @@ -0,0 +1,124 @@ +import time +from threading import Thread, Lock + +import motioncapture + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils.reset_estimator import reset_estimator +from cflib.positioning.position_hl_commander import PositionHlCommander +from cflib.positioning.motion_commander import MotionCommander + +# 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 = '192.168.5.21' + +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 run_sequence(scf): + 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 scf._link_uri == 'radio://0/80/2M/E7E7E7E7E7': + with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: + pc.set_default_velocity(0.5) + ################################################################################# + # 6 repetitions, at .5 speed, take ~1':15 and drop the battery from 4.2 to 3.9V # + ################################################################################# + for i in range(6): # 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 scf._link_uri == 'radio://0/90/2M/E7E7E7E7E8': + with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: + pc.set_default_velocity(0.3) + ###################### + # Scripted behaviour # + ###################### + for i in range(6): # 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 scf._link_uri == 'radio://0/100/2M/E7E7E7E7E9': + with MotionCommander(scf) as mc: + ################################################################################# + # 3 right loops and 3 left loops take ~1' and drop the battery from 4.2 to 4.0V # + ################################################################################# + mc.back(0.8) + time.sleep(1) + mc.up(0.5) + time.sleep(1) + mc.circle_right(0.5, velocity=0.4, angle_degrees=1080) + time.sleep(1) + mc.up(0.5) + time.sleep(1) + mc.circle_left(0.5, velocity=0.4, angle_degrees=1080) + 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() + + # Uncomment the URIs to connect to + uris = [ + 'radio://0/80/2M/E7E7E7E7E7', + # 'radio://0/90/2M/E7E7E7E7E8', + # 'radio://0/100/2M/E7E7E7E7E9', + ] + + # Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive + rbs = { + 'radio://0/80/2M/E7E7E7E7E7' : 'E7', + 'radio://0/90/2M/E7E7E7E7E8' : 'E8', + 'radio://0/100/2M/E7E7E7E7E9' : 'E9' + } + + + 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_thred = MocapWrapper(active_rbs_cfs) + + swarm.reset_estimators() + time.sleep(2) + swarm.parallel_safe(run_sequence) + + mocap_thred.close() From 5430e961f670ef396ec91d4250b6f9712ca1c7a9 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Tue, 2 Sep 2025 14:26:06 -0400 Subject: [PATCH 814/861] style --- .../multicf_mocap_hl_and_motion_commander.py | 50 +++++++++++++------ 1 file changed, 35 insertions(+), 15 deletions(-) diff --git a/examples/mocap/multicf_mocap_hl_and_motion_commander.py b/examples/mocap/multicf_mocap_hl_and_motion_commander.py index 759bcc998..265152c6e 100644 --- a/examples/mocap/multicf_mocap_hl_and_motion_commander.py +++ b/examples/mocap/multicf_mocap_hl_and_motion_commander.py @@ -1,3 +1,33 @@ +# -*- 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 positions (only) +to a multiple Crazyflie, using the motioncapture library. + +The script uses the 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, Lock @@ -50,15 +80,11 @@ def run_sequence(scf): scf.cf.platform.send_arming_request(True) time.sleep(1.0) - ################################################################################ - # .takeoff() is automatic when entering the "with PositionHlCommander" context # - ################################################################################ + # .takeoff() is automatic when entering the "with PositionHlCommander" context if scf._link_uri == 'radio://0/80/2M/E7E7E7E7E7': with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.5) - ################################################################################# - # 6 repetitions, at .5 speed, take ~1':15 and drop the battery from 4.2 to 3.9V # - ################################################################################# + # 6 repetitions, at .5 speed, take ~1':15 and drop the battery from 4.2 to 3.9V for i in range(6): # fly a triangle with changing altitude pc.go_to(1.0, 1.0, 1.5) pc.go_to(1.0, -1.0, 1.5) @@ -67,18 +93,14 @@ def run_sequence(scf): elif scf._link_uri == 'radio://0/90/2M/E7E7E7E7E8': with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.3) - ###################### - # Scripted behaviour # - ###################### + # Scripted behaviour for i in range(6): # 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 scf._link_uri == 'radio://0/100/2M/E7E7E7E7E9': with MotionCommander(scf) as mc: - ################################################################################# - # 3 right loops and 3 left loops take ~1' and drop the battery from 4.2 to 4.0V # - ################################################################################# + # 3 right loops and 3 left loops take ~1' and drop the battery from 4.2 to 4.0V mc.back(0.8) time.sleep(1) mc.up(0.5) @@ -90,9 +112,7 @@ def run_sequence(scf): mc.circle_left(0.5, velocity=0.4, angle_degrees=1080) time.sleep(1) mc.down(0.5) - ##################################################################################### - # .land() is automatic when exiting the scope of context "with PositionHlCommander" # - ##################################################################################### + # .land() is automatic when exiting the scope of context "with PositionHlCommander" if __name__ == '__main__': cflib.crtp.init_drivers() From 52e62a8a485629d01b3fa99d822a5703a57e21a9 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Fri, 5 Sep 2025 15:34:06 +0200 Subject: [PATCH 815/861] Update packaging dependency to 25 Fixes #561 --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index bca3da757..70d6f667e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -39,7 +39,7 @@ dependencies = [ "libusb-package~=1.0", "scipy~=1.14", "numpy~=2.2", - "packaging~=24.2", + "packaging~=25", ] [project.urls] From 35b20d76cff7592f6ec6cd4a74b5900d7a8b8926 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Tue, 9 Sep 2025 09:18:58 +0200 Subject: [PATCH 816/861] Specify exact version for packaging dependency (PEP 440) --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 70d6f667e..6199bf298 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -39,7 +39,7 @@ dependencies = [ "libusb-package~=1.0", "scipy~=1.14", "numpy~=2.2", - "packaging~=25", + "packaging~=25.0", ] [project.urls] From e76d0fbe8712758533c5da65621ae2e5370fcb8e Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 15 Sep 2025 16:28:13 +0200 Subject: [PATCH 817/861] Remove OpenCV-based lighthouse geometry estimator from cflib Removes LighthouseBsGeoEstimator and associated OpenCV dependency checking code. This feature was effectively deprecated due to: - Poor UX: required manual OpenCV installation - Limited functionality: only supported 1-2 base stations - No tests or active maintenance - Complex pkg_resources dependency checking Users can still use the (now standard and) robust lighthouse geometry estimation for multi-basestation setups. Follows UI removal in crazyflie-clients-python@b0062f4ab8dd7ac436301a9a06414f2b7eebcb8a --- cflib/localization/__init__.py | 2 - cflib/localization/lighthouse_bs_geo.py | 231 -------------------- test/localization/test_lighthouse_bs_geo.py | 50 ----- 3 files changed, 283 deletions(-) delete mode 100644 cflib/localization/lighthouse_bs_geo.py delete mode 100644 test/localization/test_lighthouse_bs_geo.py diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py index 6f4252c3e..9456d06e1 100644 --- a/cflib/localization/__init__.py +++ b/cflib/localization/__init__.py @@ -19,7 +19,6 @@ # 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 . -from .lighthouse_bs_geo import LighthouseBsGeoEstimator from .lighthouse_bs_vector import LighthouseBsVector from .lighthouse_config_manager import LighthouseConfigFileManager from .lighthouse_config_manager import LighthouseConfigWriter @@ -28,7 +27,6 @@ from .param_io import ParamFileManager __all__ = [ - 'LighthouseBsGeoEstimator', 'LighthouseBsVector', 'LighthouseSweepAngleAverageReader', 'LighthouseSweepAngleReader', diff --git a/cflib/localization/lighthouse_bs_geo.py b/cflib/localization/lighthouse_bs_geo.py deleted file mode 100644 index 0041ee5a9..000000000 --- a/cflib/localization/lighthouse_bs_geo.py +++ /dev/null @@ -1,231 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# 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, 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 . -""" -Functionality to handle base station geometry in the lighthouse poistioning system -""" -import math - -import numpy as np -import pkg_resources -installed = {pkg.key for pkg in pkg_resources.working_set} -if {'opencv-python-headless'} - installed: - OPENCV_INSTALLED = False -else: - import cv2 as cv - OPENCV_INSTALLED = True - - -class LighthouseBsGeoEstimator: - """ - This class is used to estimate the geometry (position and attitude) - of a lighthouse base station, given angles measured using a lighthouse deck. - """ - - def __init__(self): - - self._estimator_available = True - if OPENCV_INSTALLED is False: - self._estimator_available = False - - self._directions = { - self._hash_sensor_order([2, 0, 1, 3]): math.radians(0), - self._hash_sensor_order([2, 0, 3, 1]): math.radians(25), - self._hash_sensor_order([2, 3, 0, 1]): math.radians(65), - self._hash_sensor_order([3, 2, 0, 1]): math.radians(90), - self._hash_sensor_order([3, 2, 1, 0]): math.radians(115), - self._hash_sensor_order([3, 1, 2, 0]): math.radians(155), - self._hash_sensor_order([1, 3, 2, 0]): math.radians(180), - self._hash_sensor_order([1, 3, 0, 2]): math.radians(205), - self._hash_sensor_order([1, 0, 3, 2]): math.radians(245), - self._hash_sensor_order([0, 1, 3, 2]): math.radians(270), - self._hash_sensor_order([0, 1, 2, 3]): math.radians(295), - self._hash_sensor_order([0, 2, 1, 3]): math.radians(335), - } - - # Sensor distances on the lighthouse deck - sensor_distance_width = 0.015 - sensor_distance_length = 0.03 - - # Sensor positions in world coordinates, open cv style - self._lighthouse_3d = np.float32( - [ - [-sensor_distance_width / 2, 0, -sensor_distance_length / 2], - [sensor_distance_width / 2, 0, -sensor_distance_length / 2], - [-sensor_distance_width / 2, 0, sensor_distance_length / 2], - [sensor_distance_width / 2, 0, sensor_distance_length / 2] - ]) - - # Camera matrix - self._K = np.float64( - [ - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0] - ]) - - self._dist_coef = np.zeros(4) - - # Sanity check maximum pos - self._sanity_max_pos = 10 - - def is_available(self): - return self._estimator_available - - def estimate_geometry(self, bs_vectors): - """ - Estimate the full pose of a base station based on angles from the 4 sensors - on a lighthouse deck. The result is a rotation matrix and position of the - base station, in the Crazyflie reference frame. - - :param bs_vectors A list of 4 LighthouseBsVector objects specifying vectors to the 4 sensors - :return rot_bs_in_cf_coord: Rotation matrix of the BS in the CFs coordinate system - :return pos_bs_in_cf_coord: Position vector of the BS in the CFs coordinate system - """ - - if OPENCV_INSTALLED is False: - raise Exception('OpenCV is not installed. To use this function,' + - 'do "pip3 install opencv-python-headless"' + - ' and restart the cfclient') - - guess_yaw = self._find_initial_yaw_guess(bs_vectors) - rvec_guess, tvec_guess = self._convert_yaw_to_open_cv(guess_yaw) - rw_ocv, tw_ocv = self._estimate_pose_by_pnp(bs_vectors, rvec_guess, tvec_guess) - rot_bs_in_cf_coord, pos_bs_in_cf_coord = self._opencv_to_cf(rw_ocv, tw_ocv) - return rot_bs_in_cf_coord, pos_bs_in_cf_coord - - def sanity_check_result(self, pos_bs_in_cf_coord): - """ - Checks if the estimated geometry is within reasonable bounds. It returns - true if it seems reasonable or false if it doesn't - """ - for coord in pos_bs_in_cf_coord: - if (abs(coord) > self._sanity_max_pos): - return False - return True - - def _find_initial_yaw_guess(self, bs_vectors): - # Assume bs is faicing slighly downwards and fairly horizontal - # Sort sensors in the order they are hit by the horizontal sweep - # and use the order to figure out roughly the direction to the - # base station - sweeps_x = { - 0: bs_vectors[0].lh_v1_horiz_angle, - 1: bs_vectors[1].lh_v1_horiz_angle, - 2: bs_vectors[2].lh_v1_horiz_angle, - 3: bs_vectors[3].lh_v1_horiz_angle - } - - ordered_map = {k: v for k, v in sorted(sweeps_x.items(), key=lambda item: item[1])} - sensor_order = list(ordered_map.keys()) - - # The base station is roughly in this direction, in CF (world) coordinates - return self._directions[self._hash_sensor_order(sensor_order)] - - def _hash_sensor_order(self, order): - hash = 0 - for i in range(4): - hash += order[i] * 4 ** i - return hash - - def _convert_yaw_to_open_cv(self, yaw): - # Base station height - bs_h = 2.0 - # Distance to base station along the floor - bs_fd = 3.0 - # Distance to base station - bs_dist = math.sqrt(bs_h ** 2 + bs_fd ** 2) - elevation = math.atan2(bs_h, bs_fd) - - # Initial position of the CF in camera coordinate system, open cv style - tvec_start = np.array([0, 0, bs_dist]) - - # Calculate rotation matrix - d_c = math.cos(-yaw + math.pi) - d_s = math.sin(-yaw + math.pi) - R_rot_y = np.array([ - [d_c, 0.0, d_s], - [0.0, 1.0, 0.0], - [-d_s, 0.0, d_c], - ]) - - e_c = math.cos(elevation) - e_s = math.sin(elevation) - R_rot_x = np.array([ - [1.0, 0.0, 0.0], - [0.0, e_c, -e_s], - [0.0, e_s, e_c], - ]) - - R = np.dot(R_rot_x, R_rot_y) - rvec_start, _ = cv.Rodrigues(R) - - return rvec_start, tvec_start - - def _estimate_pose_by_pnp(self, bs_vectors, rvec_start, tvec_start): - # Sensors as seen by the "camera" - lighthouse_image_projection = np.float32( - [ - [-math.tan(bs_vectors[0].lh_v1_horiz_angle), -math.tan(bs_vectors[0].lh_v1_vert_angle)], - [-math.tan(bs_vectors[1].lh_v1_horiz_angle), -math.tan(bs_vectors[1].lh_v1_vert_angle)], - [-math.tan(bs_vectors[2].lh_v1_horiz_angle), -math.tan(bs_vectors[2].lh_v1_vert_angle)], - [-math.tan(bs_vectors[3].lh_v1_horiz_angle), -math.tan(bs_vectors[3].lh_v1_vert_angle)] - ]) - - _ret, rvec_est, tvec_est = cv.solvePnP( - self._lighthouse_3d, - lighthouse_image_projection, - self._K, - self._dist_coef, - flags=cv.SOLVEPNP_ITERATIVE, - rvec=rvec_start, - tvec=tvec_start, - useExtrinsicGuess=True) - - if not _ret: - raise Exception('No solution found') - - Rw_ocv, Tw_ocv = self._cam_to_world(rvec_est, tvec_est) - return Rw_ocv, Tw_ocv - - def _cam_to_world(self, rvec_c, tvec_c): - R_c, _ = cv.Rodrigues(rvec_c) - R_w = np.linalg.inv(R_c) - tvec_w = -np.matmul(R_w, tvec_c) - return R_w, tvec_w - - def _opencv_to_cf(self, R_cv, t_cv): - R_opencv_to_cf = np.array([ - [0.0, 0.0, 1.0], - [-1.0, 0.0, 0.0], - [0.0, -1.0, 0.0], - ]) - - R_cf_to_opencv = np.array([ - [0.0, -1.0, 0.0], - [0.0, 0.0, -1.0], - [1.0, 0.0, 0.0], - ]) - - t_cf = np.dot(R_opencv_to_cf, t_cv) - R_cf = np.dot(R_opencv_to_cf, np.dot(R_cv, R_cf_to_opencv)) - - return R_cf, t_cf diff --git a/test/localization/test_lighthouse_bs_geo.py b/test/localization/test_lighthouse_bs_geo.py deleted file mode 100644 index aee8cf295..000000000 --- a/test/localization/test_lighthouse_bs_geo.py +++ /dev/null @@ -1,50 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# 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, 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 unittest - -from cflib.localization import LighthouseBsGeoEstimator - - -class TestLighthouseBsGeoEstimator(unittest.TestCase): - def setUp(self): - - self.sut = LighthouseBsGeoEstimator() - - def test_that_sanity_check_finds_coordinate_out_of_bounds(self): - # Fixture - pos_bs_in_cf_coord = [0, -20, 0] - - # Test - actual = self.sut.sanity_check_result(pos_bs_in_cf_coord) - - # Assert - self.assertFalse(actual) - - def test_that_sanity_check_passes_ok_position(self): - # Fixture - pos_bs_in_cf_coord = [0, 1, 2] - - # Test - actual = self.sut.sanity_check_result(pos_bs_in_cf_coord) - - # Assert - self.assertTrue(actual) From f898125b0032ca864b918985a4e7d905747e200c Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 16 Sep 2025 12:58:26 +0200 Subject: [PATCH 818/861] Cleaned up installation doc --- docs/installation/install.md | 113 ++++++++++++++++------------------- 1 file changed, 51 insertions(+), 62 deletions(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 9ad605839..40f872d00 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -7,90 +7,79 @@ page_id: install This project requires Python 3.10+. -To install on Python 3.13, build tools and Python development headers are required. +***NOTE: Running with Python 3.13 on macOS is not officially supported*** -***NOTE: Running with python 3.13 on macOS is not officially supported*** +> **Recommendation**: Use a Python virtual environment to isolate dependencies. See the [official Python venv documentation](https://docs.python.org/3/library/venv.html) for setup instructions. -See below sections for more platform-specific requirements. -## Install from Source -### Clone the repository - ``` - git clone https://github.com/bitcraze/crazyflie-lib-python.git - ``` -### Install cflib from source - ``` - cd crazyflie-lib-python - pip install -e . - ``` +## Platform Prerequisites -### Uninstall cflib +### Ubuntu/Linux - ``` -pip uninstall cflib - ``` +The Crazyradio is easily recognized on Linux, but you need to set up udev permissions. See the [USB permission instructions](/docs/installation/usb_permissions.md) to configure udev on Ubuntu/Linux. -Note: If you are developing for the cflib you must use python3. On Ubuntu (20.04+) use `pip3` instead of `pip`. +### Windows -### Linux, macOS, Windows +Install the Crazyradio drivers using the [Zadig instructions](https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/usbwindows/). -The following should be executed in the root of the crazyflie-lib-python file tree. +If you're using Python 3.13, you need to install [Visual Studio](https://visualstudio.microsoft.com/downloads/). During the installation process, you only need to select the Desktop Development with C++ workload in the Visual Studio Installer. -#### Virtualenv -This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) -with package dependencies. If you don't want to use virtualenv and don't mind installing cflib dependencies system-wide -you can skip this section. +### macOS -* Install virtualenv: `pip install virtualenv` -* create an environment: `virtualenv venv` -* Activate the environment: `source venv/bin/activate` +For Python 3.12+ on macOS, you need to install libusb using Homebrew: +```bash +$ brew install libusb +``` +If your Homebrew installation is in a non-default location, you might need to link the libusb library: +```bash +$ export DYLD_LIBRARY_PATH="YOUR_HOMEBREW_PATH/lib:$DYLD_LIBRARY_PATH" +``` -* To deactivate the virtualenv when you are done using it `deactivate` +## Installation Methods -### Pre-commit hooks (Ubuntu) -If you want some extra help with keeping to the mandated Python coding style you can install hooks that verify your style at commit time. This is done by running: -``` -$ pip3 install pre-commit -``` -go to crazyflie-lib-python root folder and run -``` -$ pre-commit install -$ pre-commit run --all-files +### From PyPI (Recommended) + +```bash +pip install cflib ``` -This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes and alert you if you need to change anything. -## Testing -### With docker and the toolbelt +### From Source (Development) -For information and installation of the -[toolbelt.](https://github.com/bitcraze/toolbelt) +#### Clone the repository +```bash +git clone https://github.com/bitcraze/crazyflie-lib-python.git +cd crazyflie-lib-python +``` -* Check to see if you pass tests: `tb test` -* Check to see if you pass style guidelines: `tb verify` +#### Install cflib from source +```bash +pip install -e . +``` -Note: Docker and the toolbelt is an optional way of running tests and reduces the -work needed to maintain your python environment. +#### Uninstall cflib +```bash +pip uninstall cflib +``` -## Platform notes +## Development Tools (Optional) -### Linux +### Pre-commit hooks +If you want help maintaining Python coding standards, you can install hooks that verify your style at commit time: -With linux, the crazyradio is easily recognized, but you have to setup UDEVpermissions. Look at the [usb permission instructions](/docs/installation/usb_permissions.md) to setup udev on linux. +```bash +pip3 install pre-commit +cd crazyflie-lib-python +pre-commit install +pre-commit run --all-files +``` -### Windows +This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes. -Look at the [Zadig crazyradio instructions](https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/usbwindows/) to install crazyradio on Windows +### Testing with Docker and Toolbelt -If you're using Python 3.13, you need to install [Visual Studio](https://visualstudio.microsoft.com/downloads/). During the installation process, you only need to select the Desktop Development with C++ workload in the Visual Studio Installer. +For information and installation of the [toolbelt](https://github.com/bitcraze/toolbelt): -### macOS -If you are using python 3.12 on mac you need to install libusb using homebrew. -``` -$ brew install libusb -``` +* Check to see if you pass tests: `tb test` +* Check to see if you pass style guidelines: `tb verify` -If your Homebrew installation is in a non default location or if you want to install libusb in some other way you -might need to link the libusb library like this; -``` -$ export DYLD_LIBRARY_PATH="YOUR_HOMEBREW_PATH/lib:$DYLD_LIBRARY_PATH" -``` +**Note**: Docker and the toolbelt is an optional way of running tests and reduces the work needed to maintain your Python environment. From 2f3225abb14a5ea3deb56d336a3a03d0e0c4d613 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 16 Sep 2025 16:06:15 +0200 Subject: [PATCH 819/861] Include Qualisys SDK as optional dependency Add qtm-rt as optional [qualisys] dependency to help users discover the required package for the Qualisys mocap example. The installation effort is the same, but now it's clear which package is needed. Users with Qualisys systems can now install with: pip install cflib[qualisys] --- examples/mocap/qualisys_hl_commander.py | 3 +++ pyproject.toml | 1 + 2 files changed, 4 insertions(+) diff --git a/examples/mocap/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py index 3470a5c00..d109a16ae 100644 --- a/examples/mocap/qualisys_hl_commander.py +++ b/examples/mocap/qualisys_hl_commander.py @@ -26,6 +26,9 @@ 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 diff --git a/pyproject.toml b/pyproject.toml index 6199bf298..b797bce96 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -50,6 +50,7 @@ Issues = "https://github.com/bitcraze/crazyflie-lib-python/issues" [project.optional-dependencies] dev = ["pre-commit"] +qualisys = ["qtm-rt>=3.0.2"] [tool.setuptools] include-package-data = true From be9ae912a6ebdf0098ead8e9127b3a3ba776d6e7 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 17 Sep 2025 15:10:09 +0200 Subject: [PATCH 820/861] Fix examples requesting thread closure from within same thread Prevents "cannot join current thread" error by avoiding requests to close the IncomingPacketHandler thread from within its own callbacks. Uses flag-based approach instead. --- examples/memory/erase-ow.py | 5 ++++- examples/memory/flash-memory.py | 19 +++++++++---------- examples/memory/read-eeprom.py | 5 ++++- examples/memory/write-eeprom.py | 5 ++++- examples/memory/write-ow.py | 5 ++++- examples/parameters/basicparam.py | 7 +++++-- 6 files changed, 30 insertions(+), 16 deletions(-) diff --git a/examples/memory/erase-ow.py b/examples/memory/erase-ow.py index 16a0bcb54..f79508d6b 100644 --- a/examples/memory/erase-ow.py +++ b/examples/memory/erase-ow.py @@ -67,6 +67,7 @@ def __init__(self, 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 @@ -97,7 +98,7 @@ def _data_updated(self, mem): for key in mem.elements: print('\t\t{}={}'.format(key, mem.elements[key])) - self._cf.close_link() + self.should_disconnect = True def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" @@ -137,6 +138,8 @@ def _disconnected(self, link_uri): # 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 index bd86bf943..e08e2784a 100644 --- a/examples/memory/flash-memory.py +++ b/examples/memory/flash-memory.py @@ -17,7 +17,6 @@ Flash the DS28E05 EEPROM via CRTP. """ import datetime -import os import sys import time @@ -50,6 +49,7 @@ def __init__(self, link_uri): # Initialize variables self.connected = False + self.should_disconnect = False # Public methods @@ -201,11 +201,7 @@ def abort(): # Callback function when data has been written def data_written(mem, addr): print('Data has been written to memory!') - flasher.disconnect() - - # We need to use os.kill because this is a callback - SIGTERM = 15 - os.kill(os.getpid(), SIGTERM) + flasher.should_disconnect = True # Write data sure = input('Are you sure? [y/n] ') @@ -214,9 +210,12 @@ def data_written(mem, addr): abort() mem.write_data(data_written) - # Timeout 10 seconds + # Wait for completion or timeout (10 seconds) for _ in range(10 * 2): + if flasher.should_disconnect: + flasher.disconnect() + break time.sleep(0.5) - print('Apparently data could not be written to memory... :(') - - flasher.disconnect() + else: + print('Apparently data could not be written to memory... :(') + flasher.disconnect() diff --git a/examples/memory/read-eeprom.py b/examples/memory/read-eeprom.py index e3239d6cf..ce8766618 100644 --- a/examples/memory/read-eeprom.py +++ b/examples/memory/read-eeprom.py @@ -64,6 +64,7 @@ def __init__(self, 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 @@ -88,7 +89,7 @@ def _data_updated(self, mem): self._mems_to_update -= 1 if self._mems_to_update == 0: - self._cf.close_link() + self.should_disconnect = True def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" @@ -126,6 +127,8 @@ def _disconnected(self, link_uri): # 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-eeprom.py b/examples/memory/write-eeprom.py index be07b7feb..0a5305e45 100644 --- a/examples/memory/write-eeprom.py +++ b/examples/memory/write-eeprom.py @@ -65,6 +65,7 @@ def __init__(self, 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 @@ -100,7 +101,7 @@ def _data_updated(self, mem): for key in mem.elements: print('\t\t{}={}'.format(key, mem.elements[key])) - self._cf.close_link() + self.should_disconnect = True def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" @@ -138,6 +139,8 @@ def _disconnected(self, link_uri): # 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 index ebadd3508..36ded0c3b 100644 --- a/examples/memory/write-ow.py +++ b/examples/memory/write-ow.py @@ -61,6 +61,7 @@ def __init__(self, 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 @@ -103,7 +104,7 @@ def _data_updated(self, mem): for key in mem.elements: print('\t\t{}={}'.format(key, mem.elements[key])) - self._cf.close_link() + self.should_disconnect = True def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" @@ -141,6 +142,8 @@ def _disconnected(self, link_uri): # 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/parameters/basicparam.py b/examples/parameters/basicparam.py index 51c57a894..b391ae22b 100644 --- a/examples/parameters/basicparam.py +++ b/examples/parameters/basicparam.py @@ -65,6 +65,7 @@ def __init__(self, link_uri): # Variable used to keep main loop occupied until disconnect self.is_connected = True + self.should_disconnect = False self._param_check_list = [] self._param_groups = [] @@ -129,8 +130,8 @@ def _a_pitch_kd_callback(self, name, value): """Callback for pid_attitude.pitch_kd""" print('Read back: {0}={1}'.format(name, value)) - # This is the end of the example, close link - self._cf.close_link() + # This is the end of the example, signal to close link + self.should_disconnect = True def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie @@ -160,4 +161,6 @@ def _disconnected(self, link_uri): # alive, so this is where your application should do something. In our # case we are just waiting until we are disconnected. while pe.is_connected: + if pe.should_disconnect: + pe._cf.close_link() time.sleep(1) From 3f9abb29dff0828d6c0035719534340ff086ecf5 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 18 Sep 2025 10:53:16 +0200 Subject: [PATCH 821/861] Update commander example (autonomous sequence) - Rename to use snake case - Land drone before sending sotp setpoint to prevent crash (detection) --- .../autonomy/{autonomousSequence.py => autonomous_sequence.py} | 1 + 1 file changed, 1 insertion(+) rename examples/autonomy/{autonomousSequence.py => autonomous_sequence.py} (99%) diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomous_sequence.py similarity index 99% rename from examples/autonomy/autonomousSequence.py rename to examples/autonomy/autonomous_sequence.py index ed7445a49..8c7a1b33c 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomous_sequence.py @@ -53,6 +53,7 @@ (-0.5, -0.5, 1.2, 0), (0.0, 0.0, 1.2, 0), (0.0, 0.0, 0.4, 0), + (0.0, 0.0, 0.0, 0), ] From 7d8a02a63c19968b147efbb98b373ac2ccf49a98 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 18 Sep 2025 11:26:26 +0200 Subject: [PATCH 822/861] Standardize example filenames to snake_case - Aligns with PEP 8 module naming conventions - Improves consistency across examples directory --- docs/development/eeprom.md | 2 +- docs/development/wireshark.md | 4 ++-- examples/cfbridge.py | 2 +- examples/logging/{basiclogSync.py => basiclog_sync.py} | 0 examples/memory/{erase-ow.py => erase_ow.py} | 0 examples/memory/{flash-memory.py => flash_memory.py} | 0 examples/memory/{read-eeprom.py => read_eeprom.py} | 0 examples/memory/{read-l5.py => read_l5.py} | 0 examples/memory/{read-ow.py => read_ow.py} | 0 examples/memory/{read-paa3905.py => read_paa3905.py} | 0 examples/memory/{write-eeprom.py => write_eeprom.py} | 0 examples/memory/{write-ow.py => write_ow.py} | 0 .../positioning/{flowsequenceSync.py => flowsequence_sync.py} | 0 examples/radio/{radio-test.py => radio_test.py} | 0 .../swarm/{asynchronizedSwarm.py => asynchronized_swarm.py} | 0 .../swarm/{hl-commander-swarm.py => hl_commander_swarm.py} | 0 examples/swarm/{leader-follower.py => leader_follower.py} | 0 examples/swarm/{swarmSequence.py => swarm_sequence.py} | 0 .../{swarmSequenceCircle.py => swarm_sequence_circle.py} | 0 .../{synchronizedSequence.py => synchronized_sequence.py} | 0 20 files changed, 4 insertions(+), 4 deletions(-) rename examples/logging/{basiclogSync.py => basiclog_sync.py} (100%) rename examples/memory/{erase-ow.py => erase_ow.py} (100%) rename examples/memory/{flash-memory.py => flash_memory.py} (100%) rename examples/memory/{read-eeprom.py => read_eeprom.py} (100%) rename examples/memory/{read-l5.py => read_l5.py} (100%) rename examples/memory/{read-ow.py => read_ow.py} (100%) rename examples/memory/{read-paa3905.py => read_paa3905.py} (100%) rename examples/memory/{write-eeprom.py => write_eeprom.py} (100%) rename examples/memory/{write-ow.py => write_ow.py} (100%) rename examples/positioning/{flowsequenceSync.py => flowsequence_sync.py} (100%) rename examples/radio/{radio-test.py => radio_test.py} (100%) rename examples/swarm/{asynchronizedSwarm.py => asynchronized_swarm.py} (100%) rename examples/swarm/{hl-commander-swarm.py => hl_commander_swarm.py} (100%) rename examples/swarm/{leader-follower.py => leader_follower.py} (100%) rename examples/swarm/{swarmSequence.py => swarm_sequence.py} (100%) rename examples/swarm/{swarmSequenceCircle.py => swarm_sequence_circle.py} (100%) rename examples/swarm/{synchronizedSequence.py => synchronized_sequence.py} (100%) diff --git a/docs/development/eeprom.md b/docs/development/eeprom.md index 74a77c394..e61abe21a 100644 --- a/docs/development/eeprom.md +++ b/docs/development/eeprom.md @@ -17,7 +17,7 @@ following steps: - python3 write-eeprom.py + python3 write_eeprom.py This will find your first Crazyflie (which is the one you connected over USB) and write the default values to the EEPROM. diff --git a/docs/development/wireshark.md b/docs/development/wireshark.md index c8980fd5c..d181e20c6 100644 --- a/docs/development/wireshark.md +++ b/docs/development/wireshark.md @@ -34,14 +34,14 @@ The de facto standard network packet capture format is libpcap (pcap), which is To tell the CFLIB to generate a PCAP file of what it thinks the CRTP traffic looks like you can go: ```bash -$ CRTP_PCAP_LOG=filename.pcap python3 examples/swarm/hl-commander-swarm.py +$ CRTP_PCAP_LOG=filename.pcap python3 examples/swarm/hl_commander_swarm.py $ wireshark filename.pcap ``` and on Windows based computers in a shell window: ```bash > set CRTP_PCAP_LOG=filename.pcap -> python3 examples/swarm/hl-commander-swarm.py +> python3 examples/swarm/hl_commander_swarm.py > wireshark filename.pcap ``` To generate a PCAP file and open it with Wireshark. You can also use the text based `tshark` tool, and you can add a filter, for instance, only shoow CRTP port 8 (Highlevel setpoint): diff --git a/examples/cfbridge.py b/examples/cfbridge.py index ca02b2e56..466e80f9c 100755 --- a/examples/cfbridge.py +++ b/examples/cfbridge.py @@ -9,7 +9,7 @@ @author: Dennis Shtatnov (densht@gmail.com) -Based off example code from crazyflie-lib-python/examples/read-eeprom.py +Based off example code from crazyflie-lib-python/examples/read_eeprom.py """ # import struct import logging diff --git a/examples/logging/basiclogSync.py b/examples/logging/basiclog_sync.py similarity index 100% rename from examples/logging/basiclogSync.py rename to examples/logging/basiclog_sync.py diff --git a/examples/memory/erase-ow.py b/examples/memory/erase_ow.py similarity index 100% rename from examples/memory/erase-ow.py rename to examples/memory/erase_ow.py diff --git a/examples/memory/flash-memory.py b/examples/memory/flash_memory.py similarity index 100% rename from examples/memory/flash-memory.py rename to examples/memory/flash_memory.py diff --git a/examples/memory/read-eeprom.py b/examples/memory/read_eeprom.py similarity index 100% rename from examples/memory/read-eeprom.py rename to examples/memory/read_eeprom.py diff --git a/examples/memory/read-l5.py b/examples/memory/read_l5.py similarity index 100% rename from examples/memory/read-l5.py rename to examples/memory/read_l5.py diff --git a/examples/memory/read-ow.py b/examples/memory/read_ow.py similarity index 100% rename from examples/memory/read-ow.py rename to examples/memory/read_ow.py diff --git a/examples/memory/read-paa3905.py b/examples/memory/read_paa3905.py similarity index 100% rename from examples/memory/read-paa3905.py rename to examples/memory/read_paa3905.py diff --git a/examples/memory/write-eeprom.py b/examples/memory/write_eeprom.py similarity index 100% rename from examples/memory/write-eeprom.py rename to examples/memory/write_eeprom.py diff --git a/examples/memory/write-ow.py b/examples/memory/write_ow.py similarity index 100% rename from examples/memory/write-ow.py rename to examples/memory/write_ow.py diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequence_sync.py similarity index 100% rename from examples/positioning/flowsequenceSync.py rename to examples/positioning/flowsequence_sync.py diff --git a/examples/radio/radio-test.py b/examples/radio/radio_test.py similarity index 100% rename from examples/radio/radio-test.py rename to examples/radio/radio_test.py diff --git a/examples/swarm/asynchronizedSwarm.py b/examples/swarm/asynchronized_swarm.py similarity index 100% rename from examples/swarm/asynchronizedSwarm.py rename to examples/swarm/asynchronized_swarm.py diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl_commander_swarm.py similarity index 100% rename from examples/swarm/hl-commander-swarm.py rename to examples/swarm/hl_commander_swarm.py diff --git a/examples/swarm/leader-follower.py b/examples/swarm/leader_follower.py similarity index 100% rename from examples/swarm/leader-follower.py rename to examples/swarm/leader_follower.py diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarm_sequence.py similarity index 100% rename from examples/swarm/swarmSequence.py rename to examples/swarm/swarm_sequence.py diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarm_sequence_circle.py similarity index 100% rename from examples/swarm/swarmSequenceCircle.py rename to examples/swarm/swarm_sequence_circle.py diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronized_sequence.py similarity index 100% rename from examples/swarm/synchronizedSequence.py rename to examples/swarm/synchronized_sequence.py From 7eb329442823b9473fe4989fb6006740e241a6db Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 18 Sep 2025 11:27:21 +0200 Subject: [PATCH 823/861] Lower "landing" setpoint height in full state setpoint demo to prevent crash (detection) --- examples/autonomy/full_state_setpoint_demo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 9f81f9de3..b5954fc85 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -103,7 +103,7 @@ def run_sequence(scf): 0.0, 0.0, 0.0) print('land') send_setpoint(cf, 2.0, - [0.0, 0.0, 0.2], + [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), From 1c4d0f6fa069544951d477ef50ed18f0f7b36ce3 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 18 Sep 2025 11:35:14 +0200 Subject: [PATCH 824/861] Force Lighthouse deck into V2 mode for multi BS geometry estimation example --- examples/lighthouse/multi_bs_geometry_estimation.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py index 4f6dc7c15..7bdb7c498 100644 --- a/examples/lighthouse/multi_bs_geometry_estimation.py +++ b/examples/lighthouse/multi_bs_geometry_estimation.py @@ -35,6 +35,10 @@ 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. @@ -354,6 +358,10 @@ def connect_and_estimate(uri: str, file_name: str | None = None): 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.') From 6b10a11b302e5f486be92b3132a70ac1cdbcd1db Mon Sep 17 00:00:00 2001 From: aris Date: Thu, 18 Sep 2025 11:49:07 +0200 Subject: [PATCH 825/861] Added arming in asynchronized swarm example --- examples/swarm/asynchronizedSwarm.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/swarm/asynchronizedSwarm.py b/examples/swarm/asynchronizedSwarm.py index 08fdd4cfe..3d9b3d591 100644 --- a/examples/swarm/asynchronizedSwarm.py +++ b/examples/swarm/asynchronizedSwarm.py @@ -152,5 +152,8 @@ def async_flight(scf): 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) From 08516f95f1e990fe7b9a2a84803ad918580a0dad Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 18 Sep 2025 15:17:24 +0200 Subject: [PATCH 826/861] Fix ramp example threading --- examples/motors/ramp.py | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index b0f78831f..0c6b1bc61 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -64,10 +64,6 @@ def _connected(self, link_uri): 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)""" @@ -82,7 +78,7 @@ 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): + def ramp_motors(self): thrust_mult = 1 thrust_step = 500 thrust = 20000 @@ -103,10 +99,12 @@ def _ramp_motors(self): # 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) - # 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 + + def disconnect(self): self._cf.close_link() @@ -114,4 +112,8 @@ def _ramp_motors(self): # Initialize the low-level drivers cflib.crtp.init_drivers() - le = MotorRampExample(uri) + me = MotorRampExample(uri) + + me.ramp_motors() + + me.disconnect() From 3212aa60f914732b52808240d2ee761ecb2f6a08 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Thu, 18 Sep 2025 15:40:11 +0200 Subject: [PATCH 827/861] Remove unused Thread import in ramp.py --- examples/motors/ramp.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index 0c6b1bc61..3fe556388 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -27,7 +27,6 @@ """ import logging import time -from threading import Thread import cflib from cflib.crazyflie import Crazyflie From bae3af9f39cb2124243a2f0779a1eaf34071a49c Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Thu, 18 Sep 2025 15:48:31 +0200 Subject: [PATCH 828/861] Move venv recommendation up --- docs/installation/install.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 40f872d00..651e487eb 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -7,10 +7,10 @@ page_id: install This project requires Python 3.10+. -***NOTE: Running with Python 3.13 on macOS is not officially supported*** - > **Recommendation**: Use a Python virtual environment to isolate dependencies. See the [official Python venv documentation](https://docs.python.org/3/library/venv.html) for setup instructions. +***NOTE: Running with Python 3.13 on macOS is not officially supported*** + ## Platform Prerequisites ### Ubuntu/Linux From de587d769799f254c85b249d14de98fb216f0ec4 Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Thu, 18 Sep 2025 15:51:58 +0200 Subject: [PATCH 829/861] Abstract away docker implementation details from user docs Users don't need to know the toolbelt runs on docker --- docs/installation/install.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index 651e487eb..a81bffa2f 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -75,11 +75,11 @@ pre-commit run --all-files This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes. -### Testing with Docker and Toolbelt +### Testing with the Toolbelt For information and installation of the [toolbelt](https://github.com/bitcraze/toolbelt): * Check to see if you pass tests: `tb test` * Check to see if you pass style guidelines: `tb verify` -**Note**: Docker and the toolbelt is an optional way of running tests and reduces the work needed to maintain your Python environment. +**Note**: The toolbelt is an optional way of running tests and reduces the work needed to maintain your Python environment. From 76cbbda2bab9724d7bac65bbac71d98e74868688 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 18 Sep 2025 16:26:14 +0200 Subject: [PATCH 830/861] Update installation instructions for Ubuntu 20.04 and correct pip command --- docs/installation/install.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index a81bffa2f..de18998f3 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -17,6 +17,8 @@ This project requires Python 3.10+. The Crazyradio is easily recognized on Linux, but you need to set up udev permissions. See the [USB permission instructions](/docs/installation/usb_permissions.md) to configure udev on Ubuntu/Linux. +> **Note for Ubuntu 20.04 users**: Use `pip3` instead of `pip` in all installation commands below. + ### Windows Install the Crazyradio drivers using the [Zadig instructions](https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/usbwindows/). @@ -67,7 +69,7 @@ pip uninstall cflib If you want help maintaining Python coding standards, you can install hooks that verify your style at commit time: ```bash -pip3 install pre-commit +pip install pre-commit cd crazyflie-lib-python pre-commit install pre-commit run --all-files From ea6c9bdc64dac07c1c96cd1f482f7361d2a2fb36 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Fri, 19 Sep 2025 11:20:47 +0200 Subject: [PATCH 831/861] Add optional dependency for motioncapture --- pyproject.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index b797bce96..a2c3a5dc7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -50,7 +50,8 @@ Issues = "https://github.com/bitcraze/crazyflie-lib-python/issues" [project.optional-dependencies] dev = ["pre-commit"] -qualisys = ["qtm-rt>=3.0.2"] +qualisys = ["qtm-rt~=3.0.2"] +motioncapture = ["motioncapture~=1.0a4"] [tool.setuptools] include-package-data = true From 7885e5f05caccb95519a220fde9c7bfb370cc017 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 22 Sep 2025 15:27:37 +0200 Subject: [PATCH 832/861] Remove comment about lack of Python 3.13 support on MacOS. Now supporting 3.10-3.13 on MacOS --- docs/installation/install.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/docs/installation/install.md b/docs/installation/install.md index de18998f3..97d7bd2a2 100644 --- a/docs/installation/install.md +++ b/docs/installation/install.md @@ -9,8 +9,6 @@ This project requires Python 3.10+. > **Recommendation**: Use a Python virtual environment to isolate dependencies. See the [official Python venv documentation](https://docs.python.org/3/library/venv.html) for setup instructions. -***NOTE: Running with Python 3.13 on macOS is not officially supported*** - ## Platform Prerequisites ### Ubuntu/Linux From 2cc24a7694814a0273393567a631272fa85b6ab2 Mon Sep 17 00:00:00 2001 From: aris Date: Fri, 26 Sep 2025 10:31:18 +0200 Subject: [PATCH 833/861] Moved the arming_request --- examples/aideck/fpv.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index bea7924de..908c836ef 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -184,12 +184,12 @@ def __init__(self, URI): self._imgDownload = ImageDownloader(self.cf.link.cpx, self.updateImage) self._imgDownload.start() - self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} - # 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) From a40f6b895a6b51e1666efcc21fc168fa32fb23de Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 7 Oct 2025 15:18:10 +0200 Subject: [PATCH 834/861] Added missing description --- cflib/utils/reset_estimator.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/cflib/utils/reset_estimator.py b/cflib/utils/reset_estimator.py index e22debbd1..42aaa59d9 100644 --- a/cflib/utils/reset_estimator.py +++ b/cflib/utils/reset_estimator.py @@ -1,3 +1,30 @@ +# -*- 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 . +""" +The reset_estimator() function resets the Kalman filter and makes the +Crazyflie wait until it has an accurate position estimate. It is mostly +used with the Lighthouse Positioning System to ensure that the Crazyflie +won't take off before it has accurate positioning data. +""" import time from cflib.crazyflie.log import LogConfig From 562e23da3ef57ce51e939afbc37ee4bb291586e9 Mon Sep 17 00:00:00 2001 From: aris Date: Thu, 9 Oct 2025 10:18:48 +0200 Subject: [PATCH 835/861] Added a simple mocap swarm example --- examples/mocap/mocap_swarm.py | 148 ++++++++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 examples/mocap/mocap_swarm.py diff --git a/examples/mocap/mocap_swarm.py b/examples/mocap/mocap_swarm.py new file mode 100644 index 000000000..432d7718d --- /dev/null +++ b/examples/mocap/mocap_swarm.py @@ -0,0 +1,148 @@ +# -*- 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 = 'qualisys' + +# The host name or ip address of the mocap system +host_name = '192.168.5.21' + +uris = [ + 'radio://0/80/2M/E7E7E7E7E7', + 'radio://0/80/2M/E7E7E7E7E8', + 'radio://0/80/2M/E7E7E7E7E9', + # 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 +] + +# Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive +rbs = { + uris[0]: 'cf1', + uris[1]: 'cf2', + uris[2]: 'cf3', +} + + +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() From 03d50779f0b2dbe5be5f930151433ecfbb6ce614 Mon Sep 17 00:00:00 2001 From: aris Date: Thu, 9 Oct 2025 11:21:16 +0200 Subject: [PATCH 836/861] Updated the description and addressed flake8 checks --- .../multicf_mocap_hl_and_motion_commander.py | 43 +++++++++---------- 1 file changed, 21 insertions(+), 22 deletions(-) diff --git a/examples/mocap/multicf_mocap_hl_and_motion_commander.py b/examples/mocap/multicf_mocap_hl_and_motion_commander.py index 265152c6e..3390c0182 100644 --- a/examples/mocap/multicf_mocap_hl_and_motion_commander.py +++ b/examples/mocap/multicf_mocap_hl_and_motion_commander.py @@ -6,7 +6,7 @@ # | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2023 Bitcraze AB +# 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 @@ -21,27 +21,24 @@ # along with this program. If not, see . """ Example of how to connect to a motion capture system and feed positions (only) -to a multiple Crazyflie, using the motioncapture library. +to multiple Crazyflies, using the motioncapture library. -The script uses the high level and the motion commander to fly circles and waypoints. +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 +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, Lock +from threading import Thread import motioncapture import cflib.crtp from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm -from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils.reset_estimator import reset_estimator -from cflib.positioning.position_hl_commander import PositionHlCommander 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' @@ -49,6 +46,7 @@ # The host name or ip address of the mocap system host_name = '192.168.5.21' + class MocapWrapper(Thread): def __init__(self, active_rbs_cfs): Thread.__init__(self) @@ -75,8 +73,9 @@ def run(self): if self.counter == 200: self.counter = 0 -def run_sequence(scf): - print("This is: ", scf._link_uri) + +def run_sequence(scf: SyncCrazyflie): + print('This is: ', scf._link_uri) scf.cf.platform.send_arming_request(True) time.sleep(1.0) @@ -85,7 +84,7 @@ def run_sequence(scf): with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.5) # 6 repetitions, at .5 speed, take ~1':15 and drop the battery from 4.2 to 3.9V - for i in range(6): # fly a triangle with changing altitude + for i in range(6): # 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) @@ -94,7 +93,7 @@ def run_sequence(scf): with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.3) # Scripted behaviour - for i in range(6): # fly side to side + for i in range(6): # 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) @@ -114,24 +113,24 @@ def run_sequence(scf): mc.down(0.5) # .land() is automatic when exiting the scope of context "with PositionHlCommander" + if __name__ == '__main__': cflib.crtp.init_drivers() # Uncomment the URIs to connect to uris = [ - 'radio://0/80/2M/E7E7E7E7E7', - # 'radio://0/90/2M/E7E7E7E7E8', - # 'radio://0/100/2M/E7E7E7E7E9', - ] + 'radio://0/80/2M/E7E7E7E7E7', + # 'radio://0/90/2M/E7E7E7E7E8', + # 'radio://0/100/2M/E7E7E7E7E9', + ] # Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive rbs = { - 'radio://0/80/2M/E7E7E7E7E7' : 'E7', - 'radio://0/90/2M/E7E7E7E7E8' : 'E8', - 'radio://0/100/2M/E7E7E7E7E9' : 'E9' - } + 'radio://0/80/2M/E7E7E7E7E7': 'E7', + 'radio://0/90/2M/E7E7E7E7E8': 'E8', + 'radio://0/100/2M/E7E7E7E7E9': 'E9' + } - 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()} From 2d017fb753f2883972dd9a91b97895532dbfda5f Mon Sep 17 00:00:00 2001 From: aris Date: Thu, 9 Oct 2025 11:23:53 +0200 Subject: [PATCH 837/861] Updated uris and moved them to the top --- .../multicf_mocap_hl_and_motion_commander.py | 36 ++++++++++--------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/examples/mocap/multicf_mocap_hl_and_motion_commander.py b/examples/mocap/multicf_mocap_hl_and_motion_commander.py index 3390c0182..91887b8b8 100644 --- a/examples/mocap/multicf_mocap_hl_and_motion_commander.py +++ b/examples/mocap/multicf_mocap_hl_and_motion_commander.py @@ -46,6 +46,22 @@ # The host name or ip address of the mocap system host_name = '192.168.5.21' +# Uncomment the URIs to connect to +uris = [ + 'radio://0/80/2M/E7E7E7E7E7', + 'radio://0/80/2M/E7E7E7E7E8', + 'radio://0/80/2M/E7E7E7E7E9', + # 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 +] + +# Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive +rbs = { + uris[0]: 'cf1', + uris[1]: 'cf2', + uris[2]: 'cf3', +} + class MocapWrapper(Thread): def __init__(self, active_rbs_cfs): @@ -80,7 +96,7 @@ def run_sequence(scf: SyncCrazyflie): time.sleep(1.0) # .takeoff() is automatic when entering the "with PositionHlCommander" context - if scf._link_uri == 'radio://0/80/2M/E7E7E7E7E7': + if scf._link_uri == uris[0]: with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.5) # 6 repetitions, at .5 speed, take ~1':15 and drop the battery from 4.2 to 3.9V @@ -89,7 +105,7 @@ def run_sequence(scf: SyncCrazyflie): 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 scf._link_uri == 'radio://0/90/2M/E7E7E7E7E8': + elif scf._link_uri == uris[0]: with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.3) # Scripted behaviour @@ -97,7 +113,7 @@ def run_sequence(scf: SyncCrazyflie): 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 scf._link_uri == 'radio://0/100/2M/E7E7E7E7E9': + elif scf._link_uri == uris[0]: with MotionCommander(scf) as mc: # 3 right loops and 3 left loops take ~1' and drop the battery from 4.2 to 4.0V mc.back(0.8) @@ -117,20 +133,6 @@ def run_sequence(scf: SyncCrazyflie): if __name__ == '__main__': cflib.crtp.init_drivers() - # Uncomment the URIs to connect to - uris = [ - 'radio://0/80/2M/E7E7E7E7E7', - # 'radio://0/90/2M/E7E7E7E7E8', - # 'radio://0/100/2M/E7E7E7E7E9', - ] - - # Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive - rbs = { - 'radio://0/80/2M/E7E7E7E7E7': 'E7', - 'radio://0/90/2M/E7E7E7E7E8': 'E8', - 'radio://0/100/2M/E7E7E7E7E9': 'E9' - } - 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()} From c7d7b77f3496fc8419f8b8b193c8d29a06309809 Mon Sep 17 00:00:00 2001 From: aris Date: Thu, 9 Oct 2025 11:28:17 +0200 Subject: [PATCH 838/861] renamed the script --- ..._hl_and_motion_commander.py => mocap_swarm_multi_commander.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/mocap/{multicf_mocap_hl_and_motion_commander.py => mocap_swarm_multi_commander.py} (100%) diff --git a/examples/mocap/multicf_mocap_hl_and_motion_commander.py b/examples/mocap/mocap_swarm_multi_commander.py similarity index 100% rename from examples/mocap/multicf_mocap_hl_and_motion_commander.py rename to examples/mocap/mocap_swarm_multi_commander.py From 357a5ed0f00928da9a04c096272c5178868c3472 Mon Sep 17 00:00:00 2001 From: aris Date: Thu, 9 Oct 2025 16:57:27 +0200 Subject: [PATCH 839/861] Added the activate_kalman_estimator function --- examples/mocap/mocap_swarm.py | 2 +- examples/mocap/mocap_swarm_multi_commander.py | 12 +++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/examples/mocap/mocap_swarm.py b/examples/mocap/mocap_swarm.py index 432d7718d..a0c37448e 100644 --- a/examples/mocap/mocap_swarm.py +++ b/examples/mocap/mocap_swarm.py @@ -84,7 +84,7 @@ def run(self): # 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}") + print(f'Sent pos {pos} for {name}') if self.counter == 200: self.counter = 0 diff --git a/examples/mocap/mocap_swarm_multi_commander.py b/examples/mocap/mocap_swarm_multi_commander.py index 91887b8b8..e56bb841f 100644 --- a/examples/mocap/mocap_swarm_multi_commander.py +++ b/examples/mocap/mocap_swarm_multi_commander.py @@ -85,11 +85,19 @@ def run(self): # 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}") + 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) @@ -138,6 +146,8 @@ def run_sequence(scf: SyncCrazyflie): active_rbs_cfs = {rbs[uri]: scf.cf for uri, scf in swarm._cfs.items()} mocap_thred = MocapWrapper(active_rbs_cfs) + swarm.parallel_safe(activate_kalman_estimator) + time.sleep(1) swarm.reset_estimators() time.sleep(2) swarm.parallel_safe(run_sequence) From 7bf438b0f1a03a84cc13304ab8b1dba89e903c85 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Mon, 13 Oct 2025 13:48:07 -0400 Subject: [PATCH 840/861] optitrack test --- examples/mocap/mocap_swarm.py | 27 +++++------ examples/mocap/mocap_swarm_multi_commander.py | 45 +++++++++---------- 2 files changed, 32 insertions(+), 40 deletions(-) diff --git a/examples/mocap/mocap_swarm.py b/examples/mocap/mocap_swarm.py index a0c37448e..e84c89ad4 100644 --- a/examples/mocap/mocap_swarm.py +++ b/examples/mocap/mocap_swarm.py @@ -41,25 +41,22 @@ # The type of the mocap system # Valid options are: 'vicon', 'optitrack', 'optitrack_closed_source', 'qualisys', 'nokov', 'vrpn', 'motionanalysis' -mocap_system_type = 'qualisys' +mocap_system_type = 'optitrack' # The host name or ip address of the mocap system -host_name = '192.168.5.21' - -uris = [ - 'radio://0/80/2M/E7E7E7E7E7', - 'radio://0/80/2M/E7E7E7E7E8', - 'radio://0/80/2M/E7E7E7E7E9', - # 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 -] +# host_name = '192.168.5.21' +host_name = '10.223.0.31' # Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive -rbs = { - uris[0]: 'cf1', - uris[1]: 'cf2', - uris[2]: 'cf3', -} +swarm_config = [ + ('radio://0/80/2M/E7E7E7E7E7', 'cf1'), +# ('radio://0/90/2M/E7E7E7E7E8', 'cf2'), +# ('radio://0/100/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): diff --git a/examples/mocap/mocap_swarm_multi_commander.py b/examples/mocap/mocap_swarm_multi_commander.py index e56bb841f..8d6785866 100644 --- a/examples/mocap/mocap_swarm_multi_commander.py +++ b/examples/mocap/mocap_swarm_multi_commander.py @@ -39,28 +39,25 @@ 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 = '192.168.5.21' - -# Uncomment the URIs to connect to -uris = [ - 'radio://0/80/2M/E7E7E7E7E7', - 'radio://0/80/2M/E7E7E7E7E8', - 'radio://0/80/2M/E7E7E7E7E9', - # 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 -] +# host_name = '192.168.5.21' +host_name = '10.223.0.31' # Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive -rbs = { - uris[0]: 'cf1', - uris[1]: 'cf2', - uris[2]: 'cf3', -} +swarm_config = [ + ('radio://0/80/2M/E7E7E7E7E7', 'cf1'), +# ('radio://0/90/2M/E7E7E7E7E8', 'cf2'), +# ('radio://0/100/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): @@ -104,35 +101,33 @@ def run_sequence(scf: SyncCrazyflie): time.sleep(1.0) # .takeoff() is automatic when entering the "with PositionHlCommander" context - if scf._link_uri == uris[0]: + if rbs[scf._link_uri] == 'cf1': with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.5) - # 6 repetitions, at .5 speed, take ~1':15 and drop the battery from 4.2 to 3.9V - for i in range(6): # fly a triangle with changing altitude + 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 scf._link_uri == uris[0]: + elif rbs[scf._link_uri] == 'cf2': with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.set_default_velocity(0.3) - # Scripted behaviour - for i in range(6): # fly side to side + 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 scf._link_uri == uris[0]: + elif rbs[scf._link_uri] == 'cf3': with MotionCommander(scf) as mc: - # 3 right loops and 3 left loops take ~1' and drop the battery from 4.2 to 4.0V + # 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=1080) + 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=1080) + 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" From 094d3a836dde7ce6625176e382d2b09f53eead91 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Mon, 13 Oct 2025 14:01:26 -0400 Subject: [PATCH 841/861] typo --- examples/mocap/mocap_swarm_multi_commander.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/mocap/mocap_swarm_multi_commander.py b/examples/mocap/mocap_swarm_multi_commander.py index 8d6785866..4ae3e8ff6 100644 --- a/examples/mocap/mocap_swarm_multi_commander.py +++ b/examples/mocap/mocap_swarm_multi_commander.py @@ -139,7 +139,7 @@ def run_sequence(scf: SyncCrazyflie): 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_thred = MocapWrapper(active_rbs_cfs) + mocap_thread = MocapWrapper(active_rbs_cfs) swarm.parallel_safe(activate_kalman_estimator) time.sleep(1) @@ -147,4 +147,4 @@ def run_sequence(scf: SyncCrazyflie): time.sleep(2) swarm.parallel_safe(run_sequence) - mocap_thred.close() + mocap_thread.close() From 4c84f36fb52b82f9d8d53413f42599a6bc49edbe Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 14 Oct 2025 11:09:18 +0200 Subject: [PATCH 842/861] Fixed failing CI --- examples/mocap/mocap_swarm.py | 6 +++--- examples/mocap/mocap_swarm_multi_commander.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/mocap/mocap_swarm.py b/examples/mocap/mocap_swarm.py index e84c89ad4..2d4ea220e 100644 --- a/examples/mocap/mocap_swarm.py +++ b/examples/mocap/mocap_swarm.py @@ -50,9 +50,9 @@ # 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/90/2M/E7E7E7E7E8', 'cf2'), -# ('radio://0/100/2M/E7E7E7E7E9', 'cf3'), -# Add more URIs if you want more copters in the swarm + # ('radio://0/90/2M/E7E7E7E7E8', 'cf2'), + # ('radio://0/100/2M/E7E7E7E7E9', 'cf3'), + # Add more URIs if you want more copters in the swarm ] uris = [uri for uri, _ in swarm_config] diff --git a/examples/mocap/mocap_swarm_multi_commander.py b/examples/mocap/mocap_swarm_multi_commander.py index 4ae3e8ff6..f97920d3a 100644 --- a/examples/mocap/mocap_swarm_multi_commander.py +++ b/examples/mocap/mocap_swarm_multi_commander.py @@ -51,9 +51,9 @@ # 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/90/2M/E7E7E7E7E8', 'cf2'), -# ('radio://0/100/2M/E7E7E7E7E9', 'cf3'), -# Add more URIs if you want more copters in the swarm + # ('radio://0/90/2M/E7E7E7E7E8', 'cf2'), + # ('radio://0/100/2M/E7E7E7E7E9', 'cf3'), + # Add more URIs if you want more copters in the swarm ] uris = [uri for uri, _ in swarm_config] From da0b4b3826ec3b4254bcd1a138f301c44b750ba0 Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 14 Oct 2025 13:02:43 +0200 Subject: [PATCH 843/861] Changed radio channels --- examples/mocap/mocap_swarm.py | 5 ++--- examples/mocap/mocap_swarm_multi_commander.py | 5 ++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/examples/mocap/mocap_swarm.py b/examples/mocap/mocap_swarm.py index 2d4ea220e..134586028 100644 --- a/examples/mocap/mocap_swarm.py +++ b/examples/mocap/mocap_swarm.py @@ -44,14 +44,13 @@ mocap_system_type = 'optitrack' # The host name or ip address of the mocap system -# host_name = '192.168.5.21' 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/90/2M/E7E7E7E7E8', 'cf2'), - # ('radio://0/100/2M/E7E7E7E7E9', 'cf3'), + # ('radio://0/80/2M/E7E7E7E7E8', 'cf2'), + # ('radio://0/80/2M/E7E7E7E7E9', 'cf3'), # Add more URIs if you want more copters in the swarm ] diff --git a/examples/mocap/mocap_swarm_multi_commander.py b/examples/mocap/mocap_swarm_multi_commander.py index f97920d3a..71ab49778 100644 --- a/examples/mocap/mocap_swarm_multi_commander.py +++ b/examples/mocap/mocap_swarm_multi_commander.py @@ -45,14 +45,13 @@ mocap_system_type = 'optitrack' # The host name or ip address of the mocap system -# host_name = '192.168.5.21' 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/90/2M/E7E7E7E7E8', 'cf2'), - # ('radio://0/100/2M/E7E7E7E7E9', 'cf3'), + # ('radio://0/80/2M/E7E7E7E7E8', 'cf2'), + # ('radio://0/80/2M/E7E7E7E7E9', 'cf3'), # Add more URIs if you want more copters in the swarm ] From 95d0c2a7ab9cf4509da87cb4a94db86d0c77ed34 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 15 Oct 2025 16:05:08 +0200 Subject: [PATCH 844/861] Add HP RGBW deck example scripts with thermal monitoring MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add two Python examples for controlling the HP RGBW deck: - hprgbw_set.py: Set a static color with configurable RGBW values - Supports both rgbw() and rgb() classes (with auto white extraction) - Easy-to-edit color examples for users - Configurable brightness correction parameter - hprgbw_cycle.py: Cycle through HSV color space - HSV to RGBW conversion for smooth color transitions - Continuous 360° hue cycling Both examples include: - Thermal status monitoring via log subscription (100ms polling) - Warning messages when thermal throttling is active - Proper LED shutdown on Ctrl-C - pack_rgbw() helper to format 0xWWRRGGBB uint32 values --- examples/hprgbw/hprgbw_cycle.py | 109 ++++++++++++++++++++++++++++++++ examples/hprgbw/hprgbw_set.py | 88 ++++++++++++++++++++++++++ 2 files changed, 197 insertions(+) create mode 100644 examples/hprgbw/hprgbw_cycle.py create mode 100644 examples/hprgbw/hprgbw_set.py diff --git a/examples/hprgbw/hprgbw_cycle.py b/examples/hprgbw/hprgbw_cycle.py new file mode 100644 index 000000000..ce9c268ac --- /dev/null +++ b/examples/hprgbw/hprgbw_cycle.py @@ -0,0 +1,109 @@ +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +# URI to the Crazyflie to connect to +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + + +class rgbw: + def __init__(self, r: int, g: int, b: int, w: int = 0): + self.r = r + self.g = g + self.b = b + self.w = w + + +class hsv: + def __init__(self, h: int, s: int, v: int): + self.h = h + self.s = s + self.v = v + + +def hsv_to_rgbw(input: hsv) -> rgbw: + h, s, v = input.h, input.s / 255.0, input.v / 255.0 + + if s == 0: + r = g = b = v + else: + h = h / 60.0 + i = int(h) + f = h - i + p = v * (1.0 - s) + q = v * (1.0 - s * f) + t = v * (1.0 - s * (1.0 - f)) + + if i == 0: + r, g, b = v, t, p + elif i == 1: + r, g, b = q, v, p + elif i == 2: + r, g, b = p, v, t + elif i == 3: + r, g, b = p, q, v + elif i == 4: + r, g, b = t, p, v + else: + r, g, b = v, p, q + + return rgbw(int(r * 255), int(g * 255), int(b * 255), 0) + + +def cycle_colors(step: int) -> rgbw: + h = step % 360 + s = 255 + v = 255 + return hsv_to_rgbw(hsv(h, s, v)) + + +def rgbw_to_rgbwuint32(input: rgbw) -> int: + return (input.r << 24) | (input.g << 16) | (input.b << 8) | input.w + + +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 + + # Thermal status callback + def thermal_status_callback(timestamp, data, logconf): + throttle_pct = data['hprgbw.throttlePct'] + if throttle_pct > 0: + temp = data['hprgbw.deckTemp'] + print(f"WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%") + + # Setup log configuration for thermal monitoring + log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) + log_conf.add_variable('hprgbw.deckTemp', 'uint8_t') + log_conf.add_variable('hprgbw.throttlePct', 'uint8_t') + + cf.log.add_config(log_conf) + log_conf.data_received_cb.add_callback(thermal_status_callback) + log_conf.start() + + # Brightness correction: balances luminance across R/G/B/W channels + # Set to 1 (enabled, default) for perceptually uniform colors + # Set to 0 (disabled) for maximum brightness per channel + cf.param.set_value('hprgbw.brightnessCorr', 1) + time.sleep(0.1) + + try: + print('Cycling through colors. Press Ctrl-C to stop.') + while True: + for i in range(0, 360, 1): + color = cycle_colors(i) + # print(color.r, color.g, color.b) + color_uint32 = rgbw_to_rgbwuint32(color) + cf.param.set_value('hprgbw.rgbw8888', str(color_uint32)) + time.sleep(0.01) + except KeyboardInterrupt: + print('\nStopping and turning off LED...') + cf.param.set_value('hprgbw.rgbw8888', '0') + time.sleep(0.1) diff --git a/examples/hprgbw/hprgbw_set.py b/examples/hprgbw/hprgbw_set.py new file mode 100644 index 000000000..8fe0c4dfd --- /dev/null +++ b/examples/hprgbw/hprgbw_set.py @@ -0,0 +1,88 @@ +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +# URI to the Crazyflie to connect to +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + + +class rgbw: + def __init__(self, r: int, g: int, b: int, w: int = 0): + self.r = r + self.g = g + self.b = b + self.w = w + + +class rgb: + def __init__(self, r: int, g: int, b: int): + self.w = min(r, g, b) + self.r = r - self.w + self.g = g - self.w + self.b = b - self.w + + +def pack_rgbw(input: rgbw | rgb) -> int: + """Pack RGBW values into uint32 format: 0xWWRRGGBB""" + return (input.w << 24) | (input.r << 16) | (input.g << 8) | input.b + + +if __name__ == '__main__': + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + + # Thermal status callback + def thermal_status_callback(timestamp, data, logconf): + throttle_pct = data['hprgbw.throttlePct'] + if throttle_pct > 0: + temp = data['hprgbw.deckTemp'] + print(f"WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%") + + # Setup log configuration for thermal monitoring + log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) + log_conf.add_variable('hprgbw.deckTemp', 'uint8_t') + log_conf.add_variable('hprgbw.throttlePct', 'uint8_t') + + cf.log.add_config(log_conf) + log_conf.data_received_cb.add_callback(thermal_status_callback) + log_conf.start() + + # Brightness correction: balances luminance across R/G/B/W channels + # Set to 1 (enabled, default) for perceptually uniform colors + # Set to 0 (disabled) for maximum brightness per channel + cf.param.set_value('hprgbw.brightnessCorr', 1) + time.sleep(0.1) + + try: + # ======================================== + # Set your desired color here: + # ======================================== + # Examples: + # color = rgbw(r=255, g=0, b=0, w=0) # Pure red + # color = rgbw(r=0, g=255, b=0, w=0) # Pure green + # color = rgbw(r=0, g=0, b=255, w=0) # Pure blue + # color = rgbw(r=0, g=0, b=0, w=255) # Pure white LED + # color = rgbw(r=255, g=128, b=0, w=50) # Orange + white + # color = rgb(r=255, g=255, b=255) # White (auto W extraction) + + color = rgbw(r=255, g=0, b=100, w=0) + # ======================================== + + color_uint32 = pack_rgbw(color) + cf.param.set_value('hprgbw.rgbw8888', color_uint32) + time.sleep(0.01) + print(f"Setting LED to R={color.r}, G={color.g}, B={color.b}, W={color.w}") + print('Press Ctrl-C to turn off LED and exit.') + while True: + time.sleep(0.1) + except KeyboardInterrupt: + print('\nStopping and turning off LED...') + cf.param.set_value('hprgbw.rgbw8888', 0) + time.sleep(0.1) From d11d9d7100aebdcf1f61bd5a2f091be4c7eec1af Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 16 Oct 2025 11:43:42 +0200 Subject: [PATCH 845/861] Add generic LED deck controller example --- examples/generic_led_deck/led_cycle.py | 84 ++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100644 examples/generic_led_deck/led_cycle.py diff --git a/examples/generic_led_deck/led_cycle.py b/examples/generic_led_deck/led_cycle.py new file mode 100644 index 000000000..4779ed7f0 --- /dev/null +++ b/examples/generic_led_deck/led_cycle.py @@ -0,0 +1,84 @@ +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +# URI to the Crazyflie to connect to +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + + +class rgb: + def __init__(self, r: int, g: int, b: int): + self.r = r + self.g = g + self.b = b + + +class hsv: + def __init__(self, h: int, s: int, v: int): + self.h = h + self.s = s + self.v = v + + +def hsv_to_rgb(input: hsv) -> rgb: + h, s, v = input.h, input.s / 255.0, input.v / 255.0 + + if s == 0: + r = g = b = v + else: + h = h / 60.0 + i = int(h) + f = h - i + p = v * (1.0 - s) + q = v * (1.0 - s * f) + t = v * (1.0 - s * (1.0 - f)) + + if i == 0: + r, g, b = v, t, p + elif i == 1: + r, g, b = q, v, p + elif i == 2: + r, g, b = p, v, t + elif i == 3: + r, g, b = p, q, v + elif i == 4: + r, g, b = t, p, v + else: + r, g, b = v, p, q + + return rgb(int(r * 255), int(g * 255), int(b * 255)) + + +def cycle_colors(step: int) -> rgb: + h = step % 360 + s = 255 + v = 255 + return hsv_to_rgb(hsv(h, s, v)) + + +def construct_uint32_color(input: rgb) -> int: + return (input.r << 16) | (input.g << 8) | input.b + + +if __name__ == '__main__': + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + + try: + print('Cycling through colors. Press Ctrl-C to stop.') + while True: + for i in range(0, 360, 1): + color = cycle_colors(i) + color_uint32 = construct_uint32_color(color) + cf.param.set_value('led_deck_ctrl.rgb888', str(color_uint32)) + time.sleep(0.01) + except KeyboardInterrupt: + print('\nStopping and turning off LED...') + cf.param.set_value('led_deck_ctrl.rgb888', '0') + time.sleep(0.1) From ce4c1b8ab2032570eddc1ff8afc8b42a2a3c0684 Mon Sep 17 00:00:00 2001 From: aris Date: Fri, 17 Oct 2025 13:35:39 +0200 Subject: [PATCH 846/861] Added visualization script --- .../lighthouse_config_visualization.py | 155 ++++++++++++++++++ 1 file changed, 155 insertions(+) create mode 100644 examples/lighthouse/lighthouse_config_visualization.py diff --git a/examples/lighthouse/lighthouse_config_visualization.py b/examples/lighthouse/lighthouse_config_visualization.py new file mode 100644 index 000000000..5174e6b51 --- /dev/null +++ b/examples/lighthouse/lighthouse_config_visualization.py @@ -0,0 +1,155 @@ +# -*- 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() From a43ff8edcbe7195a2fb201dbd8446f1e88a0505e Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 21 Oct 2025 14:04:55 +0200 Subject: [PATCH 847/861] Added a script for mass lighthouse config upload --- examples/lighthouse/lighthouse_mass_upload.py | 88 +++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 examples/lighthouse/lighthouse_mass_upload.py diff --git a/examples/lighthouse/lighthouse_mass_upload.py b/examples/lighthouse/lighthouse_mass_upload.py new file mode 100644 index 000000000..ee4747761 --- /dev/null +++ b/examples/lighthouse/lighthouse_mass_upload.py @@ -0,0 +1,88 @@ +# -*- 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 From fa821118eb95cd43d3992a2204e8a400f97e5f6d Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 22 Oct 2025 14:24:03 +0200 Subject: [PATCH 848/861] Use updated Color LED deck parameter names --- .../color_led_cycle.py} | 0 .../color_led_set.py} | 14 +++++++------- 2 files changed, 7 insertions(+), 7 deletions(-) rename examples/{hprgbw/hprgbw_cycle.py => color_led_deck/color_led_cycle.py} (100%) rename examples/{hprgbw/hprgbw_set.py => color_led_deck/color_led_set.py} (87%) diff --git a/examples/hprgbw/hprgbw_cycle.py b/examples/color_led_deck/color_led_cycle.py similarity index 100% rename from examples/hprgbw/hprgbw_cycle.py rename to examples/color_led_deck/color_led_cycle.py diff --git a/examples/hprgbw/hprgbw_set.py b/examples/color_led_deck/color_led_set.py similarity index 87% rename from examples/hprgbw/hprgbw_set.py rename to examples/color_led_deck/color_led_set.py index 8fe0c4dfd..3cd52f65a 100644 --- a/examples/hprgbw/hprgbw_set.py +++ b/examples/color_led_deck/color_led_set.py @@ -40,15 +40,15 @@ def pack_rgbw(input: rgbw | rgb) -> int: # Thermal status callback def thermal_status_callback(timestamp, data, logconf): - throttle_pct = data['hprgbw.throttlePct'] + throttle_pct = data['colorled.throttlePct'] if throttle_pct > 0: - temp = data['hprgbw.deckTemp'] + temp = data['colorled.deckTemp'] print(f"WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%") # Setup log configuration for thermal monitoring log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) - log_conf.add_variable('hprgbw.deckTemp', 'uint8_t') - log_conf.add_variable('hprgbw.throttlePct', 'uint8_t') + log_conf.add_variable('colorled.deckTemp', 'uint8_t') + log_conf.add_variable('colorled.throttlePct', 'uint8_t') cf.log.add_config(log_conf) log_conf.data_received_cb.add_callback(thermal_status_callback) @@ -57,7 +57,7 @@ def thermal_status_callback(timestamp, data, logconf): # Brightness correction: balances luminance across R/G/B/W channels # Set to 1 (enabled, default) for perceptually uniform colors # Set to 0 (disabled) for maximum brightness per channel - cf.param.set_value('hprgbw.brightnessCorr', 1) + cf.param.set_value('colorled.brightnessCorr', 1) time.sleep(0.1) try: @@ -76,7 +76,7 @@ def thermal_status_callback(timestamp, data, logconf): # ======================================== color_uint32 = pack_rgbw(color) - cf.param.set_value('hprgbw.rgbw8888', color_uint32) + cf.param.set_value('colorled.rgbw8888', color_uint32) time.sleep(0.01) print(f"Setting LED to R={color.r}, G={color.g}, B={color.b}, W={color.w}") print('Press Ctrl-C to turn off LED and exit.') @@ -84,5 +84,5 @@ def thermal_status_callback(timestamp, data, logconf): time.sleep(0.1) except KeyboardInterrupt: print('\nStopping and turning off LED...') - cf.param.set_value('hprgbw.rgbw8888', 0) + cf.param.set_value('colorled.rgbw8888', 0) time.sleep(0.1) From e158ea591f79861c41c990d1aeffcb503c9b2ba4 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 22 Oct 2025 14:51:35 +0200 Subject: [PATCH 849/861] Fix string formatting for thermal throttling warnings in LED deck examples Interestingly this was not caught by pre-commit on Python 3.13 --- examples/color_led_deck/color_led_cycle.py | 2 +- examples/color_led_deck/color_led_set.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/color_led_deck/color_led_cycle.py b/examples/color_led_deck/color_led_cycle.py index ce9c268ac..2bf0056c5 100644 --- a/examples/color_led_deck/color_led_cycle.py +++ b/examples/color_led_deck/color_led_cycle.py @@ -77,7 +77,7 @@ def thermal_status_callback(timestamp, data, logconf): throttle_pct = data['hprgbw.throttlePct'] if throttle_pct > 0: temp = data['hprgbw.deckTemp'] - print(f"WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%") + print(f'WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%') # Setup log configuration for thermal monitoring log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) diff --git a/examples/color_led_deck/color_led_set.py b/examples/color_led_deck/color_led_set.py index 3cd52f65a..fafb1a2ba 100644 --- a/examples/color_led_deck/color_led_set.py +++ b/examples/color_led_deck/color_led_set.py @@ -43,7 +43,7 @@ def thermal_status_callback(timestamp, data, logconf): throttle_pct = data['colorled.throttlePct'] if throttle_pct > 0: temp = data['colorled.deckTemp'] - print(f"WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%") + print(f'WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%') # Setup log configuration for thermal monitoring log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) @@ -78,7 +78,7 @@ def thermal_status_callback(timestamp, data, logconf): color_uint32 = pack_rgbw(color) cf.param.set_value('colorled.rgbw8888', color_uint32) time.sleep(0.01) - print(f"Setting LED to R={color.r}, G={color.g}, B={color.b}, W={color.w}") + print(f'Setting LED to R={color.r}, G={color.g}, B={color.b}, W={color.w}') print('Press Ctrl-C to turn off LED and exit.') while True: time.sleep(0.1) From d3f50ea2b6a5e4a5e92579d6ddc0cd9f76d923cd Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 22 Oct 2025 15:52:37 +0200 Subject: [PATCH 850/861] Fix color cycle example Use correct updated parameter name Correctly pack color --- examples/color_led_deck/color_led_cycle.py | 23 +++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/examples/color_led_deck/color_led_cycle.py b/examples/color_led_deck/color_led_cycle.py index 2bf0056c5..a7685f9a0 100644 --- a/examples/color_led_deck/color_led_cycle.py +++ b/examples/color_led_deck/color_led_cycle.py @@ -61,8 +61,9 @@ def cycle_colors(step: int) -> rgbw: return hsv_to_rgbw(hsv(h, s, v)) -def rgbw_to_rgbwuint32(input: rgbw) -> int: - return (input.r << 24) | (input.g << 16) | (input.b << 8) | input.w +def pack_rgbw(input: rgbw) -> int: + """Pack RGBW values into uint32 format: 0xWWRRGGBB""" + return (input.w << 24) | (input.r << 16) | (input.g << 8) | input.b if __name__ == '__main__': @@ -74,15 +75,15 @@ def rgbw_to_rgbwuint32(input: rgbw) -> int: # Thermal status callback def thermal_status_callback(timestamp, data, logconf): - throttle_pct = data['hprgbw.throttlePct'] + throttle_pct = data['colorled.throttlePct'] if throttle_pct > 0: - temp = data['hprgbw.deckTemp'] + temp = data['colorled.deckTemp'] print(f'WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%') # Setup log configuration for thermal monitoring log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) - log_conf.add_variable('hprgbw.deckTemp', 'uint8_t') - log_conf.add_variable('hprgbw.throttlePct', 'uint8_t') + log_conf.add_variable('colorled.deckTemp', 'uint8_t') + log_conf.add_variable('colorled.throttlePct', 'uint8_t') cf.log.add_config(log_conf) log_conf.data_received_cb.add_callback(thermal_status_callback) @@ -91,19 +92,19 @@ def thermal_status_callback(timestamp, data, logconf): # Brightness correction: balances luminance across R/G/B/W channels # Set to 1 (enabled, default) for perceptually uniform colors # Set to 0 (disabled) for maximum brightness per channel - cf.param.set_value('hprgbw.brightnessCorr', 1) + cf.param.set_value('colorled.brightnessCorr', 1) time.sleep(0.1) try: print('Cycling through colors. Press Ctrl-C to stop.') while True: for i in range(0, 360, 1): - color = cycle_colors(i) + color: rgbw = cycle_colors(i) # print(color.r, color.g, color.b) - color_uint32 = rgbw_to_rgbwuint32(color) - cf.param.set_value('hprgbw.rgbw8888', str(color_uint32)) + color_uint32 = pack_rgbw(color) + cf.param.set_value('colorled.rgbw8888', str(color_uint32)) time.sleep(0.01) except KeyboardInterrupt: print('\nStopping and turning off LED...') - cf.param.set_value('hprgbw.rgbw8888', '0') + cf.param.set_value('colorled.rgbw8888', '0') time.sleep(0.1) From 30bfe066bc8e77ace4fdc915955a4975fc52dc73 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 23 Oct 2025 15:19:19 +0200 Subject: [PATCH 851/861] Add license information and copyright notice to LED deck example scripts --- examples/color_led_deck/color_led_cycle.py | 21 +++++++++++++++++++++ examples/color_led_deck/color_led_set.py | 21 +++++++++++++++++++++ examples/generic_led_deck/led_cycle.py | 21 +++++++++++++++++++++ 3 files changed, 63 insertions(+) diff --git a/examples/color_led_deck/color_led_cycle.py b/examples/color_led_deck/color_led_cycle.py index a7685f9a0..bab558fea 100644 --- a/examples/color_led_deck/color_led_cycle.py +++ b/examples/color_led_deck/color_led_cycle.py @@ -1,3 +1,24 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2025 Bitcraze AB +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, in version 3. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import cflib.crtp diff --git a/examples/color_led_deck/color_led_set.py b/examples/color_led_deck/color_led_set.py index fafb1a2ba..aecb21b8e 100644 --- a/examples/color_led_deck/color_led_set.py +++ b/examples/color_led_deck/color_led_set.py @@ -1,3 +1,24 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2025 Bitcraze AB +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, in version 3. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import cflib.crtp diff --git a/examples/generic_led_deck/led_cycle.py b/examples/generic_led_deck/led_cycle.py index 4779ed7f0..b704f9477 100644 --- a/examples/generic_led_deck/led_cycle.py +++ b/examples/generic_led_deck/led_cycle.py @@ -1,3 +1,24 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2025 Bitcraze AB +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, in version 3. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . import time import cflib.crtp From 4f2a4ed1ba77f175b140aea6be00c8c70e6d9730 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 3 Nov 2025 16:26:15 +0100 Subject: [PATCH 852/861] Refactor LED deck examples to use WRGB ordering to match packet format Changed the color type from rgbw to wrgb and reordered parameters to (w, r, g, b) to better reflect the actual data packet structure (0xWWRRGGBB). --- examples/color_led_deck/color_led_cycle.py | 28 +++++++++++----------- examples/color_led_deck/color_led_set.py | 28 +++++++++++----------- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/examples/color_led_deck/color_led_cycle.py b/examples/color_led_deck/color_led_cycle.py index bab558fea..5b37bc07b 100644 --- a/examples/color_led_deck/color_led_cycle.py +++ b/examples/color_led_deck/color_led_cycle.py @@ -31,12 +31,12 @@ URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -class rgbw: - def __init__(self, r: int, g: int, b: int, w: int = 0): +class wrgb: + def __init__(self, w: int, r: int, g: int, b: int): + self.w = w self.r = r self.g = g self.b = b - self.w = w class hsv: @@ -46,7 +46,7 @@ def __init__(self, h: int, s: int, v: int): self.v = v -def hsv_to_rgbw(input: hsv) -> rgbw: +def hsv_to_wrgb(input: hsv) -> wrgb: h, s, v = input.h, input.s / 255.0, input.v / 255.0 if s == 0: @@ -72,18 +72,18 @@ def hsv_to_rgbw(input: hsv) -> rgbw: else: r, g, b = v, p, q - return rgbw(int(r * 255), int(g * 255), int(b * 255), 0) + return wrgb(0, int(r * 255), int(g * 255), int(b * 255)) -def cycle_colors(step: int) -> rgbw: +def cycle_colors(step: int) -> wrgb: h = step % 360 s = 255 v = 255 - return hsv_to_rgbw(hsv(h, s, v)) + return hsv_to_wrgb(hsv(h, s, v)) -def pack_rgbw(input: rgbw) -> int: - """Pack RGBW values into uint32 format: 0xWWRRGGBB""" +def pack_wrgb(input: wrgb) -> int: + """Pack WRGB values into uint32 format: 0xWWRRGGBB""" return (input.w << 24) | (input.r << 16) | (input.g << 8) | input.b @@ -110,7 +110,7 @@ def thermal_status_callback(timestamp, data, logconf): log_conf.data_received_cb.add_callback(thermal_status_callback) log_conf.start() - # Brightness correction: balances luminance across R/G/B/W channels + # Brightness correction: balances luminance across W/R/G/B channels # Set to 1 (enabled, default) for perceptually uniform colors # Set to 0 (disabled) for maximum brightness per channel cf.param.set_value('colorled.brightnessCorr', 1) @@ -120,12 +120,12 @@ def thermal_status_callback(timestamp, data, logconf): print('Cycling through colors. Press Ctrl-C to stop.') while True: for i in range(0, 360, 1): - color: rgbw = cycle_colors(i) + color: wrgb = cycle_colors(i) # print(color.r, color.g, color.b) - color_uint32 = pack_rgbw(color) - cf.param.set_value('colorled.rgbw8888', str(color_uint32)) + color_uint32 = pack_wrgb(color) + cf.param.set_value('colorled.wrgb8888', str(color_uint32)) time.sleep(0.01) except KeyboardInterrupt: print('\nStopping and turning off LED...') - cf.param.set_value('colorled.rgbw8888', '0') + cf.param.set_value('colorled.wrgb8888', '0') time.sleep(0.1) diff --git a/examples/color_led_deck/color_led_set.py b/examples/color_led_deck/color_led_set.py index aecb21b8e..fae4bef75 100644 --- a/examples/color_led_deck/color_led_set.py +++ b/examples/color_led_deck/color_led_set.py @@ -31,12 +31,12 @@ URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -class rgbw: - def __init__(self, r: int, g: int, b: int, w: int = 0): +class wrgb: + def __init__(self, w: int, r: int, g: int, b: int): + self.w = w self.r = r self.g = g self.b = b - self.w = w class rgb: @@ -47,8 +47,8 @@ def __init__(self, r: int, g: int, b: int): self.b = b - self.w -def pack_rgbw(input: rgbw | rgb) -> int: - """Pack RGBW values into uint32 format: 0xWWRRGGBB""" +def pack_wrgb(input: wrgb | rgb) -> int: + """Pack WRGB values into uint32 format: 0xWWRRGGBB""" return (input.w << 24) | (input.r << 16) | (input.g << 8) | input.b @@ -86,18 +86,18 @@ def thermal_status_callback(timestamp, data, logconf): # Set your desired color here: # ======================================== # Examples: - # color = rgbw(r=255, g=0, b=0, w=0) # Pure red - # color = rgbw(r=0, g=255, b=0, w=0) # Pure green - # color = rgbw(r=0, g=0, b=255, w=0) # Pure blue - # color = rgbw(r=0, g=0, b=0, w=255) # Pure white LED - # color = rgbw(r=255, g=128, b=0, w=50) # Orange + white + # color = wrgb(w=0, r=255, g=0, b=0) # Pure red + # color = wrgb(w=0, r=0, g=255, b=0) # Pure green + # color = wrgb(w=0, r=0, g=0, b=255) # Pure blue + # color = wrgb(w=255, r=0, g=0, b=0) # Pure white LED + # color = wrgb(w=50, r=255, g=128, b=0) # Orange + white # color = rgb(r=255, g=255, b=255) # White (auto W extraction) - color = rgbw(r=255, g=0, b=100, w=0) + color = wrgb(w=0, r=255, g=0, b=100) # ======================================== - color_uint32 = pack_rgbw(color) - cf.param.set_value('colorled.rgbw8888', color_uint32) + color_uint32 = pack_wrgb(color) + cf.param.set_value('colorled.wrgb8888', color_uint32) time.sleep(0.01) print(f'Setting LED to R={color.r}, G={color.g}, B={color.b}, W={color.w}') print('Press Ctrl-C to turn off LED and exit.') @@ -105,5 +105,5 @@ def thermal_status_callback(timestamp, data, logconf): time.sleep(0.1) except KeyboardInterrupt: print('\nStopping and turning off LED...') - cf.param.set_value('colorled.rgbw8888', 0) + cf.param.set_value('colorled.wrgb8888', 0) time.sleep(0.1) From 017633ba601420a0f006511872bb13a20384d2b8 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 5 Nov 2025 16:04:51 +0100 Subject: [PATCH 853/861] Add auto-detection for color LED deck variants in examples Color LED examples now detect which deck variant is present (bottom or top-facing) and use the first one found. This keeps examples simple while supporting both colorLedBot and colorLedTop parameter groups. --- examples/color_led_deck/color_led_cycle.py | 37 ++++++++++++++++++---- examples/color_led_deck/color_led_set.py | 37 ++++++++++++++++++---- 2 files changed, 60 insertions(+), 14 deletions(-) diff --git a/examples/color_led_deck/color_led_cycle.py b/examples/color_led_deck/color_led_cycle.py index 5b37bc07b..97d7a1c97 100644 --- a/examples/color_led_deck/color_led_cycle.py +++ b/examples/color_led_deck/color_led_cycle.py @@ -94,17 +94,40 @@ def pack_wrgb(input: wrgb) -> int: with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf + # Detect which color LED deck is present (bottom or top) + # We check for whichever deck is attached and use the first one found + # Check for bottom-facing deck first + if str(cf.param.get_value('deck.bcColorLedBot')) == '1': + deck_params = { + 'color': 'colorLedBot.wrgb8888', + 'brightness': 'colorLedBot.brightCorr', + 'throttle': 'colorLedBot.throttlePct', + 'temp': 'colorLedBot.deckTemp' + } + print('Detected bottom-facing color LED deck') + # Check for top-facing deck + elif str(cf.param.get_value('deck.bcColorLedTop')) == '1': + deck_params = { + 'color': 'colorLedTop.wrgb8888', + 'brightness': 'colorLedTop.brightCorr', + 'throttle': 'colorLedTop.throttlePct', + 'temp': 'colorLedTop.deckTemp' + } + print('Detected top-facing color LED deck') + else: + raise RuntimeError('No color LED deck detected!') + # Thermal status callback def thermal_status_callback(timestamp, data, logconf): - throttle_pct = data['colorled.throttlePct'] + throttle_pct = data[deck_params['throttle']] if throttle_pct > 0: - temp = data['colorled.deckTemp'] + temp = data[deck_params['temp']] print(f'WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%') # Setup log configuration for thermal monitoring log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) - log_conf.add_variable('colorled.deckTemp', 'uint8_t') - log_conf.add_variable('colorled.throttlePct', 'uint8_t') + log_conf.add_variable(deck_params['temp'], 'uint8_t') + log_conf.add_variable(deck_params['throttle'], 'uint8_t') cf.log.add_config(log_conf) log_conf.data_received_cb.add_callback(thermal_status_callback) @@ -113,7 +136,7 @@ def thermal_status_callback(timestamp, data, logconf): # Brightness correction: balances luminance across W/R/G/B channels # Set to 1 (enabled, default) for perceptually uniform colors # Set to 0 (disabled) for maximum brightness per channel - cf.param.set_value('colorled.brightnessCorr', 1) + cf.param.set_value(deck_params['brightness'], 1) time.sleep(0.1) try: @@ -123,9 +146,9 @@ def thermal_status_callback(timestamp, data, logconf): color: wrgb = cycle_colors(i) # print(color.r, color.g, color.b) color_uint32 = pack_wrgb(color) - cf.param.set_value('colorled.wrgb8888', str(color_uint32)) + cf.param.set_value(deck_params['color'], str(color_uint32)) time.sleep(0.01) except KeyboardInterrupt: print('\nStopping and turning off LED...') - cf.param.set_value('colorled.wrgb8888', '0') + cf.param.set_value(deck_params['color'], '0') time.sleep(0.1) diff --git a/examples/color_led_deck/color_led_set.py b/examples/color_led_deck/color_led_set.py index fae4bef75..d91757ebb 100644 --- a/examples/color_led_deck/color_led_set.py +++ b/examples/color_led_deck/color_led_set.py @@ -59,17 +59,40 @@ def pack_wrgb(input: wrgb | rgb) -> int: with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf + # Detect which color LED deck is present (bottom or top) + # We check for whichever deck is attached and use the first one found + # Check for bottom-facing deck first + if str(cf.param.get_value('deck.bcColorLedBot')) == '1': + deck_params = { + 'color': 'colorLedBot.wrgb8888', + 'brightness': 'colorLedBot.brightCorr', + 'throttle': 'colorLedBot.throttlePct', + 'temp': 'colorLedBot.deckTemp' + } + print('Detected bottom-facing color LED deck') + # Check for top-facing deck + elif str(cf.param.get_value('deck.bcColorLedTop')) == '1': + deck_params = { + 'color': 'colorLedTop.wrgb8888', + 'brightness': 'colorLedTop.brightCorr', + 'throttle': 'colorLedTop.throttlePct', + 'temp': 'colorLedTop.deckTemp' + } + print('Detected top-facing color LED deck') + else: + raise RuntimeError('No color LED deck detected!') + # Thermal status callback def thermal_status_callback(timestamp, data, logconf): - throttle_pct = data['colorled.throttlePct'] + throttle_pct = data[deck_params['throttle']] if throttle_pct > 0: - temp = data['colorled.deckTemp'] + temp = data[deck_params['temp']] print(f'WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%') # Setup log configuration for thermal monitoring log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) - log_conf.add_variable('colorled.deckTemp', 'uint8_t') - log_conf.add_variable('colorled.throttlePct', 'uint8_t') + log_conf.add_variable(deck_params['temp'], 'uint8_t') + log_conf.add_variable(deck_params['throttle'], 'uint8_t') cf.log.add_config(log_conf) log_conf.data_received_cb.add_callback(thermal_status_callback) @@ -78,7 +101,7 @@ def thermal_status_callback(timestamp, data, logconf): # Brightness correction: balances luminance across R/G/B/W channels # Set to 1 (enabled, default) for perceptually uniform colors # Set to 0 (disabled) for maximum brightness per channel - cf.param.set_value('colorled.brightnessCorr', 1) + cf.param.set_value(deck_params['brightness'], 1) time.sleep(0.1) try: @@ -97,7 +120,7 @@ def thermal_status_callback(timestamp, data, logconf): # ======================================== color_uint32 = pack_wrgb(color) - cf.param.set_value('colorled.wrgb8888', color_uint32) + cf.param.set_value(deck_params['color'], color_uint32) time.sleep(0.01) print(f'Setting LED to R={color.r}, G={color.g}, B={color.b}, W={color.w}') print('Press Ctrl-C to turn off LED and exit.') @@ -105,5 +128,5 @@ def thermal_status_callback(timestamp, data, logconf): time.sleep(0.1) except KeyboardInterrupt: print('\nStopping and turning off LED...') - cf.param.set_value('colorled.wrgb8888', 0) + cf.param.set_value(deck_params['color'], 0) time.sleep(0.1) From 0a2eb31f3faebbca4a64b31a696286afadeb7fe2 Mon Sep 17 00:00:00 2001 From: Marcus Date: Thu, 13 Nov 2025 10:47:45 +0100 Subject: [PATCH 854/861] Fixed memory read/write size --- cflib/crazyflie/mem/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index c9874d531..b195c3691 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -77,7 +77,7 @@ class _ReadRequest: Class used to handle memory reads that will split up the read in multiple packets if necessary """ - MAX_DATA_LENGTH = 20 + MAX_DATA_LENGTH = 24 def __init__(self, mem, addr, length, cf): """Initialize the object with good defaults""" @@ -141,7 +141,7 @@ class _WriteRequest: Class used to handle memory reads that will split up the read in multiple packets in necessary """ - MAX_DATA_LENGTH = 25 + MAX_DATA_LENGTH = 24 def __init__(self, mem, addr, data, cf, progress_cb=None): """Initialize the object with good defaults""" From 589761181405dd6e901ae2b2e0e1dddff58bb599 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Mon, 17 Nov 2025 16:04:22 +0100 Subject: [PATCH 855/861] Enhance target handling in Bootloader to support multiple targets --- cflib/bootloader/__init__.py | 30 +++++++++++++++++++++--------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index ff8b9ba36..a817b1915 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -398,15 +398,27 @@ def _get_flash_artifacts_from_zip(self, filename): # Handle version 1 of manifest where prerequisites for nRF soft-devices are not specified requires = [] if 'requires' not in metadata else metadata['requires'] provides = [] if 'provides' not in metadata else metadata['provides'] - if len(requires) == 0 and metadata['target'] == 'nrf51' and metadata['type'] == 'fw': - requires.append('sd-s110') - # If there is no requires for the nRF51 fw target then we also need the legacy s110 - # so add this to the file list afterwards - add_legacy_nRF51_s110 = True - - target = Target(metadata['platform'], metadata['target'], metadata['type'], - provides, requires) - flash_artifacts.append(FlashArtifact(content, target, metadata['release'])) + + # Support both single target (string) and multiple targets (list) + target_value = metadata['target'] + target_list = target_value if isinstance(target_value, list) else [target_value] + + for target_name in target_list: + logger.debug('Processing target: {}'.format(target_name)) + # Create copies for each target to avoid shared mutable state + target_requires = requires.copy() + target_provides = provides.copy() + + # Check for legacy nRF51 requirement + if len(target_requires) == 0 and target_name == 'nrf51' and metadata['type'] == 'fw': + target_requires.append('sd-s110') + # If there is no requires for the nRF51 fw target then we also need the legacy s110 + # so add this to the file list afterwards + add_legacy_nRF51_s110 = True + + target = Target(metadata['platform'], target_name, metadata['type'], + target_provides, target_requires) + flash_artifacts.append(FlashArtifact(content, target, metadata['release'])) if add_legacy_nRF51_s110: print('Legacy format detected for manifest, adding s110+bl binary from distro') From b39356ad10deb981ff90e2bca4b18705a30b5ec4 Mon Sep 17 00:00:00 2001 From: aris Date: Mon, 1 Dec 2025 16:32:38 +0100 Subject: [PATCH 856/861] Added the christmas tree script --- examples/swarm/christmas_tree.py | 185 +++++++++++++++++++++++++++++++ 1 file changed, 185 insertions(+) create mode 100644 examples/swarm/christmas_tree.py diff --git a/examples/swarm/christmas_tree.py b/examples/swarm/christmas_tree.py new file mode 100644 index 000000000..7404defcd --- /dev/null +++ b/examples/swarm/christmas_tree.py @@ -0,0 +1,185 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2025 Bitcraze AB +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +""" +Script for flying a swarm of 8 Crazyflies performing a coordinated spiral choreography +resembling a Christmas tree outline in 3D space. Each drone takes off to a different +height, flies in spiraling circular layers, and changes radius as it rises and descends, +forming the visual structure of a cone when viewed from outside. + +The script is using the high level commanded and has been tested with 3 Crazyradios 2.0 +and the Lighthouse positioning system. +""" +import math +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + +uri1 = 'radio://0/30/2M/E7E7E7E701' +uri2 = 'radio://0/30/2M/E7E7E7E702' +uri3 = 'radio://0/30/2M/E7E7E7E703' + +uri4 = 'radio://1/55/2M/E7E7E7E704' +uri5 = 'radio://1/55/2M/E7E7E7E705' +uri6 = 'radio://1/55/2M/E7E7E7E706' + +uri7 = 'radio://2/70/2M/E7E7E7E707' +uri8 = 'radio://2/70/2M/E7E7E7E708' + +uris = [ + uri1, + uri2, + uri3, + uri4, + uri5, + uri6, + uri7, + uri8, + # Add more URIs if you want more copters in the swarm +] + + +def arm(scf: SyncCrazyflie): + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + + +# center of the spiral +x0 = 0 +y0 = 0 +z0 = 0.5 + +x_offset = 0.4 # Vertical distance between 2 layers +z_offset = 0.5 # Radius difference between 2 layers + +starting_x = { + uri1: x0 + x_offset, + uri2: x0 + 2*x_offset, + uri3: x0 + 3*x_offset, + uri4: x0 + 4*x_offset, + uri5: x0 - x_offset, + uri6: x0 - 2*x_offset, + uri7: x0 - 3*x_offset, + uri8: x0 - 4*x_offset, +} + +starting_z = { + uri1: z0 + 3*z_offset, + uri2: z0 + 2*z_offset, + uri3: z0 + z_offset, + uri4: z0, + uri5: z0 + 3*z_offset, + uri6: z0 + 2*z_offset, + uri7: z0 + z_offset, + uri8: z0, +} + +starting_yaw = { + uri1: math.pi/2, + uri2: -math.pi/2, + uri3: math.pi/2, + uri4: -math.pi/2, + uri5: -math.pi/2, + uri6: math.pi/2, + uri7: -math.pi/2, + uri8: math.pi/2, + +} + +rotate_clockwise = { + uri1: False, + uri2: True, + uri3: False, + uri4: True, + uri5: False, + uri6: True, + uri7: False, + uri8: True, +} + +takeoff_dur = { + uri: value / 0.4 + for uri, value in starting_z.items() +} + + +def x_from_z(z): + cone_width = 4 # m + cone_height = 5 # m + """ + Returns the radius of the tree with a given z. + """ + return cone_width/2 - (cone_width/cone_height) * z + + +def run_shared_sequence(scf: SyncCrazyflie): + circle_duration = 5 # Duration of a full-circle + commander = scf.cf.high_level_commander + uri = scf._link_uri + + commander.takeoff(starting_z[uri], takeoff_dur[uri]) + time.sleep(max(takeoff_dur.values())+1) + + # Go to the starting position + commander.go_to(starting_x[uri], y0, starting_z[uri], starting_yaw[uri], 4) + time.sleep(5) + + # Full circle with ascent=0 + commander.spiral(2*math.pi, abs(starting_x[uri]), abs(starting_x[uri]), + ascent=0, duration_s=circle_duration, sideways=False, clockwise=rotate_clockwise[uri]) + time.sleep(circle_duration+1) + + # Half circle with ascent=-0.5m + commander.spiral(math.pi, abs(starting_x[uri]), x_from_z(starting_z[uri]-0.5*z_offset), + ascent=-0.5*z_offset, duration_s=0.5*circle_duration, sideways=False, + clockwise=rotate_clockwise[uri]) + time.sleep(0.5*circle_duration+0.5) + + # Full circle with ascent=+1.0m + commander.spiral(2*math.pi, x_from_z(starting_z[uri]-0.5*z_offset), x_from_z(starting_z[uri]+0.5*z_offset), + ascent=z_offset, duration_s=circle_duration, sideways=False, + clockwise=rotate_clockwise[uri]) + time.sleep(circle_duration+1) + + # Half circle with ascent=-0.5m + commander.spiral(math.pi, x_from_z(starting_z[uri]+0.5*z_offset), x_from_z(starting_z[uri]), + ascent=-0.5*z_offset, duration_s=0.5*circle_duration, sideways=False, + clockwise=rotate_clockwise[uri]) + time.sleep(0.5*circle_duration+1) + + commander.land(0, takeoff_dur[uri]) + time.sleep(max(takeoff_dur.values())+3) + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + # swarm.reset_estimators() + time.sleep(0.5) + print('arming...') + swarm.parallel_safe(arm) + print('starting sequence...') + swarm.parallel_safe(run_shared_sequence) + time.sleep(1) From 8d8a6d86589fde3ee9a370c4581fcc167e24336b Mon Sep 17 00:00:00 2001 From: Rik <49898887+gemenerik@users.noreply.github.com> Date: Tue, 16 Dec 2025 10:32:15 +0100 Subject: [PATCH 857/861] Revise README to enhance clarity of cflib Updated description to clarify library capabilities and supported models. --- README.md | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 6165ad52e..a55775933 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,10 @@ -# cflib: Crazyflie python library [![CI](https://github.com/bitcraze/crazyflie-lib-python/workflows/CI/badge.svg)](https://github.com/bitcraze/crazyflie-lib-python/actions) +# cflib: Crazyflie Python library [![CI](https://github.com/bitcraze/crazyflie-lib-python/workflows/CI/badge.svg)](https://github.com/bitcraze/crazyflie-lib-python/actions) -cflib is an API written in Python that is used to communicate with the Crazyflie -and Crazyflie 2.0 quadcopters. It is intended to be used by client software to -communicate with and control a Crazyflie quadcopter. For instance the [Crazyflie PC client](https://www.github.com/bitcraze/crazyflie-clients-python) uses the cflib. +`cflib` is a Python library for communicating with and controlling Crazyflie quadcopters, +including the Crazyflie 2.1(+), Crazyflie 2.1 Brushless, and +Crazyflie Bolt. This library provides a Python API for direct programmatic control +through Python scripts, and is also used as the backend communication layer for +applications like [`cfclient`](https://www.github.com/bitcraze/crazyflie-clients-python). See [below](#platform-notes) for platform specific instruction. For more info see our [documentation](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/). From c1abf3a3b19965a3c7d07dcefc227dd9769b0ea6 Mon Sep 17 00:00:00 2001 From: aris Date: Wed, 17 Dec 2025 13:02:49 +0100 Subject: [PATCH 858/861] Increased the delay after flashing a deck by 3 seconds --- cflib/bootloader/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index a817b1915..146fed9ce 100644 --- a/cflib/bootloader/__init__.py +++ b/cflib/bootloader/__init__.py @@ -326,7 +326,7 @@ def flash(self, filename: str, targets: List[Target], cf=None, enable_console_lo self.progress_cb('Deck updated! Restarting...', int(100)) if current_index != -1: PowerSwitch(self.clink).reboot_to_fw() - time.sleep(2.0 + boot_delay) + time.sleep(5.0 + boot_delay) # Put the crazyflie back in Bootloader mode to exit the function in the same state we entered it self.start_bootloader(warm_boot=True, cf=cf) From 8634588ac3b8a8a544563ad7dc7e1add3f92df58 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 20 Jan 2026 16:26:44 +0100 Subject: [PATCH 859/861] Fix quaternion averaging Use np.linalg.eigh instead of np.linalg.eig for the symmetric matrix Q.T @ Q. eigh is designed for symmetric matrices and guarantees real eigenvalues/eigenvectors, whereas eig can return complex values due to numerical precision. This fixes a dtype mismatch error in scipy's Rotation.from_quat which now strictly validates input types. --- cflib/localization/lighthouse_initial_estimator.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 722e66d1e..70778b1a9 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -373,9 +373,9 @@ def _avarage_poses(cls, poses: list[Pose]) -> Pose: """ def q_average(Q, W=None): if W is not None: - Q *= W[:, None] - eigvals, eigvecs = np.linalg.eig(Q.T@Q) - return eigvecs[:, eigvals.argmax()] + Q = Q * W[:, None] + _, eigvecs = np.linalg.eigh(Q.T @ Q) + return eigvecs[:, -1] positions = map(lambda x: x.translation, poses) average_pos = np.average(np.array(list(positions)), axis=0) From fbdd14a822ccd5d50d538c3c6dc4c414b5a375ed Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 21 Jan 2026 12:06:03 +0100 Subject: [PATCH 860/861] Add DeckCtrlElement class and integrate with Memory management --- cflib/crazyflie/mem/__init__.py | 5 + cflib/crazyflie/mem/deckctrl_element.py | 141 ++++++++++++++++++++++++ cflib/crazyflie/mem/memory_element.py | 6 + 3 files changed, 152 insertions(+) create mode 100644 cflib/crazyflie/mem/deckctrl_element.py diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index b195c3691..3a7f5ae4b 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -32,6 +32,7 @@ from threading import Lock from .deck_memory import DeckMemoryManager +from .deckctrl_element import DeckCtrlElement from .i2c_element import I2CElement from .led_driver_memory import LEDDriverMemory from .led_timings_driver_memory import LEDTimingsDriverMemory @@ -533,6 +534,10 @@ def _handle_cmd_info_details(self, payload): logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_read_failed_cb.add_callback(mem.read_failed) + elif mem_type == MemoryElement.TYPE_DECKCTRL: + mem = DeckCtrlElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) + logger.debug(mem) + self.mem_read_cb.add_callback(mem.new_data) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) diff --git a/cflib/crazyflie/mem/deckctrl_element.py b/cflib/crazyflie/mem/deckctrl_element.py new file mode 100644 index 000000000..73dbfdb39 --- /dev/null +++ b/cflib/crazyflie/mem/deckctrl_element.py @@ -0,0 +1,141 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2026 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 logging +import struct + +from .memory_element import MemoryElement + +logger = logging.getLogger(__name__) + +# DeckCtrl memory layout at offset 0x0000 (32 bytes): +# Offset | Size | Field +# -------|------|---------------- +# 0x00 | 2 | Magic (0xBCDC big-endian) +# 0x02 | 1 | Major Version +# 0x03 | 1 | Minor Version +# 0x04 | 1 | Vendor ID +# 0x05 | 1 | Product ID +# 0x06 | 1 | Board Revision +# 0x07 | 15 | Product Name (null-terminated) +# 0x16 | 1 | Year +# 0x17 | 1 | Month +# 0x18 | 1 | Day +# 0x19 | 6 | Reserved +# 0x1F | 1 | Checksum (makes sum of bytes 0-31 = 0) + +DECKCTRL_MAGIC = 0xBCDC +DECKCTRL_INFO_SIZE = 32 + + +class DeckCtrlElement(MemoryElement): + """Memory class with functionality for DeckCtrl memories""" + + def __init__(self, id, type, size, mem_handler): + """Initialize the memory with good defaults""" + super(DeckCtrlElement, self).__init__(id=id, type=type, size=size, + mem_handler=mem_handler) + + self.valid = False + + self.vid = None + self.pid = None + self.name = None + self.revision = None + self.fw_version_major = None + self.fw_version_minor = None + self.elements = {} + + self._update_finished_cb = None + + def new_data(self, mem, addr, data): + """Callback for when new memory data has been fetched""" + if mem.id == self.id: + if addr == 0: + if self._parse_and_check_info(data[:DECKCTRL_INFO_SIZE]): + self.valid = True + if self._update_finished_cb: + self._update_finished_cb(self) + self._update_finished_cb = None + + def _parse_and_check_info(self, data): + """Parse and validate the DeckCtrl info block""" + if len(data) < DECKCTRL_INFO_SIZE: + logger.warning('DeckCtrl data too short: {} bytes'.format(len(data))) + return False + + # Validate checksum (sum of all 32 bytes should be 0 mod 256) + checksum = sum(data[:DECKCTRL_INFO_SIZE]) & 0xFF + if checksum != 0: + logger.warning('DeckCtrl checksum failed: {}'.format(checksum)) + return False + + # Parse the header + magic = struct.unpack('>H', data[0:2])[0] # Big-endian + if magic != DECKCTRL_MAGIC: + logger.warning('DeckCtrl magic mismatch: 0x{:04X}'.format(magic)) + return False + + self.fw_version_major = data[2] + self.fw_version_minor = data[3] + self.vid = data[4] + self.pid = data[5] + self.revision = chr(data[6]) if data[6] != 0 else '' + + # Product name is 15 bytes, null-terminated + name_bytes = data[7:22] + null_pos = name_bytes.find(0) + if null_pos >= 0: + name_bytes = name_bytes[:null_pos] + self.name = name_bytes.decode('ISO-8859-1') + + # Manufacturing date + year = data[22] + month = data[23] + day = data[24] + + # Populate elements dict for compatibility with OWElement interface + self.elements['Board name'] = self.name + self.elements['Board revision'] = self.revision + if year != 0 or month != 0 or day != 0: + self.elements['Manufacturing date'] = '{:04d}-{:02d}-{:02d}'.format( + 2000 + year, month, day) + self.elements['Firmware version'] = '{}.{}'.format( + self.fw_version_major, self.fw_version_minor) + + return True + + def update(self, update_finished_cb): + """Request an update of the memory content""" + if not self._update_finished_cb: + self._update_finished_cb = update_finished_cb + self.valid = False + logger.debug('Updating content of DeckCtrl memory {}'.format(self.id)) + # Read the 32-byte info block + self.mem_handler.read(self, 0, DECKCTRL_INFO_SIZE) + + def __str__(self): + """Generate debug string for memory""" + return ('DeckCtrl ({:02X}:{:02X}): {}'.format( + self.vid or 0, self.pid or 0, self.elements)) + + def disconnect(self): + self._update_finished_cb = None diff --git a/cflib/crazyflie/mem/memory_element.py b/cflib/crazyflie/mem/memory_element.py index 895b2a5d5..83b2a6304 100644 --- a/cflib/crazyflie/mem/memory_element.py +++ b/cflib/crazyflie/mem/memory_element.py @@ -40,6 +40,8 @@ class MemoryElement(object): TYPE_DECK_MEMORY = 0x19 TYPE_DECK_MULTIRANGER = 0x1A TYPE_DECK_PAA3905 = 0x1B + TYPE_DECKCTRL_DFU = 0x20 + TYPE_DECKCTRL = 0x21 def __init__(self, id, type, size, mem_handler): """Initialize the element with default values""" @@ -69,6 +71,10 @@ def type_to_string(t): return 'Lighthouse positioning' if t == MemoryElement.TYPE_MEMORY_TESTER: return 'Memory tester' + if t == MemoryElement.TYPE_DECKCTRL: + return 'DeckCtrl' + if t == MemoryElement.TYPE_DECKCTRL_DFU: + return 'DeckCtrl DFU' return 'Unknown' def new_data(self, mem, addr, data): From 3a00e17f04b19802ddd2f4d836f1020a35c381c8 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 21 Jan 2026 14:37:28 +0100 Subject: [PATCH 861/861] Add read_failed callback to DeckCtrlElement for memory read failures --- cflib/crazyflie/mem/__init__.py | 1 + cflib/crazyflie/mem/deckctrl_element.py | 8 ++++++++ 2 files changed, 9 insertions(+) diff --git a/cflib/crazyflie/mem/__init__.py b/cflib/crazyflie/mem/__init__.py index 3a7f5ae4b..e93d43417 100644 --- a/cflib/crazyflie/mem/__init__.py +++ b/cflib/crazyflie/mem/__init__.py @@ -538,6 +538,7 @@ def _handle_cmd_info_details(self, payload): mem = DeckCtrlElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) + self.mem_read_failed_cb.add_callback(mem.read_failed) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) diff --git a/cflib/crazyflie/mem/deckctrl_element.py b/cflib/crazyflie/mem/deckctrl_element.py index 73dbfdb39..7dfa7d718 100644 --- a/cflib/crazyflie/mem/deckctrl_element.py +++ b/cflib/crazyflie/mem/deckctrl_element.py @@ -76,6 +76,14 @@ def new_data(self, mem, addr, data): self._update_finished_cb(self) self._update_finished_cb = None + def read_failed(self, mem, addr): + """Callback for when a memory read fails""" + if mem.id == self.id: + logger.warning('DeckCtrl memory read failed for id {}'.format(self.id)) + if self._update_finished_cb: + self._update_finished_cb(self) + self._update_finished_cb = None + def _parse_and_check_info(self, data): """Parse and validate the DeckCtrl info block""" if len(data) < DECKCTRL_INFO_SIZE: