diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml new file mode 100644 index 000000000..5204c1992 --- /dev/null +++ b/.github/workflows/CI.yml @@ -0,0 +1,25 @@ +# Run check and build of the lib using the Bitcraze builder docker image +name: CI + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + schedule: + # Weekly build to make sure dependencies are OK + - cron: '30 16 * * 6' + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - name: Checkout repo + uses: actions/checkout@v4 + + - 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 diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml new file mode 100644 index 000000000..aaf91788e --- /dev/null +++ b/.github/workflows/nightly.yml @@ -0,0 +1,90 @@ +# 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: + fail-fast: false + 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 }} + + steps: + - name: Checkout repo + 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: 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 + python3 -m pip install pre-commit + + - name: Code quality checks + run: pre-commit run --all-files + + - name: Build wheel + run: python3 -m build --wheel + + - 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/ + + 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 setuptools + python3 -m pip install pdoc3 pyyaml + - name: Build docs + run: ./tools/build-docs/build-docs diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml new file mode 100644 index 000000000..0e197504e --- /dev/null +++ b/.github/workflows/python-publish.yml @@ -0,0 +1,44 @@ +# 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 + environment: + name: pypi + url: https://pypi.org/p/cflib + permissions: + id-token: write + + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + - name: Set up Python + uses: actions/setup-python@v4 + 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 + with: + verbose: true diff --git a/.github/workflows/test-python-publish.yml b/.github/workflows/test-python-publish.yml new file mode 100644 index 000000000..2b4851bd3 --- /dev/null +++ b/.github/workflows/test-python-publish.yml @@ -0,0 +1,45 @@ +# 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 + environment: + name: pypi-test + url: https://pypi.org/p/cflib + permissions: + id-token: write + + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + - name: Set up Python + uses: actions/setup-python@v4 + 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: + repository-url: https://test.pypi.org/legacy/ + verbose: true diff --git a/.gitignore b/.gitignore index 77c4211a1..708332873 100644 --- a/.gitignore +++ b/.gitignore @@ -15,3 +15,16 @@ venv* #jekyll generated pages docs/.jekyll-metadata +docs/api/cflib/* + +# Generated documentation +tools/build-docs/.local +tools/build-docs/.cache +docs/api/cflib +!docs/api/cflib/index.md + +#vscode config folder +.vscode/ + +# uv lock file - not committed for libraries +uv.lock diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 59c77fa95..92355ecdf 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,5 +1,6 @@ -- repo: git://github.com/pre-commit/pre-commit-hooks - sha: 7192665e31cea6ace58a71e086c7248d7e5610c2 +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: 7192665e31cea6ace58a71e086c7248d7e5610c2 hooks: - id: autopep8-wrapper - id: check-added-large-files @@ -13,9 +14,12 @@ - id: detect-private-key - id: double-quote-string-fixer - id: end-of-file-fixer - - id: flake8 - id: requirements-txt-fixer -- repo: git://github.com/asottile/reorder_python_imports - sha: ab609b9b982729dfc287b4e75963c0c4de254a31 +- repo: https://github.com/PyCQA/flake8 + rev: 16f5f28a384f0781bebb37a08aa45e65b9526c50 + hooks: + - id: flake8 +- repo: https://github.com/asottile/reorder_python_imports + rev: ab609b9b982729dfc287b4e75963c0c4de254a31 hooks: - id: reorder-python-imports 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 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 -``` 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/README.md b/README.md index 48e2db631..a55775933 100644 --- a/README.md +++ b/README.md @@ -1,109 +1,25 @@ -# 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 [![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 [cfclient][cfclient] Crazyflie PC client 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 [wiki](http://wiki.bitcraze.se/ "Bitcraze Wiki"). +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` - -To verify the installation, connect the crazyflie and run an example: `python examples/basiclog` - -## 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 environmet. - -### 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.) - - -## 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 +Go to the [contribute page](https://www.bitcraze.io/contribute/) on our website to learn more. + +### Test code for contribution +Run the automated build locally to test your code -Everyone is encouraged to contribute to the CrazyFlie library by forking the Github repository and making a pull request or opening an issue. + python3 tools/build/build diff --git a/cflib/__init__.py b/cflib/__init__.py index 516ffc088..adff0ca58 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. @@ -39,16 +37,27 @@ 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 know URI (radio dongle 0 and +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() +``` """ +import warnings + +warnings.simplefilter('always', DeprecationWarning) # Enbable DeprecationWarnings + +__pdoc__ = {} +__pdoc__['cflib.crtp.cflinkcppdriver'] = False +__pdoc__['cflib.cpx.transports'] = False diff --git a/cflib/bootloader/__init__.py b/cflib/bootloader/__init__.py index cde26e9cb..146fed9ce 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. """ @@ -32,16 +30,32 @@ import sys import time import zipfile +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 packaging.version import Version from .boottypes import BootVersion from .boottypes import TargetTypes from .cloader import Cloader +from cflib.crazyflie import Crazyflie +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__) __author__ = 'Bitcraze AB' __all__ = ['Bootloader'] +Target = namedtuple('Target', ['platform', 'target', 'type', 'provides', 'requires']) +FlashArtifact = namedtuple('FlashArtifact', ['content', 'target', 'release']) + class Bootloader: """Bootloader utility for the Crazyflie""" @@ -64,43 +78,43 @@ def __init__(self, clink=None): self.error_code = 0 self.protocol_version = 0 - # Outgoing callbacks for progress - # int - self.progress_cb = None - # msg - self.error_cb = None - # bool - self.in_bootloader_cb = None - # Target - self.dev_info_cb = None + self.warm_booted = False - # 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, cf=None): + self.warm_booted = warm_boot - def start_bootloader(self, warm_boot=False): 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 @@ -112,177 +126,368 @@ 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} + 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 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)) + + 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 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)) + + 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 is 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 _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(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): + # 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], None)] + 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'] + + # 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) + 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 = Version(str(self._cload.targets[TargetTypes.NRF51].version)) + 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( + 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 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. + 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 and not update_contains_nrf_sd: + 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] + # 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) + + 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') + self._cload.check_link_and_get_info() + self._cload.request_info_update(TargetTypes.NRF51) + else: + print('No need to flash nRF soft device') - self._internal_flash(target=to_flash) + # Remove the softdevice+bootloader from the list of artifacts to flash + flash_artifacts = [a for a in flash_artifacts if a.target.type != + 'bootloader+softdevice'] # Also filter for nRF51 here? - 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 + # Flash the MCU flash + if len(targets) == 0 or len(flash_targets) > 0: + self._flash_flash(flash_artifacts, flash_targets) - 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 + # Flash the decks + deck_update_msg = 'Deck update skipped.' + if len(targets) == 0 or len(deck_targets) > 0: + # only in warm boot + if self.warm_booted: + if self.progress_cb: + self.progress_cb('Restarting firmware to update decks.', int(0)) + + # Reset to firmware mode + self.reset_to_firmware(boot_delay=boot_delay) + self.close() + time.sleep(2) + + # 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, + 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() + time.sleep(5.0 + boot_delay) - 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 + # 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) - else: - if len(targets) != 1: - print('Not an archive, must supply one target to flash') + deck_update_msg = 'Deck update complete.' 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,) + print('Skipping updating deck on coldboot') + deck_update_msg = 'Deck update skipped in ColdBoot mode.' - if not self.progress_cb: + if self.progress_cb: + self.progress_cb( + f'({len(flash_artifacts)}/{len(flash_artifacts)}) Flashing done! {deck_update_msg}', + int(100)) + else: 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 - - def reset_to_firmware(self): + 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, + 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. + """ + # 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: + self.terminate_flashing_cb = terminate_flash_cb + + if not self.start_bootloader(warm_boot=warm, cf=cf): + 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, 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): + 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'] > 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) + + # 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'] + + # 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 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)) + + return flash_artifacts + + 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, boot_delay: float = 0.0) -> bool: + status = False if self._cload.protocol_version == BootVersion.CF2_PROTO_VER: - self._cload.reset_to_firmware(TargetTypes.NRF51) + status = self._cload.reset_to_firmware(TargetTypes.NRF51, boot_delay) else: - self._cload.reset_to_firmware(TargetTypes.STM32) + status = self._cload.reset_to_firmware(TargetTypes.STM32, boot_delay) + + 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, page_override=None): - def _internal_flash(self, target, 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 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) progress = 0 + type_of_binary = 'Firmware' + if artifact.target.type == 'bootloader+softdevice': + type_of_binary = 'Bootloader+softdevice' + if self.progress_cb: self.progress_cb( - '({}/{}) Starting...'.format(current_file_number, total_files), + '{} ({}/{}) Starting...'.format(type_of_binary, current_file_number, total_files), int(progress)) else: 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) * 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() + raise Exception('Not enough space to flash the image file') if not self.progress_cb: logger.info(('%d bytes (%d pages) ' % ( @@ -294,6 +499,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( @@ -307,7 +515,8 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): if self.progress_cb: progress += factor - self.progress_cb('({}/{}) 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)), @@ -320,7 +529,8 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): # 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('{} ({}/{}) Writing buffer to {}...'.format( + type_of_binary, current_file_number, total_files, TargetTypes.to_string(t_data.id)), @@ -334,7 +544,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: @@ -347,7 +557,8 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): if ctr > 0: if self.progress_cb: - self.progress_cb('({}/{}) 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)), @@ -361,7 +572,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: @@ -369,13 +580,8 @@ def _internal_flash(self, target, current_file_number=1, total_files=1): ' 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('') + sys.stdout.write('\n') + sys.stdout.flush() def _get_platform_id(self): """Get platform identifier used in the zip manifest for curr copter""" @@ -384,3 +590,129 @@ def _get_platform_id(self): identifier = 'cf2' 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_incrementally(self, artifacts: List[FlashArtifact], targets: List[Target], start_index: int, + 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) + + with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: + 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) + 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) + + # 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') + + # Skip decks up to the start_index + if deck_index < start_index: + continue + + # Check that we want to flash this deck + deck_target = [t for t in targets if t == Target('deck', deck.name, 'fw', [], [])] + if (not flash_all_targets) and len(deck_target) == 0: + print(f'Skipping {deck.name}, not in the target list') + continue + + # Check that we have an artifact for this deck + deck_artifacts = [a for a in artifacts if a.target == Target('deck', deck.name, 'fw', [], [])] + if len(deck_artifacts) == 0: + print(f'Skipping {deck.name}, no artifact for it in the .zip') + continue + deck_artifact = deck_artifacts[0] + + if self.progress_cb: + self.progress_cb(f'Updating deck {deck.name}', 0) + + # Test and wait for the deck to be started + timeout_time = time.time() + 4. + boot_delay + while not deck.is_started: + if time.time() > timeout_time: + raise RuntimeError(f'Deck {deck.name} did not start') + print('Deck not yet started ...') + time.sleep(0.5) + deck = mgr.query_decks()[deck_index] + + # Supports upgrades? + if not deck.supports_fw_upgrade: + print(f'Deck {deck.name} does not support firmware update, skipping!') + continue + + # Reset to bootloader mode, if supported + if deck.supports_reset_to_bootloader: + print(f'Deck {deck.name}, reset to bootloader') + deck.reset_to_bootloader() + + time.sleep(0.1) + deck = mgr.query_decks()[deck_index] + else: + # Is an upgrade required? + if not deck.is_fw_upgrade_required: + print(f'Deck {deck.name} firmware up to date, skipping') + continue + + # Wait for bootloader to be ready + timeout_time = time.time() + 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') + print(f'Error: Deck {deck.name} bootloader not active yet...') + time.sleep(0.5) + deck = mgr.query_decks()[deck_index] + + progress_cb = self.progress_cb + if not progress_cb: + def progress_cb(msg: str, percent: int): + frames = ['|', '/', '-', '\\'] + frame = frames[int(percent) % 4] + print(f'{frame} {percent}% {msg}') + + # Flash the new firmware + deck.set_fw_new_flash_size(len(deck_artifact.content)) + result = deck.write_sync(0, deck_artifact.content, progress_cb) + if result: + if self.progress_cb: + self.progress_cb(f'Deck {deck.name} updated successful!', 100) + else: + if self.progress_cb: + self.progress_cb(f'Failed to update deck {deck.name}', int(0)) + raise RuntimeError(f'Failed to update deck {deck.name}') + + if enable_console_log: + # Wait a bit to let the console log print + time.sleep(6) + + # We flashed a deck, return for re-boot + next_index = deck_index + 1 + return next_index + + # We have flashed the last deck + return -1 diff --git a/cflib/bootloader/boottypes.py b/cflib/bootloader/boottypes.py index 8a73f2543..e3cc975f1 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. """ @@ -81,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 @@ -88,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 60b178233..87fdd2353 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. """ @@ -37,7 +35,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'] @@ -72,7 +69,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 +77,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: @@ -92,153 +89,80 @@ 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(' 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.data[22] + self.protocol_version = answer.data[22] + if len(answer.data) > 23 and len(answer.data) > 26: + code_state = '' + if answer.data[24] & 0x80 != 0: + code_state = '+dev' + answer.data[24] &= 0x7F + 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] + 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 @@ -296,7 +234,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() pk.set_header(0xFF, 0xFF) @@ -373,7 +311,7 @@ def write_flash(self, addr, page_buffer, target_page, page_count): retry_counter = 5 # print "Flasing to 0x{:X}".format(addr) - while ((not pk or pk.header != 0xFF or + while ((not pk or pk.header != 0xFF or len(pk.data) < 2 or struct.unpack('= 0): pk = CRTPPacket() @@ -381,7 +319,14 @@ def write_flash(self, addr, page_buffer, target_page, page_count): pk.data = struct.pack('. +""" 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): + """ + 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 + """ + CPX_VERSION = 0 + + def __init__(self, function=0, destination=0, source=CPXTarget.HOST, data=bytearray()): + """ + Create an empty packet with default values. + """ + self.data = data + self.source = source + self.destination = destination + self.function = function + self.version = self.CPX_VERSION + self.length = len(data) + self.lastPacket = False + + def _get_wire_data(self): + """Create raw data to send via the wire""" + raw = bytearray() + + targetsAndFlags = ((self.source.value & 0x7) << 3) | (self.destination.value & 0x7) + if self.lastPacket: + targetsAndFlags |= 0x40 + + functionAndVersion = (self.function.value & 0x3F) | ((self.version & 0x3) << 6) + 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(functionAndVersion & 0x3F) + 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, _set_wire_data) + + +class CPXRouter(threading.Thread): + + def __init__(self, transport): + threading.Thread.__init__(self) + self.daemon = True + 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 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) + + def makeTransaction(self, packet): + self.sendPacket(packet) + return self.receivePacket(packet.function) + + def sendPacket(self, packet): + self._transport.writePacket(packet) + + def transport(self): + 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, 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() + + 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, timeout) + + def makeTransaction(self, packet): + return self._router.makeTransaction(packet) + + def sendPacket(self, packet): + self._router.sendPacket(packet) + + def close(self): + self._router.transport().disconnect() diff --git a/cflib/cpx/transports.py b/cflib/cpx/transports.py new file mode 100644 index 000000000..830c46b9b --- /dev/null +++ b/cflib/cpx/transports.py @@ -0,0 +1,171 @@ +import socket +import struct +from threading import Lock + +from . import CPXPacket + +found_serial = True +try: + import serial +except ImportError: + found_serial = False + + +class CPXTransport: + def __init__(self): + raise NotImplementedError('Cannot be used') + + # Change this to URI? + def connect(host, port): + raise NotImplementedError('Cannot be used') + + def disconnect(): + raise NotImplementedError('Cannot be used') + + def send(self, data): + raise NotImplementedError('Cannot be used') + + def receive(self, size): + raise NotImplementedError('Cannot be used') + + +class SocketTransport(CPXTransport): + def __init__(self, host, port): + print('CPX socket transport') + self._host = host + self._port = port + + 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)) + print('Connected') + + def disconnect(self): + print('Closing transport') + self._socket.shutdown(socket.SHUT_WR) + self._socket.close() + self._socket = None + + def writePacket(self, packet): + 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 + + def readPacket(self): + size = struct.unpack('H', self._readData(2))[0] + + data = self._readData(size) + + packet = CPXPacket() + packet.wireData = data + + return packet + + 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() + + 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) + # 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]) + + packet = CPXPacket() + packet.wireData = data + + return packet + + +class CRTPTransport(CPXTransport): + def __init__(self): + print('CPX CRTP transport') + + # This connection will not really work... + def connect(host, port): + pass + + def disconnect(): + pass + + def send(self, data): + pass + + def receive(self, size): + pass diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 66de9e1a6..89bfedf1c 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. @@ -34,21 +32,26 @@ """ 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 import cflib.crtp +from .appchannel import Appchannel 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 from .param import Param from .platformservice import PlatformService +from .supervisor import Supervisor from .toccache import TocCache from cflib.crazyflie.high_level_commander import HighLevelCommander from cflib.utils.callbacks import Caller @@ -74,8 +77,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 @@ -86,17 +89,19 @@ 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() + # 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 self.packet_sent = Caller() - # Called when the link driver updates the link quality measurement - self.link_quality_updated = Caller() self.state = State.DISCONNECTED @@ -105,8 +110,9 @@ 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.start() + self.incoming.daemon = True + if self.link: + self.incoming.start() self.commander = Commander(self) self.high_level_commander = HighLevelCommander(self) @@ -117,6 +123,9 @@ 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_statistics = LinkStatistics(self) + self.supervisor = Supervisor(self) self.link_uri = '' @@ -130,6 +139,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)) @@ -137,17 +148,31 @@ 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( + lambda uri: logger.info('Callback->Connection completed [%s]', uri)) + self.connected.add_callback( - lambda uri: logger.info( - 'Callback->Connection setup finished [%s]', uri)) + lambda uri: self.link_statistics.start()) + 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.""" @@ -180,6 +205,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]', @@ -189,16 +219,14 @@ 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): - """Called from link driver to report link quality""" - self.link_quality_updated.call(percentage) - def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. @@ -220,7 +248,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.link_statistics.radio_link_statistics_callback, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ @@ -228,6 +256,8 @@ def open_link(self, link_uri): logger.warning(message) self.connection_failed.call(link_uri, message) else: + 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 self.packet_received.add_callback( @@ -256,8 +286,17 @@ 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(timeout=1) + 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 """Check if the communication link is open or not.""" @@ -272,6 +311,14 @@ 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""" logger.info('Resending for pattern %s', pattern) @@ -303,11 +350,15 @@ 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 """ + + 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 \ @@ -340,6 +391,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') @@ -349,9 +403,11 @@ 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 = [] + self._stop_event = Event() def add_port_callback(self, port, cb): """Add a callback for data that comes on a specific port""" @@ -361,9 +417,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): @@ -375,10 +429,27 @@ 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 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) diff --git a/cflib/crazyflie/appchannel.py b/cflib/crazyflie/appchannel.py new file mode 100644 index 000000000..e47ca5a1d --- /dev/null +++ b/cflib/crazyflie/appchannel.py @@ -0,0 +1,57 @@ +# -*- 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, see . +""" +Data channel to communicate with an application running in the Crazyflie +""" +import logging + +import cflib.crazyflie.platformservice +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort +from cflib.utils.callbacks import Caller +# 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/commander.py b/cflib/crazyflie/commander.py index 405af8e3b..82377dc1a 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 # @@ -20,26 +20,36 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 """ import struct +import warnings from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort +from cflib.utils.encoding import compress_quaternion __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_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_MANUAL = 11 + +TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0 class Commander(): @@ -62,12 +72,18 @@ def set_client_xmode(self, enabled): """ self._x_mode = enabled - def send_setpoint(self, roll, pitch, yaw, thrust): + def send_setpoint(self, roll, pitch, yawrate, thrust): """ - Send a new control setpoint for roll/pitch/yaw/thrust to the copter + Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the copter. - The arguments roll/pitch/yaw/trust is the new setpoints that should - be sent 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`. + 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') @@ -77,7 +93,19 @@ def send_setpoint(self, roll, pitch, yaw, thrust): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER - 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('. """ Crazyflie console is used to receive characters printed using printf from the firmware. @@ -47,11 +45,27 @@ 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) + self.cf.add_port_callback(CRTPPort.CONSOLE, self._incoming) - def incoming(self, packet): + def _incoming(self, packet): """ Callback for data received from the copter. """ diff --git a/cflib/crazyflie/extpos.py b/cflib/crazyflie/extpos.py index 53486dff0..724fc6ea0 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 # @@ -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 """ @@ -50,3 +48,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/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 68b63cf44..a9904af64 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 # @@ -20,13 +20,18 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 +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 cflib.crtp.crtpstack import CRTPPacket @@ -42,17 +47,22 @@ 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 + COMMAND_SPIRAL = 11 + COMMAND_GO_TO_2 = 12 + COMMAND_START_TRAJECTORY_2 = 13 ALL_GROUPS = 0 TRAJECTORY_LOCATION_MEM = 1 + TRAJECTORY_TYPE_POLY4D = 0 + TRAJECTORY_TYPE_POLY4D_COMPRESSED = 1 def __init__(self, crazyflie=None): """ @@ -64,113 +74,218 @@ 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(' 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('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 reversed: set to True, if trajectory should be executed in + :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 + :param group_mask: Mask for which CFs this should apply to :return: """ - self._send_packet(struct.pack('. +""" +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. +""" +import struct +import time +from threading import Event +from threading import Thread + +import numpy as np + +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort +from cflib.utils.callbacks import Caller + +__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 + + # 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: + """ + 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 = None + self.latency = 0 + self.latency_updated = Caller() + + 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. + """ + 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, name='ping_thread') + 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() + if self._ping_thread_instance is not None: + self._ping_thread_instance.join() + self._ping_thread_instance = None + + def _ping_thread(self, interval: float = 0.1) -> 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 0.1 seconds. + """ + 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/crazyflie/localization.py b/cflib/crazyflie/localization.py index 7b3bd053d..f032f1144 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -7,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2017 Bitcraze AB +# Copyright (C) 2017-2023 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -20,20 +20,20 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 """ import collections import logging import struct +import warnings from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort from cflib.utils.callbacks import Caller +from cflib.utils.encoding import fp16_to_float __author__ = 'Bitcraze AB' __all__ = ['Localization', 'LocalizationPacket'] @@ -56,9 +56,17 @@ 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 # Deprecated: use supervisor.send_emergency_stop() + EMERGENCY_STOP_WATCHDOG = 4 # Deprecated: use supervisor.send_emergency_stop_watchdog() + COMM_GNSS_NMEA = 6 + COMM_GNSS_PROPRIETARY = 7 + EXT_POSE = 8 + EXT_POSE_PACKED = 9 + LH_ANGLE_STREAM = 10 + LH_PERSIST_DATA = 11 def __init__(self, crazyflie=None): """ @@ -94,10 +102,33 @@ def _incoming(self, packet): anchor_id, distance = struct.unpack('= 12: + self._cf.supervisor.send_emergency_stop() + else: + pk = CRTPPacket() + pk.port = CRTPPort.LOCALIZATION + pk.channel = self.GENERIC_CH + pk.data = struct.pack('= 12: + self._cf.supervisor.send_emergency_stop_watchdog() + else: + pk = CRTPPacket() + pk.port = CRTPPort.LOCALIZATION + pk.channel = self.GENERIC_CH + pk.data = struct.pack(' 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 + for bs in geo_list: + mask_geo += 1 << bs + for bs in calib_list: + mask_calib += 1 << bs + + pk = CRTPPacket() + pk.port = CRTPPort.LOCALIZATION + pk.channel = self.GENERIC_CH + pk.data = struct.pack( + '. +# flake8: noqa """ Enables logging of variables from the Crazyflie. @@ -32,33 +31,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. | +| 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. | +| 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 @@ -99,9 +88,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__) @@ -146,6 +132,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() @@ -163,6 +152,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 = [] @@ -217,15 +207,22 @@ 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) - for var in self.variables: + return CMD_CREATE_BLOCK + + 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) @@ -233,23 +230,64 @@ def create(self): 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)) - 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)) + pk.data.append(element_id) + + return True, i + + def create(self): + """Save the log configuration in the Crazyflie""" + 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: - self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id)) + 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) + 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""" @@ -318,7 +356,7 @@ class LogTocElement: 0x04: ('int8_t', ' 0 and logconf.period < 0xFF)): logconf.valid = True logconf.cf = self.cf @@ -467,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""" @@ -476,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,) @@ -509,6 +560,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)', diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py deleted file mode 100644 index 06e0ac55a..000000000 --- a/cflib/crazyflie/mem.py +++ /dev/null @@ -1,1362 +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 -import sys -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 - -# 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' - -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 - - 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_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 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""" - # 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) - self._write_requests_lock = Lock() - - self._clear_state() - - def _clear_state(self): - self.mems = [] - 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) - 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('. +""" +Enables access to the Crazyflie memory subsystem. + +""" +import errno +import logging +import struct +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 +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 +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 +from .trajectory_memory import TrajectoryMemory +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort +from cflib.utils.callbacks import Caller + +__author__ = 'Bitcraze AB' +__all__ = ['Memory', 'Poly4D', 'CompressedStart', 'CompressedSegment', 'MemoryElement', + 'LighthouseBsGeometry', 'LighthouseBsCalibration', 'LighthouseMemHelper', + 'DeckMemoryManager'] + +# 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 + +logger = logging.getLogger(__name__) + + +class _ReadRequest: + """ + Class used to handle memory reads that will split up the read in multiple + packets if necessary + """ + MAX_DATA_LENGTH = 24 + + def __init__(self, mem, addr, length, cf): + """Initialize the object with good defaults""" + self.mem = mem + self.addr = addr + self._bytes_left = length + self.data = bytearray() + self.cf = cf + + self._current_addr = addr + + def start(self): + """Start the fetching of the data""" + self._request_new_chunk() + + def resend(self): + logger.debug('Sending write again...') + self._request_new_chunk() + + def _request_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 = self._bytes_left + if new_len > _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 = 24 + + 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 + + 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(' 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() + 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() + + self._clear_refresh_callbacks() + + # 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._refresh_failed_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 _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 + 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._clear_refresh_callbacks() + + 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, progress_cb=None): + """Write the specified data to the given memory at the given address""" + wreq = _WriteRequest(memory, addr, data, self.cf, progress_cb) + 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].append(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, 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: + 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._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 + cmd = packet.data[0] + payload = packet.data[1:] + + if chan == CHAN_INFO: + self._handle_chan_info(cmd, payload) + if chan == CHAN_WRITE: + 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: + 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: + 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._clear_refresh_callbacks() + 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) + 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) + 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) + 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) + 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._clear_refresh_callbacks() + + def _handle_chan_write(self, cmd, payload): + 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._write_requests[id].pop(0) + do_call_fail_cb = True + + # 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() + + # Call callbacks after the lock has been released to allow for new writes + # to be initiated from the callback. + 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) + + def _handle_chan_read(self, cmd, payload): + id = cmd + (addr, status) = struct.unpack('. +import logging +import struct + +from .memory_element import MemoryElement +from cflib.utils.callbacks import Syncer + +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 + + 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 + + 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._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 + 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: + 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, progress_cb) + + 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, progress_cb=progress_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: + raise Exception('Deck not ready') + + 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_field1 & self.MASK_IS_VALID) != 0 + + @property + def is_started(self): + return (self._bit_field1 & self.MASK_IS_STARTED) != 0 + + @property + def supports_read(self): + return (self._bit_field1 & self.MASK_SUPPORTS_READ) != 0 + + @property + def supports_write(self): + return (self._bit_field1 & self.MASK_SUPPORTS_WRITE) != 0 + + @property + def supports_fw_upgrade(self): + return (self._bit_field1 & self.MASK_SUPPORTS_UPGRADE) != 0 + + @property + def is_fw_upgrade_required(self): + return (self._bit_field1 & self.MASK_UPGRADE_REQUIRED) != 0 + + @property + def is_bootloader_active(self): + 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('. +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 read_failed(self, mem, addr, data): + """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: + 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/i2c_element.py b/cflib/crazyflie/mem/i2c_element.py new file mode 100644 index 000000000..7f8554a2e --- /dev/null +++ b/cflib/crazyflie/mem/i2c_element.py @@ -0,0 +1,131 @@ +# -*- 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 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..a7078159b --- /dev/null +++ b/cflib/crazyflie/mem/lighthouse_memory.py @@ -0,0 +1,495 @@ +# -*- 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_BOOL = 1 + 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 = [ + [0.0, 0.0, 0.0], + [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( + 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]), + ] + self.valid = struct.unpack(' 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_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_success) + + 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) + + lh_mem = mems[0] + + self.geo_reader = self._ObjectReader(lh_mem.read_geo_data) + self.geo_writer = self._ObjectWriter(lh_mem.write_geo_data) + + self.calib_reader = self._ObjectReader(lh_mem.read_calib_data) + self.calib_writer = self._ObjectWriter(lh_mem.write_calib_data) + + 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) + + 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/crazyflie/mem/loco_memory.py b/cflib/crazyflie/mem/loco_memory.py new file mode 100644 index 000000000..bba3b913e --- /dev/null +++ b/cflib/crazyflie/mem/loco_memory.py @@ -0,0 +1,110 @@ +# -*- 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 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..83b2a6304 --- /dev/null +++ b/cflib/crazyflie/mem/memory_element.py @@ -0,0 +1,86 @@ +# -*- 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 + TYPE_APP = 0x18 + 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""" + 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' + 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): + 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..fbdd6097b --- /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 readValidationSuccess 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.readValidationSuccess = 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 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, data): + if mem.id == self.id: + logger.debug('Read failed') + + def disconnect(self): + self._read_finished_cb = None diff --git a/cflib/crazyflie/mem/ow_element.py b/cflib/crazyflie/mem/ow_element.py new file mode 100644 index 000000000..c8f615575 --- /dev/null +++ b/cflib/crazyflie/mem/ow_element.py @@ -0,0 +1,168 @@ +# -*- 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 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 + +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, data): + if mem.id == self.id: + logger.debug('Read failed') + + def disconnect(self): + self._read_finished_cb = None diff --git a/cflib/crazyflie/mem/trajectory_memory.py b/cflib/crazyflie/mem/trajectory_memory.py new file mode 100644 index 000000000..e4bec3415 --- /dev/null +++ b/cflib/crazyflie/mem/trajectory_memory.py @@ -0,0 +1,234 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2019 - 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 math +import struct + +from .memory_element import MemoryElement +from cflib.utils.callbacks import Syncer + +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() + + 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('. """ Enables reading/writing of parameter values to/from the Crazyflie. @@ -31,9 +29,14 @@ the parameters that can be written/read. """ +import copy +import errno import logging import struct -import sys +from collections import namedtuple +from queue import Empty +from queue import Queue +from threading import Event from threading import Lock from threading import Thread @@ -42,11 +45,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' __all__ = ['Param', 'ParamTocElement'] @@ -62,6 +60,20 @@ TOC_CHANNEL = 0 READ_CHANNEL = 1 WRITE_CHANNEL = 2 +MISC_CHANNEL = 3 + +MISC_SETBYNAME = 0 +MISC_VALUE_UPDATED = 1 +MISC_GET_EXTENDED_TYPE = 2 +MISC_PERSISTENT_STORE = 3 +MISC_PERSISTENT_GET_STATE = 4 +MISC_PERSISTENT_CLEAR = 5 +MISC_GET_DEFAULT_VALUE = 6 +MISC_GET_EXTENDED_TYPE_V2 = 7 +MISC_GET_DEFAULT_VALUE_V2 = 8 + +PersistentParamState = namedtuple('PersistentParamState', 'is_stored default_value stored_value') + # One element entry in the TOC @@ -72,6 +84,8 @@ class ParamTocElement: RW_ACCESS = 0 RO_ACCESS = 1 + EXTENDED_PERSISTENT = 1 + types = {0x08: ('uint8_t', ' 0: + 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() + 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 _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 = {} + self._initialized.clear() + def _disconnected(self, uri): + logger.info('Disconnected, cleaning up threads') """Disconnected callback from Crazyflie API""" - self.param_updater.close() - self.is_updated = False + 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 + # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} @@ -251,13 +312,40 @@ def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ + if self.param_updater is None: + raise Exception('Param updater not initialized, did you call open_connection?') 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'= 11 + cmd = MISC_GET_DEFAULT_VALUE_V2 if use_v2 else MISC_GET_DEFAULT_VALUE + + def new_packet_cb(pk): + if pk.channel == MISC_CHANNEL and pk.data[0] == cmd: + if use_v2: + # V2: [CMD, ID_L, ID_H, STATUS, VALUE...] on success + # [CMD, ID_L, ID_H, ERROR_CODE] on error + if pk.data[3] != 0: + callback(complete_name, None) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + return + default_value, = struct.unpack(element.pytype, pk.data[4:]) + else: + # V1: [CMD, ID_L, ID_H, VALUE...] + # Ambiguous: ENOENT (0x02) indistinguishable from u8 value 2 + 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('= 11 + self._cmd = MISC_GET_EXTENDED_TYPE_V2 if self._use_v2 else MISC_GET_EXTENDED_TYPE + + def _new_packet_cb(self, pk): + """Callback for newly arrived packets""" + if pk.channel == MISC_CHANNEL and pk.data[0] == self._cmd: + var_id = struct.unpack('. """ Used for sending control setpoints to the Crazyflie """ import logging +import warnings from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort @@ -39,8 +38,11 @@ PLATFORM_COMMAND = 0 VERSION_COMMAND = 1 +APP_CHANNEL = 2 PLATFORM_SET_CONT_WAVE = 0 +PLATFORM_REQUEST_ARMING = 1 # Deprecated: use supervisor.send_arming_request() +PLATFORM_REQUEST_CRASH_RECOVERY = 2 # Deprecated: use supervisor.send_crash_recovery_request() VERSION_GET_PROTOCOL = 0 VERSION_GET_FIRMWARE = 1 @@ -59,15 +61,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) + 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._protocolVersion = -1 self._callback = None @@ -89,9 +85,45 @@ 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 (deprecated). + + Args: + do_arm (bool): True = arm the system, False = disarm the system + + Deprecated: + Use `supervisor.send_arming_request(do_arm)` instead. + """ + warnings.warn( + 'platform.send_arming_request is deprecated. ' + 'Use supervisor.send_arming_request(do_arm) instead.', + category=DeprecationWarning, + stacklevel=2 + ) + + self._cf.supervisor.send_arming_request(do_arm) + + def send_crash_recovery_request(self): + """ + Send crash recovery request (deprecated). + + Deprecated: + Use `supervisor.send_crash_recovery_request()` instead. + """ + warnings.warn( + 'platform.send_crash_recovery_request is deprecated. ' + 'Use supervisor.send_crash_recovery_request() instead.', + category=DeprecationWarning, + stacklevel=2 + ) + + self._cf.supervisor.send_crash_recovery_request() + def get_protocol_version(self): """ Return version of the CRTP protocol @@ -105,26 +137,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('Procotol 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('Procotol 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() diff --git a/cflib/crazyflie/supervisor.py b/cflib/crazyflie/supervisor.py new file mode 100644 index 000000000..08249871e --- /dev/null +++ b/cflib/crazyflie/supervisor.py @@ -0,0 +1,341 @@ +# -*- 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 . +""" +Provides access to the supervisor module of the Crazyflie platform. +""" +import logging +import struct +import threading +import time +import warnings + +from cflib.crazyflie.localization import Localization +from cflib.crazyflie.platformservice import PLATFORM_COMMAND +from cflib.crazyflie.platformservice import PLATFORM_REQUEST_ARMING +from cflib.crazyflie.platformservice import PLATFORM_REQUEST_CRASH_RECOVERY +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort + +__author__ = 'Bitcraze AB' +__all__ = ['Supervisor'] + +logger = logging.getLogger(__name__) + +SUPERVISOR_CH_INFO = 0 +SUPERVISOR_CH_COMMAND = 1 + +# Bit positions +BIT_CAN_BE_ARMED = 0 +BIT_IS_ARMED = 1 +BIT_IS_AUTO_ARMED = 2 +BIT_CAN_FLY = 3 +BIT_IS_FLYING = 4 +BIT_IS_TUMBLED = 5 +BIT_IS_LOCKED = 6 +BIT_IS_CRASHED = 7 +BIT_HL_CONTROL_ACTIVE = 8 +BIT_HL_TRAJ_FINISHED = 9 +BIT_HL_CONTROL_DISABLED = 10 + +CMD_GET_STATE_BITFIELD = 0x0C + +CMD_ARM_SYSTEM = 0x01 +CMD_RECOVER_SYSTEM = 0x02 +CMD_EMERGENCY_STOP = 0x03 +CMD_EMERGENCY_STOP_WATCHDOG = 0x04 + + +class Supervisor: + """ + This class provides two main functionalities: + + 1. **Reading the system state** + Accesses the Crazyflie's supervisor bitfield to determine the current state, + such as whether it can be armed, is flying or crashed. + + 2. **Sending system commands** + Sends arming/disarming requests and crash recovery commands to the Crazyflie. + """ + STATES = [ + 'Can be armed', + 'Is armed', + 'Is auto armed', + 'Can fly', + 'Is flying', + 'Is tumbled', + 'Is locked', + 'Is crashed', + 'HL control is active', + 'Finished HL trajectory', + 'HL control is disabled' + ] + + def __init__(self, crazyflie): + self._cf = crazyflie + + self._cache_timeout = 0.1 # seconds + self._last_fetch_time = 0 + self._bitfield = None + self._cf.add_port_callback(CRTPPort.SUPERVISOR, self._supervisor_callback) + self._bitfield_received = threading.Event() + + def _is_legacy_firmware(self): + """Returns True if the firmware does not support the supervisor port.""" + return self._cf.platform.get_protocol_version() < 12 + + def _warn_legacy_firmware(self): + """Warn that the firmware is too old for the supervisor port.""" + version = self._cf.platform.get_protocol_version() + warnings.warn( + 'The supervisor subsystem requires CRTP protocol version 12 or later. ' + f'Connected Crazyflie reports version {version}. ' + 'Update your Crazyflie firmware. Using legacy fallback.', + stacklevel=3, + ) + + def _supervisor_callback(self, pk: CRTPPacket): + """ + Called when a packet is received. + """ + if len(pk.data) < 1: + return + + cmd = pk.data[0] + if cmd & 0x80: # high bit = response + orig_cmd = cmd & 0x7F + if orig_cmd == CMD_GET_STATE_BITFIELD: + self._bitfield = int.from_bytes(pk.data[1:], byteorder='little') + self._bitfield_received.set() + logger.info(f'Supervisor bitfield received: 0x{self._bitfield:04X}') + + elif orig_cmd == CMD_ARM_SYSTEM: + success = bool(pk.data[1]) + is_armed = bool(pk.data[2]) + logger.info(f'Arm response: success={success}, is_armed={is_armed}') + + elif orig_cmd == CMD_RECOVER_SYSTEM: + success = bool(pk.data[1]) + recovered = bool(pk.data[2]) + logger.info(f'Recovery response: success={success}, recovered={recovered}') + + def _fetch_bitfield(self, timeout=0.2): + """ + Request the bitfield and wait for response (blocking). + Uses time-based cache to avoid sending packages too frequently. + """ + if self._is_legacy_firmware(): + self._warn_legacy_firmware() + return 0 + now = time.time() + + # Return cached value if it's recent enough + if self._bitfield is not None and (now - self._last_fetch_time) < self._cache_timeout: + return self._bitfield + + # Send a new request + self._bitfield_received.clear() + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_INFO) + pk.data = [CMD_GET_STATE_BITFIELD] + self._cf.send_packet(pk) + + # Wait for response + if not self._bitfield_received.wait(timeout): + logger.warning('Timeout waiting for supervisor bitfield response') + return self._bitfield or 0 # still return last known value + + # Update timestamp + self._last_fetch_time = time.time() + return self._bitfield or 0 + + def _bit(self, position): + bitfield = self._fetch_bitfield() + return bool((bitfield >> position) & 0x01) + + def read_bitfield(self): + """ + Directly get the full bitfield value. + """ + return self._fetch_bitfield() + + def read_state_list(self): + """ + Reads the bitfield and returns the list of all active states. + """ + bitfield = self.read_bitfield() + state_list = self.decode_bitfield(bitfield) + return state_list + + def decode_bitfield(self, value): + """ + Given a bitfield integer `value` and a list of `self.STATES`, + returns the names of all states whose bits are set. + Bit 0 corresponds to states[0], Bit 1 to states[1], etc. + * Bit 0 = Can be armed - the system can be armed and will accept an arming command. + * Bit 1 = Is armed - the system is armed. + * Bit 2 = Is auto armed - the system is configured to automatically arm. + * Bit 3 = Can fly - the Crazyflie is ready to fly. + * Bit 4 = Is flying - the Crazyflie is flying. + * Bit 5 = Is tumbled - the Crazyflie is up side down. + * Bit 6 = Is locked - the Crazyflie is in the locked state and must be restarted. + * Bit 7 = Is crashed - the Crazyflie has crashed. + * Bit 8 = High level control is actively flying the drone. + * Bit 9 = High level trajectory has finished. + * Bit 10 = High level control is disabled and not producing setpoints. + """ + if value < 0: + raise ValueError('value must be >= 0') + + result = [] + for bit_index, name in enumerate(self.STATES): + if value & (1 << bit_index): + result.append(name) + + return result + + @property + def can_be_armed(self): + return self._bit(BIT_CAN_BE_ARMED) + + @property + def is_armed(self): + return self._bit(BIT_IS_ARMED) + + @property + def is_auto_armed(self): + return self._bit(BIT_IS_AUTO_ARMED) + + @property + def can_fly(self): + return self._bit(BIT_CAN_FLY) + + @property + def is_flying(self): + return self._bit(BIT_IS_FLYING) + + @property + def is_tumbled(self): + return self._bit(BIT_IS_TUMBLED) + + @property + def is_locked(self): + return self._bit(BIT_IS_LOCKED) + + @property + def is_crashed(self): + return self._bit(BIT_IS_CRASHED) + + @property + def hl_control_active(self): + return self._bit(BIT_HL_CONTROL_ACTIVE) + + @property + def hl_traj_finished(self): + return self._bit(BIT_HL_TRAJ_FINISHED) + + @property + def hl_control_disabled(self): + return self._bit(BIT_HL_CONTROL_DISABLED) + + def send_arming_request(self, do_arm: bool): + """ + Send system arm/disarm request. + + If the connected Crazyflie does not support CRTP protocol version 12 + or later, the legacy platform channel is used as a fallback. + + Args: + do_arm (bool): True = arm the system, False = disarm the system + """ + if self._is_legacy_firmware(): + self._warn_legacy_firmware() + pk = CRTPPacket() + pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND) + pk.data = (PLATFORM_REQUEST_ARMING, do_arm) + self._cf.send_packet(pk) + else: + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_ARM_SYSTEM, do_arm) + self._cf.send_packet(pk) + logger.debug(f'Sent arming request: do_arm={do_arm}') + + def send_crash_recovery_request(self): + """ + Send crash recovery request. + + If the connected Crazyflie does not support CRTP protocol version 12 + or later, the legacy platform channel is used as a fallback. + """ + if self._is_legacy_firmware(): + self._warn_legacy_firmware() + pk = CRTPPacket() + pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND) + pk.data = (PLATFORM_REQUEST_CRASH_RECOVERY,) + self._cf.send_packet(pk) + else: + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_RECOVER_SYSTEM,) + self._cf.send_packet(pk) + logger.debug('Sent crash recovery request') + + def send_emergency_stop(self): + """ + Send emergency stop. The Crazyflie will immediately stop all motors. + + If the connected Crazyflie does not support CRTP protocol version 12 + or later, the legacy localization channel is used as a fallback. + """ + if self._is_legacy_firmware(): + self._warn_legacy_firmware() + pk = CRTPPacket() + pk.set_header(CRTPPort.LOCALIZATION, Localization.GENERIC_CH) + pk.data = struct.pack('. +import time +from collections import namedtuple 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 + +SwarmPosition = namedtuple('SwarmPosition', 'x y z') class _Factory: @@ -73,6 +77,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) @@ -107,6 +112,83 @@ 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`, 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 + + 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. 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): """ Execute a function for all Crazyflies in the swarm, in sequence. @@ -114,9 +196,11 @@ 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 def my_function(scf, optional_param0, optional_param1) ... @@ -127,10 +211,11 @@ 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 + :param func: The function to execute + :param args_dict: Parameters to pass to the function """ for uri, cf in self._cfs.items(): args = self._process_args_dict(cf, uri, args_dict) @@ -143,10 +228,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) @@ -160,10 +245,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() @@ -180,16 +265,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 +288,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 diff --git a/cflib/crazyflie/syncCrazyflie.py b/cflib/crazyflie/syncCrazyflie.py index cfd958906..9aacc5bd4 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 # @@ -19,58 +19,114 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 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 +into blocking functions. 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 cflib.crazyflie import Crazyflie +logger = logging.getLogger(__name__) + 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 + :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 else: self.cf = Crazyflie() self._link_uri = link_uri - self._connect_event = Event() + 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') self._add_callbacks() - print('Connecting to %s' % self._link_uri) + 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() + self.cf.close_link() 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 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 + self._params_updated_event.clear() def __exit__(self, exc_type, exc_val, exc_tb): self.close_link() @@ -78,29 +134,40 @@ 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.""" - print('Connected to %s' % link_uri) + logger.debug('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)) + logger.debug('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 _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.fully_connected.add_callback(self._all_params_updated) def _remove_callbacks(self): def remove_callback(container, callback): @@ -112,3 +179,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.fully_connected, self._all_params_updated) diff --git a/cflib/crazyflie/syncLogger.py b/cflib/crazyflie/syncLogger.py index ccd41de00..3735cc1f0 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 # @@ -19,25 +19,18 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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. 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 +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' @@ -46,15 +39,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,21 +61,23 @@ 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() - self._log_config.data_received_cb.remove_callback( - self._log_callback) - self._cf.disconnected.remove_callback(self._disconnected) + config.data_received_cb.remove_callback( + self._log_callback) - self._queue.empty() + self._cf.disconnected.remove_callback(self._disconnected) self._is_connected = False @@ -99,6 +97,7 @@ def __next__(self): data = self._queue.get() if data == self.DISCONNECT_EVENT: + self._queue.empty() raise StopIteration return data @@ -109,10 +108,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) diff --git a/cflib/crazyflie/toc.py b/cflib/crazyflie/toc.py index 12f085ea0..4ae36329f 100644 --- a/cflib/crazyflie/toc.py +++ b/cflib/crazyflie/toc.py @@ -20,12 +20,10 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 minipulate +A generic TableOfContents module that is used to fetch, store and manipulate a TOC for logging or parameters. """ import logging @@ -177,7 +175,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 @@ -197,7 +200,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/crazyflie/toccache.py b/cflib/crazyflie/toccache.py index 2bd8602dd..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. @@ -100,13 +98,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 +124,7 @@ def _decoder(self, obj): elem.ctype = str(obj['ctype']) elem.pytype = str(obj['pytype']) elem.access = obj['access'] + if isinstance(elem, ParamTocElement): + elem.extended = obj['extended'] return elem return obj diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 9f7ce32e7..3d410554b 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -20,17 +20,17 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 -from .debugdriver import DebugDriver from .exceptions import WrongUriType +from .prrtdriver import PrrtDriver from .radiodriver import RadioDriver from .serialdriver import SerialDriver +from .tcpdriver import TcpDriver from .udpdriver import UdpDriver from .usbdriver import UsbDriver @@ -40,18 +40,26 @@ logger = logging.getLogger(__name__) -DRIVERS = [RadioDriver, SerialDriver, UdpDriver, DebugDriver, UsbDriver] 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: - 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: + logger.warn('The debug driver is no longer supported!') + + if enable_serial_driver: + CLASSES.append(SerialDriver) + + CLASSES.extend([UdpDriver, PrrtDriver, TcpDriver]) def scan_interfaces(address=None): @@ -81,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, 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, link_quality_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 new file mode 100644 index 000000000..9d760a7a7 --- /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, see . +""" +Crazyflie driver using the cflinkcpp implementation. + +This driver is used to communicate over the radio or USB. +""" +import logging +import threading + +import cflinkcpp + +from .crtpstack import CRTPPacket +from cflib.crtp.crtpdriver import CRTPDriver +from cflib.crtp.radio_link_statistics import RadioLinkStatistics + +__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 + self._radio_link_statistics = RadioLinkStatistics() + + 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 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._radio_link_statistics_callback = radio_link_statistics_callback + self._link_error_callback = link_error_callback + + if uri.startswith('radio://') and radio_link_statistics_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: + self._radio_link_statistics.link_quality = min(ack_count, sent_count) / sent_count * 100.0 + else: + self._radio_link_statistics.link_quality = 1 + self._last_connection_stats = stats + + 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') + + threading.Timer(1.0, self._recompute_link_quality_timer).start() diff --git a/cflib/crtp/crtpdriver.py b/cflib/crtp/crtpdriver.py index 7d9f7d8cb..598b29e3d 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. """ @@ -44,11 +42,11 @@ def __init__(self): """ self.needs_resending = True - def connect(self, uri, link_quality_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 link_quality_callback Callback to report link quality in percent + @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/crtpstack.py b/cflib/crtp/crtpstack.py index 74c5d98dc..8204764cd 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'] @@ -48,8 +47,8 @@ class CRTPPort: LOCALIZATION = 0x06 COMMANDER_GENERIC = 0x07 SETPOINT_HL = 0x08 + SUPERVISOR = 0x09 PLATFORM = 0x0D - DEBUGDRIVER = 0x0E LINKCTRL = 0x0F ALL = 0xFF @@ -59,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. @@ -118,16 +120,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: - if sys.version_info < (3,): - self._data = bytearray(data) - else: - self._data = bytearray(data.encode('ISO-8859-1')) - elif type(data) == list or type(data) == tuple: + elif isinstance(data, str): + self._data = bytearray(data.encode('ISO-8859-1')) + elif isinstance(data, list) or isinstance(data, tuple): self._data = bytearray(data) - elif sys.version_info >= (3,) and type(data) == bytes: + elif isinstance(data, bytes): self._data = bytearray(data) else: raise Exception('Data must be bytearray, string, list or tuple,' @@ -145,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) diff --git a/cflib/crtp/debugdriver.py b/cflib/crtp/debugdriver.py deleted file mode 100644 index 65abc8cef..000000000 --- a/cflib/crtp/debugdriver.py +++ /dev/null @@ -1,897 +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 random -import re -import string -import struct -import sys -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 -if sys.version_info < (3,): - import Queue as queue -else: - import queue - -__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) - else: - logger.warning( - 'Not handling incoming packets on port [%d]', - pk.port) - - 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/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/pcap.py b/cflib/crtp/pcap.py new file mode 100644 index 000000000..d00e9b02c --- /dev/null +++ b/cflib/crtp/pcap.py @@ -0,0 +1,144 @@ +import os +import struct +import time +from enum import IntEnum + + +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. + """ + # 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) + '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, link_type: LinkType, receive, devid, address, channel, 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( + ' 100: + self._retry_sum -= self._retries.popleft() + self.radio_link_statistics['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.radio_link_statistics['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.radio_link_statistics['uplink_rate'] = self._amount_packets_up / \ + (time.time() - self._previous_time_stamp) + 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 + self._amount_null_packets_up = 0 + self._amount_packets_down = 0 + self._amount_null_packets_down = 0 + self._previous_time_stamp = time.time() diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py index a0a86a8d4..3f0894fb6 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. @@ -32,24 +30,31 @@ """ import array import binascii -import collections import logging +import queue import re import struct -import sys import threading +import time +from enum import Enum +from queue import Queue +from threading import Semaphore +from threading import Thread +from typing import Any +from typing import Iterable +from typing import List +from typing import Optional +from typing import Tuple +from urllib.parse import parse_qs +from urllib.parse import urlparse import cflib.drivers.crazyradio as crazyradio from .crtpstack import CRTPPacket from .exceptions import WrongUriType from cflib.crtp.crtpdriver import CRTPDriver +from cflib.crtp.radio_link_statistics import RadioLinkStatistics from cflib.drivers.crazyradio import Crazyradio -if sys.version_info < (3,): - import Queue as queue -else: - import queue - __author__ = 'Bitcraze AB' __all__ = ['RadioDriver'] @@ -63,68 +68,168 @@ 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 - def __init__(self, devid, channel=0, datarate=0, address=DEFAULT_ADDR_A): - self._devid = devid + self._channel = 2 + self._address = [0xe7]*5 + self._datarate = crazyradio.Crazyradio.DR_2MPS + + self._opened = True + + 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 + + +class _SharedRadio(Thread): + def __init__(self, devid: int): + Thread.__init__(self) + self._radio = Crazyradio(devid=devid) + self._devid = devid + self.version = self._radio.version - if _RadioManager._radios[self._devid].usage_counter == 0: - try: - _RadioManager._radios[self._devid].radio.close() - except Exception: - pass - _RadioManager._radios[self._devid] = None + self.name = 'Shared Radio' - def __enter__(self): - _RadioManager._radios[self._devid].lock.acquire() + self._cmd_queue = Queue() # type: Queue[Tuple[int, _RadioCommands, Any]] # noqa + self._rsp_queues = {} # type: Dict[int, Queue[Any]] + self._next_instance_id = 0 - _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) + self._lock = Semaphore(1) - return _RadioManager._radios[self._devid].radio + self.daemon = True - def __exit__(self, type, value, traceback): - _RadioManager._radios[self._devid].lock.release() + 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 + + if self._radio is None: + self._radio = Crazyradio(devid=self._devid) + + return _SharedRadioInstance(instance_id, + self._cmd_queue, + rsp_queue, + self.version) + + def run(self): + while True: + command = self._cmd_queue.get() + + if command[1] == _RadioCommands.STOP: + with self._lock: + del self._rsp_queues[command[0]] + if len(self._rsp_queues) == 0: + self._radio.close() + self._radio = None + 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) + + @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) + + shared_radio = RadioManager._radios[devid] + if not shared_radio: + shared_radio = _SharedRadio(devid) + RadioManager._radios[devid] = shared_radio + + return shared_radio.open_instance() + + @staticmethod + def remove(devid: int): + with RadioManager._lock: + RadioManager._radios[devid] = None class RadioDriver(CRTPDriver): @@ -133,42 +238,43 @@ 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 + self.radio_link_statistics_callback = None self.in_queue = None 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): + 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 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 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. """ - devid, channel, datarate, address = self.parse_uri(uri) + devid, channel, datarate, address, rate_limit = self.parse_uri(uri) self.uri = uri + self.rate_limit = rate_limit - 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() @@ -176,89 +282,96 @@ 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, + radio_link_statistics_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): - addr = str(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(parsed_path[2]) new_addr = struct.unpack(' 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() @@ -268,11 +381,12 @@ 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.radio_link_statistics_callback, self.link_error_callback, - self) + self, + self.rate_limit) self._thread.start() def close(self): @@ -281,28 +395,29 @@ 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() # Clear callbacks self.link_error_callback = None - self.link_quality_callback = None + self.radio_link_statistics_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 = () - for l in links: + for link in links: one_to_scan = {} uri_data = re.search('^radio://([0-9]+)((/([0-9]+))' - '(/(250K|1M|2M))?)?$', - l) + '(/(250K|1M|2M))?)?', + link) one_to_scan['channel'] = int(uri_data.group(4)) @@ -318,8 +433,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: @@ -338,61 +452,63 @@ 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! + try: + serial = crazyradio.get_serials() + except Exception as e: + print(e) 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[0]) + found = [] - if address is not None: - addr = '{:X}'.format(address) - new_addr = struct.unpack(' 100: - self._retry_sum -= self._retries.popleft() - link_quality = float(self._retry_sum) / len(self._retries) * 10 - self._link_quality_callback(link_quality) - # If no copter, retry if ackStatus.ack is False: self._retry_before_disconnect = \ @@ -520,6 +625,7 @@ def run(self): # If there is a copter in range, the packet is analysed and the # next packet to send is prepared + # 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) @@ -534,6 +640,11 @@ def run(self): else: waitTime = 0 + # If there is a rate limit setup, sleep here to force the rate limit + if self.rate_limit: + time.sleep(1.0/self.rate_limit) + waitTime = 0 + # get the next packet to send of relaxation (wait 10ms) outPacket = None try: @@ -546,13 +657,16 @@ 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)) else: + # If no packet to send, send a null packet dataOut.append(0xFF) + self._radio_link_statistics.update(ackStatus, outPacket) + def set_retries_before_disconnect(nr_of_retries): global _nr_of_retries diff --git a/cflib/crtp/serialdriver.py b/cflib/crtp/serialdriver.py index 34c54036f..80ab394d0 100644 --- a/cflib/crtp/serialdriver.py +++ b/cflib/crtp/serialdriver.py @@ -20,27 +20,52 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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. """ +import logging +import queue import re +import struct +import threading -from .crtpdriver import CRTPDriver +from .crtpstack import CRTPPacket from .exceptions import WrongUriType +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.tools.list_ports as list_ports +except ImportError: + found_serial = False __author__ = 'Bitcraze AB' __all__ = ['SerialDriver'] +logger = logging.getLogger(__name__) + 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 + self.needs_resending = False + logger.info('Initialized serial driver.') def connect(self, uri, linkQualityCallback, linkErrorCallback): # check if the URI is a serial URI @@ -48,12 +73,144 @@ 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) - if not uriRe: + uri_data = re.search('^serial://([-a-zA-Z0-9/.]+)$', uri) + if not uri_data: raise Exception('Invalid serial URI') + # Move to Serial transport? + 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 + + # Prepare the inter-thread communication queue + self.in_queue = queue.Queue() + + self.cpx = CPX(UARTTransport(device, 576000)) + + self._thread = _CPXReceiveThread(self.cpx, self.in_queue, + linkErrorCallback) + self._thread.start() + + # Switch the link bridge to CPX in the Crazyflie + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.SYSTEM, + data=[0x21, 0x01])) + # Force client connect to true + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.SYSTEM, + data=[0x20, 0x01])) + + def send_packet(self, pk): + 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: + 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 [] + 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): + """ Close the link. """ + # Stop the comm thread + self._thread.stop() + + # Close the socket + try: + self.cpx.close() + self.cpx = None + + except Exception as e: + print(e) + logger.error('Could not close {}'.format(e)) + pass + print('Driver closed') + + 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 _CPXReceiveThread(threading.Thread): + """ + Radio link receiver thread used to read data from the + Socket. """ + + 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 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: + 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())) diff --git a/cflib/crtp/tcpdriver.py b/cflib/crtp/tcpdriver.py new file mode 100644 index 000000000..8325f5c87 --- /dev/null +++ b/cflib/crtp/tcpdriver.py @@ -0,0 +1,163 @@ +#!/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 logging +import queue +import re +import struct +import threading +from urllib.parse import urlparse + +from .crtpdriver import CRTPDriver +from .crtpstack import CRTPPacket +from .exceptions import WrongUriType +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' +__all__ = ['TcpDriver'] + + +class TcpDriver(CRTPDriver): + + def __init__(self): + self.needs_resending = False + + def connect(self, uri, linkQualityCallback, linkErrorCallback): + if not re.search('^tcp://', uri): + raise WrongUriType('Not an UDP URI') + + parse = urlparse(uri.split(' ')[0]) + + self.in_queue = queue.Queue() + + self.cpx = CPX(SocketTransport(parse.hostname, parse.port)) + + self._thread = _CPXReceiveThread(self.cpx, self.in_queue, + linkErrorCallback) + self._thread.start() + + self.cpx.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.SYSTEM, + data=[0x21, 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.sendPacket(CPXPacket(destination=CPXTarget.STM32, + function=CPXFunction.CRTP, + data=raw)) + + def close(self): + """ Close the link. """ + # Stop the comm thread + self._thread.stop() + + # Close the socket + try: + self.cpx.close() + self.cpx = None + + except Exception as e: + print(e) + logger.error('Could not close {}'.format(e)) + pass + print('Driver closed') + self.cpx = None + + def get_name(self): + return 'cpx' + + def scan_interface(self, address): + return [] + +# Transmit/receive thread + + +class _CPXReceiveThread(threading.Thread): + """ + Radio link receiver thread used to read data from the + Socket. """ + + 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 packet is available though the socket + # CPX receive will only return full packets + 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: + 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())) diff --git a/cflib/crtp/udpdriver.py b/cflib/crtp/udpdriver.py index dfba56d57..abaec32ee 100644 --- a/cflib/crtp/udpdriver.py +++ b/cflib/crtp/udpdriver.py @@ -20,88 +20,278 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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. -""" CRTP UDP Driver. Work either with the UDP server or with an UDP device -See udpserver.py for the protocol""" +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +""" +Crazyflie UDP driver. + +This driver communicates with a Crazyflie (or simulator) over UDP using the CRTP +protocol. It enables connecting to software-in-the-loop (SITL) simulations. +Scanning feature assumes a crazyflie server is running on port 19850-19859 +that will respond to a null CRTP packet with a valid CRTP packet. + +Wire Protocol +------------- +The UDP driver uses a simple wire protocol where each UDP datagram contains exactly +one CRTP packet. The packet format is: + + Byte 0: CRTP header (port, channel, and reserved bits) + Bytes 1-N: CRTP payload data (0 to 30 bytes) set by CRTP_MAX_DATA_SIZE in firmware + +Total packet size: 1 to 31 bytes per UDP datagram + +The payload follows standard CRTP format as defined in the Crazyflie protocol +specification. No additional framing, checksums, or encapsulation is added by +the UDP driver. + +Scan Behavior +------------- +The scan_interface() method discovers available Crazyflie devices by probing +UDP ports in sequence: + +Port Range: 19850 to 19859 (10 ports total, BASE_PORT + 0 through 9) +Scan Address: Configurable via SCAN_ADDRESS environment variable (default: 127.0.0.1) +Probe Packet: Single 0xFF byte (null CRTP packet) +Timeout: 0.1 seconds per port + +Scan Procedure: + 1. For each port in range: + - Send 0xFF probe packet to SCAN_ADDRESS:port + - Wait up to 0.1 seconds for response + - If any response received, device is present + - Add URI "udp://SCAN_ADDRESS:port" to results + +Environment Variables +--------------------- +SCAN_ADDRESS: IP address to scan for Crazyflie devices + Default: 127.0.0.1 + Example: export SCAN_ADDRESS=192.168.1.100 + + This is useful when the Crazyflie server and client run on different + hosts. The client will scan the specified address instead of localhost. + +Connection URI Format +-------------------- +udp://: + +Examples: + udp://127.0.0.1:19850 - Local simulator on port 19850 + udp://192.168.1.5:19850 - Remote device at 192.168.1.5 +""" +# changelog: +# - Complete rewrite to align with other CRTP driver implementations +# - Added dedicated _UdpReceiveThread class for asynchronous packet reception +# - Implemented functional scan_interface() that probes UDP ports 19850-19859 +# - Fixed send_packet() with null checks, proper error callbacks, and removed checksum +# - Added proper socket cleanup in close() method +# - Changed variable naming to align with other CRTP drivers and added docstrings +# - Added environment variable SCAN_ADDRESS for scan_interface() to specify target IP address +# This is useful for server and clients running on different hosts +import logging +import os +import queue import re +import socket import struct -import sys -from socket import socket +import threading +from urllib.parse import urlparse -from .crtpdriver import CRTPDriver from .crtpstack import CRTPPacket from .exceptions import WrongUriType -if sys.version_info < (3,): - import Queue as queue -else: - import queue +from cflib.crtp.crtpdriver import CRTPDriver __author__ = 'Bitcraze AB' __all__ = ['UdpDriver'] +logger = logging.getLogger(__name__) + +_BASE_PORT = 19850 +_NR_OF_PORTS_TO_SCAN = 10 +_SCAN_TIMEOUT = 0.1 + class UdpDriver(CRTPDriver): + """ Crazyflie UDP link driver """ def __init__(self): - None + """ Create the link driver """ + CRTPDriver.__init__(self) + self.socket = None + self.addr = None + self.uri = '' + self.link_error_callback = None + self.in_queue = None + self._thread = None + self.needs_resending = False - def connect(self, uri, linkQualityCallback, linkErrorCallback): - # check if the URI is a radio URI + def connect(self, uri, radio_link_statistics_callback, link_error_callback): + """ + Connect the link driver to a specified URI of the format: + udp://: + + The callback for radio link statistics is not used by the UDP driver. + The callback from link_error_callback will be called when an error + occurs with an error message. + """ if not re.search('^udp://', uri): - raise WrongUriType('Not an UDP URI') + raise WrongUriType('Not a UDP URI') - self.queue = queue.Queue() - self.socket = socket(socket.AF_INET, socket.SOCK_DGRAM) - self.addr = ('localhost', 7777) - self.socket.connect(self.addr) + if self.socket is not None: + raise Exception('Link already open!') - # Add this to the server clients list - self.socket.sendto('\xFF\x01\x01\x01', self.addr) + self.uri = uri + self.link_error_callback = link_error_callback - def receive_packet(self, time=0): - data, addr = self.socket.recvfrom(1024) + parse = urlparse(uri) - if data: - data = struct.unpack('b' * (len(data) - 1), data[0:len(data) - 1]) - pk = CRTPPacket() - pk.port = data[0] - pk.data = data[1:] - return pk + # Prepare the inter-thread communication queue + self.in_queue = queue.Queue() - try: - if time == 0: - return self.rxqueue.get(False) - elif time < 0: - while True: - return self.rxqueue.get(True, 10) - else: - return self.rxqueue.get(True, time) - except queue.Empty: - return None + self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.addr = (parse.hostname, parse.port) + self.socket.connect(self.addr) + + # Launch the comm thread + self._thread = _UdpReceiveThread(self.socket, self.in_queue, + link_error_callback) + self._thread.start() + + 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 wait == 0: + try: + return self.in_queue.get(False) + except queue.Empty: + return None + elif wait < 0: + try: + return self.in_queue.get(True) + except queue.Empty: + return None + else: + try: + return self.in_queue.get(True, wait) + except queue.Empty: + return None def send_packet(self, pk): - raw = (pk.port,) + struct.unpack('B' * len(pk.data), pk.data) + """ Send the packet pk though the link """ + if self.socket is None: + return - cksum = 0 - for i in raw: - cksum += i + try: + raw = (pk.header,) + struct.unpack('B' * len(pk.data), pk.data) + data = struct.pack('B' * len(raw), *raw) + self.socket.send(data) + except Exception as e: + if self.link_error_callback: + self.link_error_callback( + 'UdpDriver: Could not send packet to Crazyflie\n' + 'Exception: %s' % e) - cksum %= 256 + def pause(self): + self._thread.stop() + self._thread = None - data = ''.join(chr(v) for v in (raw + (cksum,))) + def restart(self): + if self._thread: + return - # print tuple(data) - self.socket.sendto(data, self.addr) + self._thread = _UdpReceiveThread(self.socket, self.in_queue, + self.link_error_callback) + self._thread.start() def close(self): - # Remove this from the server clients list - self.socket.sendto('\xFF\x01\x02\x02', self.addr) + """ Close the link. """ + # Stop the comm thread + if self._thread: + self._thread.stop() + + # Close the UDP socket + try: + if self.socket: + self.socket.close() + except Exception as e: + logger.info('Could not close {}'.format(e)) + self.socket = None + + # Clear callbacks + self.link_error_callback = None + + def get_status(self): + return 'No information available' def get_name(self): return 'udp' - def scan_interface(self, address): - return [] + def scan_interface(self, address=None): + """ Scan interface for Crazyflies """ + found = [] + scan_address = os.getenv('SCAN_ADDRESS', '127.0.0.1') + + for i in range(_NR_OF_PORTS_TO_SCAN): + port = _BASE_PORT + i + try: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.settimeout(_SCAN_TIMEOUT) + s.connect((scan_address, port)) + s.send(b'\xFF') # Null CRTP packet as probe + s.recv(1024) + # Got a response, Crazyflie is available + s.close() + found.append(['udp://{}:{}'.format(scan_address, port), '']) + except socket.timeout: + s.close() + except Exception: + pass + + return found + + +# Receive thread +class _UdpReceiveThread(threading.Thread): + """ + UDP link receiver thread used to read data from the + UDP socket. """ + + def __init__(self, sock, inQueue, link_error_callback): + """ Create the object """ + threading.Thread.__init__(self, name='UdpReceiveThread') + self._socket = sock + self._in_queue = inQueue + self._sp = False + self._link_error_callback = link_error_callback + self.daemon = True + + def stop(self): + """ Stop the thread """ + self._sp = True + try: + self.join() + except Exception: + pass + + def run(self): + """ Run the receiver thread """ + self._socket.settimeout(1.0) + + while True: + if self._sp: + break + try: + packet = self._socket.recv(1024) + data = struct.unpack('B' * len(packet), packet) + if len(data) > 0: + pk = CRTPPacket(header=data[0], data=data[1:]) + self._in_queue.put(pk) + except socket.timeout: + pass + 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())) diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py index 475a70ba6..349e58d5c 100644 --- a/cflib/crtp/usbdriver.py +++ b/cflib/crtp/usbdriver.py @@ -20,28 +20,22 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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. 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'] @@ -58,25 +52,25 @@ def __init__(self): self.cfusb = None self.uri = '' self.link_error_callback = None - self.link_quality_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, link_quality_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 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 - 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 - 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 @@ -84,9 +78,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: @@ -107,7 +98,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, + radio_link_statistics_callback, link_error_callback) self._thread.start() @@ -159,7 +150,7 @@ def restart(self): return self._thread = _UsbReceiveThread(self.cfusb, self.in_queue, - self.link_quality_callback, + self.radio_link_statistics_callback, self.link_error_callback) self._thread.start() @@ -215,9 +206,7 @@ class _UsbReceiveThread(threading.Thread): Radio link receiver thread used to read data from the Crazyradio USB driver. """ - # RETRYCOUNT_BEFORE_DISCONNECT = 10 - - def __init__(self, cfusb, inQueue, link_quality_callback, + def __init__(self, cfusb, inQueue, radio_link_statistics_callback, link_error_callback): """ Create the object """ threading.Thread.__init__(self) @@ -225,7 +214,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.radio_link_statistics_callback = radio_link_statistics_callback def stop(self): """ Stop the thread """ 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 c0452e9e0..80d12a321 100644 --- a/cflib/drivers/cfusb.py +++ b/cflib/drivers/cfusb.py @@ -20,17 +20,24 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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. """ import logging import os +import platform +import libusb_package import usb +import usb.core + +try: + if os.environ['CRTP_PCAP_LOG'] is not None: + from cflib.crtp.pcap import PCAPLog +except KeyError: + pass __author__ = 'Bitcraze AB' __all__ = ['CfUsb'] @@ -40,19 +47,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(): """ @@ -62,17 +56,19 @@ 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): - ret.append(d) + if os.name == 'nt': + import usb.backend.libusb0 as libusb0 + + backend = libusb0.get_backend() 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, ] + backend = libusb_package.get_libusb1_backend() + + devices = usb.core.find(idVendor=USB_VID, idProduct=USB_PID, find_all=1, + backend=backend) + if devices: + for d in devices: + if d.manufacturer == 'Bitcraze AB': + ret.append(d) return ret @@ -94,18 +90,22 @@ def __init__(self, device=None, devid=0): except Exception: self.dev = None + 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 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.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, @@ -117,13 +117,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() - self.handle.reset() - else: - if self.dev: - self.dev.reset() + if self.dev: + usb.util.dispose_resources(self.dev) self.handle = None self.dev = None @@ -134,32 +129,43 @@ 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: _send_vendor_setup(self.handle, 0x01, 0x01, 0, ()) + def _log_packet(self, receive, id, packet): + try: + if os.environ['CRTP_PCAP_LOG'] is not None: + if len(packet) > 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 - 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): - 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: @@ -174,24 +180,18 @@ def receive_packet(self): # supported, but the "normal" case will work. pass + self._log_packet(True, self.dev.port_number, dataIn) + return dataIn # 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 6668fa27a..884f97cbd 100644 --- a/cflib/drivers/crazyradio.py +++ b/cflib/drivers/crazyradio.py @@ -20,17 +20,24 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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. """ import logging import os +import platform +import libusb_package import usb +import usb.core + +try: + if os.environ['CRTP_PCAP_LOG'] is not None: + from cflib.crtp.pcap import PCAPLog +except KeyError: + pass __author__ = 'Bitcraze AB' __all__ = ['Crazyradio'] @@ -42,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 @@ -54,18 +62,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): """ @@ -73,21 +69,20 @@ def _find_devices(serial=None): """ ret = [] - if pyusb1: - for d in usb.core.find(idVendor=0x1915, idProduct=0x7777, find_all=1, - backend=pyusb_backend): + if os.name == 'nt': + import usb.backend.libusb0 as libusb0 + + backend = libusb0.get_backend() + else: + backend = libusb_package.get_libusb1_backend() + + devices = usb.core.find(idVendor=0x1915, idProduct=0x7777, find_all=1, + backend=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 @@ -121,6 +116,7 @@ def __init__(self, device=None, devid=0, serial=None): self.current_channel = None self.current_address = None self.current_datarate = None + self.devid = devid if device is None: try: @@ -136,16 +132,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+' @@ -153,7 +143,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+') - # 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 @@ -165,14 +157,18 @@ def __init__(self, device=None, devid=0, serial=None): self.set_ard_bytes(32) self.set_ack_enable(True) + def _log_packet(self, receive, devid, address, channel, packet): + try: + if os.environ['CRTP_PCAP_LOG'] is not None: + if len(packet) > 0: + logger = PCAPLog.instance() + logger.logCRTP(logger.LinkType.RADIO, receive, devid, address, channel, packet) + except KeyError: + pass + def close(self): - if (pyusb1 is False): - if self.handle: - self.handle.releaseInterface() - self.handle.reset() - else: - if self.dev: - self.dev.reset() + if self.dev: + usb.util.dispose_resources(self.dev) self.handle = None self.dev = None @@ -256,7 +252,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 @@ -276,23 +272,27 @@ 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 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 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, + self.devid, + self.current_address, + self.current_channel, + dataOut + ) except usb.USBError: pass @@ -306,24 +306,25 @@ 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 # 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/localization/__init__.py b/cflib/localization/__init__.py new file mode 100644 index 000000000..9456d06e1 --- /dev/null +++ b/cflib/localization/__init__.py @@ -0,0 +1,35 @@ +# -*- 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, see . +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 +from .param_io import ParamFileManager + +__all__ = [ + 'LighthouseBsVector', + 'LighthouseSweepAngleAverageReader', + 'LighthouseSweepAngleReader', + 'LighthouseConfigFileManager', + 'LighthouseConfigWriter', + 'ParamFileManager'] diff --git a/cflib/localization/_ippe.py b/cflib/localization/_ippe.py new file mode 100644 index 000000000..d09cae277 --- /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 should 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..51472dea0 --- /dev/null +++ b/cflib/localization/ippe_cf.py @@ -0,0 +1,117 @@ +# -*- 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 + +from collections import namedtuple + +import numpy as np +import numpy.typing as npt + +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 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 + camera's image. + + This is a wrapper function to convert from/to CF coordinate system/array style + + :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) + 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)) diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py new file mode 100644 index 000000000..67e035964 --- /dev/null +++ b/cflib/localization/lighthouse_bs_vector.py @@ -0,0 +1,164 @@ +# -*- 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 . +from __future__ import annotations + +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 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: 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, + 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: 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 + :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: list[float]) -> 'LighthouseBsVector': + """ + 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) + + @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) -> float: + """ + Lightouse V1 horizontal sweep angle + """ + return self._lh_v1_horiz_angle + + @property + def lh_v1_vert_angle(self) -> float: + """ + Lightouse V1 vertical sweep angle + """ + return self._lh_v1_vert_angle + + @property + 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) -> float: + """ + Lightouse V2 second sweep angle + """ + return self._lh_v1_horiz_angle + math.asin(self._q() * math.tan(self.T)) + + @property + def cart(self) -> npt.NDArray[np.float32]: + """ + A normalized vector in cartesian coordinates + """ + 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) + + +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 + """ + 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_config_manager.py b/cflib/localization/lighthouse_config_manager.py new file mode 100644 index 000000000..49bf86683 --- /dev/null +++ b/cflib/localization/lighthouse_config_manager.py @@ -0,0 +1,228 @@ +# -*- 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 manage lighthouse system configuration (geometry and calibration data). +""" +import time + +import yaml + +from cflib.crazyflie.mem import LighthouseBsCalibration +from cflib.crazyflie.mem import LighthouseBsGeometry +from cflib.crazyflie.mem import LighthouseMemHelper + + +class LighthouseConfigFileManager: + TYPE_ID = 'type' + TYPE = 'lighthouse_system_configuration' + VERSION_ID = 'version' + 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={}, system_type=SYSTEM_TYPE_V2): + 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.SYSTEM_TYPE_ID: system_type, + 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_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 = {} + + 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, result_system_type + + +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, system_type=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 + 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') + 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 + + 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() + + 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, 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: + 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, success): + if not success: + 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 diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py new file mode 100644 index 000000000..ef33b49e0 --- /dev/null +++ b/cflib/localization/lighthouse_geometry_solver.py @@ -0,0 +1,451 @@ +# -*- 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 + +from cflib.localization.lighthouse_types import LhBsCfPoses +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import Pose + + +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 = 100 + + 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 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 = {} + + # Indicates if the solution converged (True). + # If it did not converge, the solution is probably not good enough to use + self.success = False + + +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: LhBsCfPoses, 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 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. + :param sensor_positions: Sensor positions (3D), in the CF reference frame + :return: an instance of LighthouseGeometrySolution + """ + solution = LighthouseGeometrySolution() + + 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._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( + 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) + + # 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=solution.max_nr_iter, + args=args) + + cls._condense_results(result, solution, matched_samples) + return solution + + @classmethod + def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample]) -> 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: 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(): + 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, 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 + + @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][:, 3:] - cfs_full[index_angle_pair_to_cf][:, 3:], 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 + :param 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 _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, + 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 + + @classmethod + def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution, + matched_samples: list[LhCfPoseSample]) -> None: + 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 in range(len(matched_samples) - 1): + solution.cf_poses.append(cls._params_to_pose(cf_poses[i], solution)) + + # Extract base station pose estimates + 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.success = lsq_result.success + + # 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_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(solution.estimated_errors) + + @classmethod + def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]) -> dict[str, float]: + error_per_bs = {} + errors = [] + 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) + 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_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py new file mode 100644 index 000000000..70778b1a9 --- /dev/null +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -0,0 +1,431 @@ +# -*- 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 + +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 LhException +from cflib.localization.lighthouse_types import Pose + + +ArrayFloat = npt.NDArray[np.float64] + + +class BsPairIds(NamedTuple): + """A type representing the ids of a pair of base stations""" + bs1: int + bs2: int + + +class BsPairPoses(NamedTuple): + """A type representing the poses of a pair of base stations""" + bs1: Pose + bs2: Pose + + +class LighthouseInitialEstimator: + """ + 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. + """ + + OUTLIER_DETECTION_ERROR = 0.5 + + @classmethod + 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. + + 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 lighthouse angles. + :param sensor_positions: An array with the sensor positions on the lighthouse deck (3D, CF ref frame) + :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, 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) + 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 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 + 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), cleaned_matched_samples + + @classmethod + 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. + + 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[BsPairIds, list[list[ArrayFloat]]] = {} + for sample in matched_samples: + 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) + 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, BsPairPoses], + 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 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()) + + 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 = BsPairIds(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: 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. + + 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, 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, BsPairPoses] = {} + 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 + + poses: dict[int, Pose] = {} + ids = sorted(solutions.keys()) + first = ids[0] + is_sample_valid = True + + for other in ids[1:]: + pair_ids = BsPairIds(first, other) + expected = bs_positions[pair_ids] + + 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 + + if is_sample_valid: + result.append(poses) + cleaned_matched_samples.append(sample) + + return result, cleaned_matched_samples + + @classmethod + def _choose_solutions(cls, solutions_1: BsPairPoses, solutions_2: 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 + best = BsPairPoses(Pose(), Pose()) + success = True + + 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 + best = BsPairPoses(solution_1, solution_2) + + if min_dist > cls.OUTLIER_DETECTION_ERROR: + success = False + + return success, best + + @classmethod + def _find_most_likely_positions(cls, position_permutations: dict[BsPairIds, + 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, 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[ArrayFloat]] = [[], [], [], []] + + 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 _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. + """ + + 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[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. + """ + 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]) -> BsPairPoses: + """ + Convert the two ippe solutions from the base station reference frame to the CF reference frame + """ + 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) + + 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 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 + 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: + 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() + + # run through the list of samples until we manage to find them all + remaining = len(to_find) + while remaining > 0: + 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()) + + # 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_id in unknown: + # The unknown BS pose in the CF reference frame (of this sample) + unknown_cf = bs_poses_in_sample[bs_id] + # Finally we can calculate the BS pose in the global reference frame + 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) + + # 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 LhException('Can not link positions between all base stations') + + remaining = len(to_find) + + @classmethod + 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 = 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) + + 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: dict[int, Pose]) -> list[Pose]: + """ + Find the pose of the Crazyflie in all samples, based on the base station poses. + """ + cf_poses: list[Pose] = [] + + 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(cls._avarage_poses(poses)) + + return cf_poses + + @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 new file mode 100644 index 000000000..afe26fb8f --- /dev/null +++ b/cflib/localization/lighthouse_sample_matcher.py @@ -0,0 +1,64 @@ +# -*- 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 + +from cflib.localization.lighthouse_types import LhCfPoseSample +from cflib.localization.lighthouse_types import 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 measured approximately 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.020, + min_nr_of_bs_in_match: int = 0) -> list[LhCfPoseSample]: + """ + Aggregate samples close in time into lists + """ + + 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 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 new file mode 100644 index 000000000..8c653c4a5 --- /dev/null +++ b/cflib/localization/lighthouse_sweep_angle_reader.py @@ -0,0 +1,157 @@ +# -*- 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 . +from cflib.localization import LighthouseBsVector +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors + + +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, LighthouseBsVectors(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 LighthouseBsVectors(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/cflib/localization/lighthouse_system_aligner.py b/cflib/localization/lighthouse_system_aligner.py new file mode 100644 index 000000000..3ec964a0a --- /dev/null +++ b/cflib/localization/lighthouse_system_aligner.py @@ -0,0 +1,130 @@ +# -*- 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 . +from __future__ import annotations + +import numpy as np +import numpy.typing as npt +import scipy.optimize + +from cflib.localization.lighthouse_types import Pose + + +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]) -> 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 + 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 and the transformation + """ + 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, transformation + + @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 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) + + # 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/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 new file mode 100644 index 000000000..941bc5e74 --- /dev/null +++ b/cflib/localization/lighthouse_types.py @@ -0,0 +1,188 @@ +# -*- 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 + +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 + + +class Pose: + """ Holds the full pose (position and orientation) of an object. + Contains functionality to convert between various formats.""" + + _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: + # Rotation as a matix + 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) + + @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: + """ + Get the rotation matrix of the pose + """ + return self._R_matrix + + @property + def rot_vec(self) -> npt.NDArray: + """ + Get the rotation vector of the pose + """ + 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: + """ + Get the translation vector of the pose + """ + return self._t_vec + + @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 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 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) + + 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.""" + timestamp: float + base_station_id: int + 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: + - 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, angles_calibrated: dict[int, LighthouseBsVectors] = None) -> None: + self.timestamp: float = timestamp + + # 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 = {} + + +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.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), + (_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/cflib/localization/param_io.py b/cflib/localization/param_io.py new file mode 100644 index 000000000..7d337f46f --- /dev/null +++ b/cflib/localization/param_io.py @@ -0,0 +1,86 @@ +# -*- 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 PersistentParamState + + +class ParamFileManager(): + """Reads and writes parameter configurations from file""" + TYPE_ID = 'type' + TYPE = 'persistent_param_state' + 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): + 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/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 f075bcc8a..28f258a2e 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 @@ -42,17 +40,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""" @@ -63,8 +57,8 @@ def __init__(self, crazyflie, default_height=0.3): """ Construct an instance of a MotionCommander - :param crazyflie: a Crazyflie or SyncCrazyflie instance - :param default_height: the default height to fly at + :param crazyflie: A Crazyflie or SyncCrazyflie instance + :param default_height: The default height to fly at """ if isinstance(crazyflie, SyncCrazyflie): self._cf = crazyflie.cf @@ -80,13 +74,13 @@ 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. - :param height: the height (meters) to hover at. None uses the default + :param height: The height (meters) to hover at. None uses the default height set when constructed. - :param velocity: the velocity (meters/second) when taking off + :param velocity: The velocity (meters/second) when taking off :return: """ if self._is_flying: @@ -123,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): @@ -136,8 +133,8 @@ def left(self, distance_m, velocity=VELOCITY): """ Go left - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, distance_m, 0.0, velocity) @@ -146,8 +143,8 @@ def right(self, distance_m, velocity=VELOCITY): """ Go right - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, -distance_m, 0.0, velocity) @@ -156,8 +153,8 @@ def forward(self, distance_m, velocity=VELOCITY): """ Go forward - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(distance_m, 0.0, 0.0, velocity) @@ -166,8 +163,8 @@ def back(self, distance_m, velocity=VELOCITY): """ Go backwards - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(-distance_m, 0.0, 0.0, velocity) @@ -176,8 +173,8 @@ def up(self, distance_m, velocity=VELOCITY): """ Go up - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, distance_m, velocity) @@ -186,8 +183,8 @@ def down(self, distance_m, velocity=VELOCITY): """ Go down - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, -distance_m, velocity) @@ -197,7 +194,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 +208,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 @@ -263,7 +260,7 @@ def move_distance(self, distance_x_m, distance_y_m, distance_z_m, :param distance_x_m: The distance to travel along the X-axis (meters) :param distance_y_m: The distance to travel along the Y-axis (meters) :param distance_z_m: The distance to travel along the Z-axis (meters) - :param velocity: the velocity of the motion (meters/second) + :param velocity: The velocity of the motion (meters/second) :return: """ distance = math.sqrt(distance_x_m * distance_x_m + @@ -350,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): """ @@ -359,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): """ @@ -372,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): """ @@ -385,11 +382,11 @@ 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): + 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 @@ -398,10 +395,11 @@ def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m): :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, 0.0) + 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: diff --git a/cflib/positioning/position_hl_commander.py b/cflib/positioning/position_hl_commander.py index 5d8c58fb2..e11aa8751 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 @@ -52,17 +50,19 @@ 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 - :param crazyflie: a Crazyflie or SyncCrazyflie instance + :param crazyflie: A Crazyflie or SyncCrazyflie instance :param x: Initial position, x :param y: Initial position, y :param z: Initial position, z - :param default_velocity: the default velocity to use - :param default_height: the default height to fly at + :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: Landing height (zero if not specified); for landing on objects off the ground """ if isinstance(crazyflie, SyncCrazyflie): self._cf = crazyflie.cf @@ -73,21 +73,28 @@ def __init__(self, crazyflie, self._default_height = default_height self._controller = controller + self._activate_controller() + 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() + + 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. Do not call this function if you use the with keyword. Take off is done automatically when the context is created. - :param height: the height (meters) to hover at. None uses the default + :param height: The height (meters) to hover at. None uses the default height set when constructed. - :param velocity: the velocity (meters/second) when taking off + :param velocity: The velocity (meters/second) when taking off :return: """ if self._is_flying: @@ -96,11 +103,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) @@ -109,7 +118,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=DEFAULT): """ Go straight down and turn off the motors. @@ -120,10 +129,11 @@ def land(self, velocity=DEFAULT): :return: """ if self._is_flying: - duration_s = self._z / self._velocity(velocity) - self._hl_commander.land(0, duration_s) + 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) - self._z = 0.0 + self._z = landing_height self._hl_commander.stop() self._is_flying = False @@ -139,8 +149,8 @@ def left(self, distance_m, velocity=DEFAULT): """ Go left - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, distance_m, 0.0, velocity) @@ -149,8 +159,8 @@ def right(self, distance_m, velocity=DEFAULT): """ Go right - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, -distance_m, 0.0, velocity) @@ -159,8 +169,8 @@ def forward(self, distance_m, velocity=DEFAULT): """ Go forward - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(distance_m, 0.0, 0.0, velocity) @@ -169,8 +179,8 @@ def back(self, distance_m, velocity=DEFAULT): """ Go backwards - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(-distance_m, 0.0, 0.0, velocity) @@ -179,8 +189,8 @@ def up(self, distance_m, velocity=DEFAULT): """ Go up - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, distance_m, velocity) @@ -189,8 +199,8 @@ def down(self, distance_m, velocity=DEFAULT): """ Go down - :param distance_m: the distance to travel (meters) - :param velocity: the velocity of the motion (meters/second) + :param distance_m: The distance to travel (meters) + :param velocity: The velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, -distance_m, velocity) @@ -206,7 +216,7 @@ def move_distance(self, distance_x_m, distance_y_m, distance_z_m, :param distance_x_m: The distance to travel along the X-axis (meters) :param distance_y_m: The distance to travel along the Y-axis (meters) :param distance_z_m: The distance to travel along the Z-axis (meters) - :param velocity: the velocity of the motion (meters/second) + :param velocity: The velocity of the motion (meters/second) :return: """ @@ -223,7 +233,7 @@ def go_to(self, x, y, z=DEFAULT, velocity=DEFAULT): :param x: X coordinate :param y: Y coordinate :param z: Z coordinate - :param velocity: the velocity (meters/second) + :param velocity: The velocity (meters/second) :return: """ @@ -260,9 +270,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,22 +277,10 @@ 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') - 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: @@ -296,3 +291,15 @@ 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): + """ + 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/cflib/resources/binaries/nrf51-s110-and-bl.bin b/cflib/resources/binaries/nrf51-s110-and-bl.bin new file mode 100644 index 000000000..74542ecb0 Binary files /dev/null and b/cflib/resources/binaries/nrf51-s110-and-bl.bin differ 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 3102bc630..36d9cb2c2 100644 --- a/cflib/utils/callbacks.py +++ b/cflib/utils/callbacks.py @@ -20,13 +20,12 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 """ +from threading import Event __author__ = 'Bitcraze AB' __all__ = ['Caller'] @@ -50,5 +49,32 @@ 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) + + +class Syncer: + """A class to create synchronous behavior 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() diff --git a/cflib/utils/encoding.py b/cflib/utils/encoding.py new file mode 100644 index 000000000..9f43fab7e --- /dev/null +++ b/cflib/utils/encoding.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +# -*- 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 struct + +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 + 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] + + +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 + 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 + + +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(quat_n[i]) > abs(quat_n[i_largest]): + i_largest = i + 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 = 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 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/cflib/utils/param_file_helper.py b/cflib/utils/param_file_helper.py new file mode 100644 index 000000000..08ae21c35 --- /dev/null +++ b/cflib/utils/param_file_helper.py @@ -0,0 +1,57 @@ +# -*- 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 threading import Event + +from cflib.crazyflie import Crazyflie +from cflib.localization.param_io import ParamFileManager + + +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') + + 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): + 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._persistent_stored_callback) + self.persistent_sema.wait() + if not self.success: + break + return self.success diff --git a/cflib/utils/power_switch.py b/cflib/utils/power_switch.py new file mode 100644 index 000000000..b53054309 --- /dev/null +++ b/cflib/utils/power_switch.py @@ -0,0 +1,124 @@ +# -*- 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 time + +import cflib.crtp +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.radiodriver import RadioManager + + +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 + 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 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. + Same as turning off the platform with the power button.""" + 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(self.BOOTLOADER_CMD_SYSOFF) + + def stm_power_up(self): + """ Power up (boot) the STM MCU and decks.""" + self._send(self.BOOTLOADER_CMD_SYSON) + + 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 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, data=[]): + if not self.link: + packet = [0xf3, 0xfe, cmd] + data + + 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 + + time.sleep(0.01) + + cr.close() + + if not success: + raise Exception( + 'Failed to connect to Crazyflie at {}'.format(self.uri)) + else: + + # send command (will be repeated until acked) + pk = CRTPPacket(0xFF, [0xfe, cmd] + data) + 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)) diff --git a/cflib/utils/reset_estimator.py b/cflib/utils/reset_estimator.py new file mode 100644 index 000000000..42aaa59d9 --- /dev/null +++ b/cflib/utils/reset_estimator.py @@ -0,0 +1,83 @@ +# -*- 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 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...', end='\r') + + 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('Waiting for estimator to find position...success!') + break diff --git a/examples/scan.py b/cflib/utils/uri_helper.py similarity index 55% rename from examples/scan.py rename to cflib/utils/uri_helper.py index 06e0c8143..280be06f7 100644 --- a/examples/scan.py +++ b/cflib/utils/uri_helper.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python # -*- coding: utf-8 -*- # # || ____ _ __ @@ -6,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2014 Bitcraze AB +# Copyright (C) 2021 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -19,20 +20,30 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 scans for available Crazyflies and lists them. -""" -import cflib.crtp +# 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 + -# Initiate the low level drivers -cflib.crtp.init_drivers(enable_debug_driver=False) +def address_from_env(env='CFLIB_URI', default=0xE7E7E7E7E7) -> int: + try: + uri = os.environ[env] + except KeyError: + return default -print('Scanning interfaces for Crazyflies...') -available = cflib.crtp.scan_interfaces() -print('Crazyflies found:') -for i in available: - print(i[0]) + # 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 diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml deleted file mode 100644 index 28e1aff22..000000000 --- a/docs/_data/menu.yml +++ /dev/null @@ -1,16 +0,0 @@ -- page_id: home -- title: Install Instructions - subs: - - {page_id: install_from_source} - - {page_id: usd_permissions} -- title: User guides - subs: - - {page_id: python_api} -- title: Explanation Functionalities - subs: - - {page_id: crazyradio_lib} -- title: Development - subs: - - {page_id: testing} - - {page_id: matlab} - - {page_id: eeprom} diff --git a/docs/api/cflib/index.md b/docs/api/cflib/index.md new file mode 100644 index 000000000..9adba0119 --- /dev/null +++ b/docs/api/cflib/index.md @@ -0,0 +1,6 @@ +--- +title: API reference for CFLib +page_id: api_reference +--- + +{% include_relative_generated index.md_raw info="Placeholder for generated file. Run `tb build-docs` to generate content." %} diff --git a/docs/api/index.md b/docs/api/index.md new file mode 100644 index 000000000..c934bc138 --- /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 %} diff --git a/docs/api/template/text.mako b/docs/api/template/text.mako new file mode 100644 index 000000000..6f3ccc43e --- /dev/null +++ b/docs/api/template/text.mako @@ -0,0 +1,230 @@ +## Define mini-templates for each portion of the doco. + +<%! + def title(heading, name): + return name.split('.')[-1] +%> + +<%! + def page_id(name): + return name.replace('.', '-') +%> + +<%! + 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] + + # we do not support linkin to builtins + index = link.find('.ext') + if index > 0: + return name + + return f'[{name}]({link})' +%> + +<%! + import re + def deflist(s): + param_re = r':param (.*):|@param (\w+)' + 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(): + 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 + + 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: + desc += line + else: + out += line + '\n' + + if in_param: + out += f'{desc} |' + '\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/docs/eeprom.md b/docs/development/eeprom.md similarity index 95% rename from docs/eeprom.md rename to docs/development/eeprom.md index 74a77c394..e61abe21a 100644 --- a/docs/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/index.md b/docs/development/index.md new file mode 100644 index 000000000..c51c3de8c --- /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 %} diff --git a/docs/matlab.md b/docs/development/matlab.md similarity index 94% rename from docs/matlab.md rename to docs/development/matlab.md index 15a94cb54..b5d5a8677 100644 --- a/docs/matlab.md +++ b/docs/development/matlab.md @@ -3,15 +3,15 @@ title: Using Matlab with the Crazyflie API page_id: matlab --- +**Note that these are old instructions and they might not work anymore** -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 +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 @@ -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/uart_communication.md b/docs/development/uart_communication.md new file mode 100644 index 000000000..35068c423 --- /dev/null +++ b/docs/development/uart_communication.md @@ -0,0 +1,110 @@ +--- +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. + +> 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). + +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 Gdn -- Raspberry Pi Gdn (if not both connected to same powersource) + +## Crazyflie Firmware + +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 + +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 to print the log variables of the Crazyflie on the Raspberry pi + +```python +#!/usr/bin/env python3 + +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 + +# choose the serial URI that matches the setup serial device +URI = 'serial://ttyAMA0' + +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) + 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.') + 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 + +* 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 diff --git a/docs/development/wireshark.md b/docs/development/wireshark.md new file mode 100644 index 000000000..05ce220e9 --- /dev/null +++ b/docs/development/wireshark.md @@ -0,0 +1,67 @@ +--- +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. + +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) + +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 + +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 `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 + +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 .py +$ wireshark filename.pcap +``` +and on Windows based computers in a shell window: + +```bash +> set CRTP_PCAP_LOG=filename.pcap +> python3 .py +> wireshark filename.pcap +``` +To generate a PCAP file and open it with Wireshark. You can also use the text based `tshark` tool, and you can add a filter, for instance, only shoow CRTP port 8 (Highlevel setpoint): + +```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/docs/crazyradio_lib.md b/docs/functional-areas/crazyradio_lib.md similarity index 96% rename from docs/crazyradio_lib.md rename to docs/functional-areas/crazyradio_lib.md index 25c1b747d..2c0be82bc 100644 --- a/docs/crazyradio_lib.md +++ b/docs/functional-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. @@ -27,7 +27,7 @@ contains data. This is an example on how to use the lib: -``` {.python} +``` .python import crazyradio cr = crazyradio.Crazyradio() @@ -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) diff --git a/docs/functional-areas/index.md b/docs/functional-areas/index.md new file mode 100644 index 000000000..651eecc6d --- /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 %} diff --git a/docs/images/wireshark-dissector.png b/docs/images/wireshark-dissector.png new file mode 100644 index 000000000..ad9482333 Binary files /dev/null and b/docs/images/wireshark-dissector.png differ diff --git a/docs/index.md b/docs/index.md index bfefafcdf..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.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 diff --git a/docs/install_from_source.md b/docs/install_from_source.md deleted file mode 100644 index 8c3cacf05..000000000 --- a/docs/install_from_source.md +++ /dev/null @@ -1,37 +0,0 @@ ---- -title: Installing 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` - - -* [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` - -To verify the installation, connect the crazyflie and run an example: `python examples/basiclog` diff --git a/docs/installation/index.md b/docs/installation/index.md new file mode 100644 index 000000000..b308ff986 --- /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 %} diff --git a/docs/installation/install.md b/docs/installation/install.md new file mode 100644 index 000000000..97d7bd2a2 --- /dev/null +++ b/docs/installation/install.md @@ -0,0 +1,85 @@ +--- +title: Installation +page_id: install +--- + +## Requirements + +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. + +## Platform Prerequisites + +### Ubuntu/Linux + +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/). + +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 + +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" +``` + +## Installation Methods + +### From PyPI (Recommended) + +```bash +pip install cflib +``` + +### From Source (Development) + +#### Clone the repository +```bash +git clone https://github.com/bitcraze/crazyflie-lib-python.git +cd crazyflie-lib-python +``` + +#### Install cflib from source +```bash +pip install -e . +``` + +#### Uninstall cflib +```bash +pip uninstall cflib +``` + +## Development Tools (Optional) + +### Pre-commit hooks +If you want help maintaining Python coding standards, you can install hooks that verify your style at commit time: + +```bash +pip install pre-commit +cd crazyflie-lib-python +pre-commit install +pre-commit run --all-files +``` + +This will run the lint checkers defined in `.pre-commit-config-yaml` on your proposed changes. + +### 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**: The toolbelt is an optional way of running tests and reduces the work needed to maintain your Python environment. diff --git a/docs/usb_permissions.md b/docs/installation/usb_permissions.md similarity index 68% rename from docs/usb_permissions.md rename to docs/installation/usb_permissions.md index e88959e7c..5fc5cf71b 100644 --- a/docs/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: diff --git a/docs/python_api.md b/docs/python_api.md deleted file mode 100644 index 0c9e3c4a3..000000000 --- a/docs/python_api.md +++ /dev/null @@ -1,414 +0,0 @@ ---- -title: The Crazyflie Python API -page_id: python_api ---- - -In order to easily use and control the Crazyflie there\'s an 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 - [logging](https://www.bitcraze.io/docs/crazyflie-firmware/master/ctrp_log/) or - [parameters](https://www.bitcraze.io/docs/crazyflie-firmware/master/ctrp_parameters/) - -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` -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) ---------------------------------- - -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: - -- \%%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 - -Variables and logging ---------------------- - -The library supports setting up logging configurations that are used for -logging variables from the firmware. Each log configuration contains a -number of variables that should be logged as well as a time period (in -ms) of how often the data should be sent back to the host. Once the log -configuration is added to the firmware the firmware will automatically -send back the data at every period. These configurations are used in the -following way: - -- Connect to the Crazyflie (log configurations needs a TOC to work) -- Create a log configuration that contains a number of variables to - log and a period at which they should be logged -- Add the log configuration. This will also validate that the log - configuration is sane (i.e uses a supported period and all variables - 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 - -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 minimum period of a for a log configuration is multiples of 10ms - -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 -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. -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: - -- Register parameter updated callbacks at any time in your application -- Connect to your Crazyflie (this will download the parameter TOC) -- Request updates for all the parameters -- The library will call all the callbacks registered -- The host can now write parameters that will be forwarded to the - firmware -- For each write all the callbacks registered for this parameter will - be called back - -Variable and parameter names ----------------------------- - -All names of parameters and log variables use the same structure: -`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 -`stabilizer`. - -There\'s a limit of 28 chars in total and here are some examples: - -- stabilizer.roll -- stabilizer.pitch -- pm.vbat -- imu\_tests.MPU6050 -- pid\_attitide.pitch\_kd - -Utilities -========= - -Callbacks ---------- - -All callbacks are handled using the `Caller` class that contains the -following methods: - -``` {.python} - add_callback(cb) - """ Register cb as a new callback. Will not register duplicates. """ - - remove_callback(cb) - """ Un-register cb from the callbacks """ - - 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.""" -``` - -Connection- and link-callbacks -============================== - -Operations on the link and connection will return directly and will call -the following callbacks when events occur: - -``` {.python} - # Called on disconnect, no matter the reason - disconnected = Caller() - # Called on unintentional disconnect only - connection_lost = Caller() - # Called when the first packet in a new link is received - link_established = Caller() - # Called when the user requests a connection - connection_requested = Caller() - # Called when the link is established and the TOCs (that are not cached) - # have been downloaded - connected = Caller() - # Called if establishing of the link fails (i.e times out) - connection_failed = Caller() - # Called for every packet received - packet_received = Caller() - # Called for every packet sent - packet_sent = Caller() - # Called when the link driver updates the link quality measurement - link_quality_updated = Caller() -``` - -To register for callbacks the following is used: - -``` {.python} - crazyflie = Crazyflie() - crazyflie.connected.add_callback(crazyflie_connected) -``` - -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 -available interfaces (currently the debug and radio interface). - -``` {.python} - cflib.crtp.init_drivers() - available = cflib.crtp.scan_interfaces() - for i in available: - 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 -object: - -``` {.python} - crazyflie = Crazyflie() - crazyflie.connected.add_callback(crazyflie_connected) - crazyflie.open_link("radio://0/10/250K") -``` - -Then you can use the following to close the link again: - -``` {.python} - crazyflie.close_link() -``` - -Sending control commands -======================== - -The control commands are not implemented as parameters, instead they -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: - -``` {.python} - roll = 0.0 - pitch = 0.0 - yawrate = 0 - thrust = 0 - crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) -``` - -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. - -Parameters -========== - -The parameter framework is used to read and set parameters. This -functionality should be used when: - -- The parameter is not changed by the Crazyflie but by the client -- The parameter is not read periodically - -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 -parameter is set using: - -``` {.python} - param_name = "group.name" - param_value = 3 - crazyflie.param.set_value(param_name, param_value) -``` - -The parameter reading is done using callbacks. When a parameter is -updated from the host (using the code above) the parameter will be read -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} - add_update_callback(group, name=None, cb=None) - """ - Add a callback for a specific parameter name or group. If not name is 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. - """ - - request_param_update(complete_name) - """ Request an update of the value for the supplied parameter. """ - - set_value(complete_name, value) - """ Set the value for the supplied parameter. """ -``` - -Here\'s an example of how to use the calls. - -``` {.python} - crazyflie.param.add_update_callback(group="group", name="name", param_updated_callback) - - def param_updated_callback(name, value): - print "%s has value %d" % (name, value) -``` - -Logging -======= - -The logging framework is used to enable the \"automatic\" sending of -variable values at specified intervals to the client. This functionality -should be used when: - -- The variable is changed by the Crazyflie and not by the client -- The variable is updated at high rate and you want to read the value - periodically - -If this is not the case then the parameter framework should be used -instead. - -The API to create and get information from LogConfig: - -``` {.python} - # Called when new logging data arrives - data_received_cb = Caller() - # Called when there's an error - error_cb = Caller() - # Called when the log configuration is confirmed to be started - started_cb = Caller() - # Called when the log configuration is confirmed to be added - added_cb = Caller() - - add_variable(name, fetch_as=None) - """Add a new variable to the configuration. - - name - Complete 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 - (i.e the type of the fetched variable is the same as it's stored in the - Crazyflie).""" - - start() - """Start the logging for this entry""" - - stop() - """Stop the logging for this entry""" - - delete() - """Delete this entry in the Crazyflie""" -``` - -The API for the log in the Crazyflie: - -``` {.python} - add_config(logconf) - """Add a log configuration to the logging framework. - - When doing this the contents of the log configuration will be validated - and listeners for new log configurations will be notified. When - validating the configuration the variables are checked against the TOC - to see that they actually exist. If they don't then the configuration - cannot be used. Since a valid TOC is required, a Crazyflie has to be - connected when calling this method, otherwise it will fail.""" -``` - -To create a logging configuration the following can be used: - -``` {.python} - logconf = LogConfig(name="Logging", period_in_ms=100) - logconf.add_variable("group1.name1", "float") - logconf.add_variable("group1.name2", "uint8_t") - logconf.add_variable("group2.name1", "int16_t") -``` - -The datatype is the transferred datatype, it will be converted from -internal type to transferred type before transfers: - -- float -- 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 - in one packet at the expense of precision. - -The logging cannot be started until your are connected to a Crazyflie: - -``` {.python} - # Callback called when the connection is established to the Crazyflie - def connected(link_uri): - crazyflie.log.add_config(logconf) - - if logconf.valid: - logconf.data_received_cb.add_callback(data_received_callback) - logconf.error_cb.add_callback(logging_error) - logconf.start() - else: - print "One or more of the variables in the configuration was not found in log TOC. No logging will be possible." - - 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 -``` - -Examples -======== - -The examples are now placed in the repository in the examples folder. diff --git a/docs/testing.md b/docs/testing.md deleted file mode 100644 index cd05d12bd..000000000 --- a/docs/testing.md +++ /dev/null @@ -1,26 +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 environmet. - -### 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.) diff --git a/docs/user-guides/index.md b/docs/user-guides/index.md new file mode 100644 index 000000000..643028801 --- /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 %} diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md new file mode 100644 index 000000000..0b5887525 --- /dev/null +++ b/docs/user-guides/python_api.md @@ -0,0 +1,551 @@ +--- +title: The Crazyflie Python API explanation +page_id: python_api +--- + +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 + [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_log/) or + [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_parameters/) +- [Automated documentation for Python API](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/) +- Examples: See the [example folder of the repository](https://github.com/bitcraze/crazyflie-lib-python/tree/master/examples) for basic examples, or [crazyflie-demos](https://github.com/bitcraze/crazyflie-demos) for more advanced demos. + + +## 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` +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 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 built up of the +following: InterfaceType://InterfaceId/InterfaceChannel/InterfaceSpeed + +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 + 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 + +The library supports setting up logging configurations that are used for +logging variables from the firmware. Each log configuration contains a +number of variables that should be logged as well as a time period (in +ms) of how often the data should be sent back to the host. Once the log +configuration is added to the firmware the firmware will automatically +send back the data at every period. These configurations are used in the +following way: + +- Connect to the Crazyflie (log configurations needs a TOC to work) +- Create a log configuration that contains a number of variables to + log and a period at which they should be logged +- Add the log configuration. This will also validate that the log + configuration is sane (i.e uses a supported period and all variables + 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 + be called with a time-stamp and the data + +There\'s are few limitations that need to be taken into account: + +- 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 + +### 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 changed by the firmware, like setting regulation parameters and +reading out if the power-on self-tests passed. Parameters should only be +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 +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: + +- Register parameter updated callbacks at any time in your application +- Connect to your Crazyflie (this will download the parameter TOC) +- Request updates for all the parameters +- The library will call all the callbacks registered +- The host can now write parameters that will be forwarded to the + firmware +- 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: +`group.name`. + +The group should be used to bundle together logical groups, like +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: + +- stabilizer.roll +- stabilizer.pitch +- pm.vbat +- imu\_tests.MPU6050 +- pid\_attitide.pitch\_kd + +## Utilities + +### Callbacks + +All callbacks are handled using the `Caller` class that contains the +following methods: + +``` python + add_callback(cb) + """ Register cb as a new callback. Will not register duplicates. """ + + remove_callback(cb) + """ Un-register cb from the callbacks """ + + call(*args) + """ Call the callbacks registered with the arguments args """ +``` +## 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() + """ Search for and initialize link drivers.""" +``` + +### 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 immediately and will call +the following callbacks when events occur: + +``` python + # Called on disconnect, no matter the reason + disconnected = Caller() + # Called on unintentional disconnect only + connection_lost = Caller() + # Called when the first packet in a new link is received + link_established = Caller() + # Called when the user requests a connection + connection_requested = Caller() + # 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 + packet_received = Caller() + # Called for every packet sent + packet_sent = Caller() + # Called when the link driver updates the link quality measurement + link_quality_updated = Caller() +``` + +To register for callbacks the following is used: + +``` python + crazyflie = Crazyflie() + crazyflie.connected.add_callback(crazyflie_connected) +``` + +## 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 +available interfaces (currently the debug and radio interface). + +``` python + cflib.crtp.init_drivers() + available = cflib.crtp.scan_interfaces() + for i in available: + print "Interface with URI [%s] found and name/comment [%s]" % (i[0], i[1]) +``` + +Opening and closing a communication link is done by using the Crazyflie +object: + +``` python + crazyflie = Crazyflie() + crazyflie.connected.add_callback(crazyflie_connected) + crazyflie.open_link("radio://0/10/2M") +``` + +Then you can use the following to close the link again: + +``` python + crazyflie.close_link() +``` + +## 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): +``` + +To send a new control set-point use the following: + +``` python + roll = 0.0 + pitch = 0.0 + yawrate = 0 + thrust = 10001 + crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) +``` + +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 is in degree/seconds. + +This command will set the attitude controller setpoint for the next +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 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 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). + +### 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: + +``` +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 framework to find out what other functions you can use. + +## Parameters + +The parameter framework is used to read and set parameters. This +functionality should be used when: + +- The parameter is not changed by the Crazyflie but by the client +- The parameter is not read periodically + +If this is not the case then the logging framework should be used +instead. + +To set a parameter you must be connected to the Crazyflie. A +parameter is set using: + +``` python + param_name = "group.name" + param_value = 3 + crazyflie.param.set_value(param_name, param_value) +``` + +The parameter reading is done using callbacks. When a parameter is +updated from the host (using the code above) the parameter will be read +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 + add_update_callback(group, name=None, cb=None) + """ + 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. + """ + + request_param_update(complete_name) + """ Request an update of the value for the supplied parameter. """ + + set_value(complete_name, value) + """ Set the value for the supplied parameter. """ +``` + +Here\'s an example of how to use the calls. + +``` python + crazyflie.param.add_update_callback(group="group", name="name", param_updated_callback) + + def param_updated_callback(name, value): + 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 +variable values at specified intervals to the client. This functionality +should be used when: + +- The variable is changed by the Crazyflie and not by the client +- The variable is updated at high rate and you want to read the value + periodically + +If this is not the case then the parameter framework should be used +instead. + +The API to create and get information from LogConfig: + +``` python + # Called when new logging data arrives + data_received_cb = Caller() + # Called when there's an error + error_cb = Caller() + # Called when the log configuration is confirmed to be started + started_cb = Caller() + # Called when the log configuration is confirmed to be added + added_cb = Caller() + + add_variable(name, fetch_as=None) + """Add a new variable to the configuration. + + 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 type will be used + (i.e the type of the fetched variable is the same as it's stored in the + Crazyflie).""" + + start() + """Start the logging for this entry""" + + stop() + """Stop the logging for this entry""" + + delete() + """Delete this entry in the Crazyflie""" +``` + +The API for the log in the Crazyflie: + +``` python + add_config(logconf) + """Add a log configuration to the logging framework. + + When doing this the contents of the log configuration will be validated + and listeners for new log configurations will be notified. When + validating the configuration the variables are checked against the TOC + to see that they actually exist. If they don't then the configuration + cannot be used. Since a valid TOC is required, a Crazyflie has to be + connected when calling this method, otherwise it will fail.""" +``` + +To create a logging configuration the following can be used: + +``` python + logconf = LogConfig(name="Logging", period_in_ms=100) + logconf.add_variable("group1.name1", "float") + logconf.add_variable("group1.name2", "uint8_t") + logconf.add_variable("group2.name1", "int16_t") +``` + +The datatype is the transferred datatype, it will be converted from +internal type to transferred type before transfers: + +- float +- 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 variables + in one packet at the expense of precision. + +The logging cannot be started until your are connected to a Crazyflie: + +``` python + # Callback called when the connection is established to the Crazyflie + def connected(link_uri): + crazyflie.log.add_config(logconf) + + if logconf.valid: + logconf.data_received_cb.add_callback(data_received_callback) + logconf.error_cb.add_callback(logging_error) + logconf.start() + else: + print "One or more of the variables in the configuration was not found in log TOC. No logging will be possible." + + 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 +``` + +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 configuration may +be added. + +``` python + crazyflie.log.add_config([logconf1, logconfig2]) +``` + +## Synchronous API + +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. + +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 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 fails, + # 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` 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 +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, 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 +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. + +``` python + with SyncCrazyflie(URI) as scf: + 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 + # When leaving this "with" section, the connection is automatically closed +``` diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md new file mode 100644 index 000000000..f468a8cb6 --- /dev/null +++ b/docs/user-guides/sbs_connect_log_param.md @@ -0,0 +1,530 @@ +--- +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. + +## 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. + +```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 +from cflib.utils import uri_helper +``` + +* 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: + +```python +# URI to the Crazyflie to connect to +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 + +Write the following in the script: +```python +if __name__ == '__main__': + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + 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: + +```python +def simple_connect(): + + print("Yeah, I'm connected! :D") + time.sleep(3) + print("Now I will disconnect :'(") +``` + + + +### Run the script + +Make sure your Crazyflie is on and your Crazyradio is connected to your computer, and that the Crazyflie is not connected to anything else (like the cfclient). +Now run the script in your terminal: + + +`python3 connect_log_param.py` + +You are supposed to see the following in the terminal: +```python +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 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. If everything is peachy, please continue to the next part! + +```python +import logging +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 simple_connect(): + + print("Yeah, I'm connected! :D") + time.sleep(3) + print("Now I will disconnect :'(") + +if __name__ == '__main__': + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + 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 means to read out logging variables to the script! + + + +### More imports + +Now we need to add imports to the top of the script connect_log_param.py: + + ```python + ... +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: + +```python +# 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 : + ```python + ... + cflib.crtp.init_drivers() + + 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: + ```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, logconf) 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_ to `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: + ```python +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger + +# 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) + +def simple_log(scf, logconf): + + with SyncLogger(scf, logconf) 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 + cflib.crtp.init_drivers() + + 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 `def simple_log(..)`, begin a new function: + +```python +def simple_log_async(scf, logconf): + cf = scf.cf + cf.log.add_config(logconf) +``` + +Here you add the logging configuration 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: +```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 is 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() +``` + +## 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 this: + +```python +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger + +# 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) + +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) # Your possibility to interact with the Crazyflie + logconf.stop() + +(...) + +if __name__ == '__main__': + # 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') + 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__`. + +```python + group = "stabilizer" + name = "estimator" +``` + +### Start the function + +Start the following function above `def log_stab_callback(...)`: + +```python +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`: + +```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) + +``` +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` + + +### Set a parameter + +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) +``` + +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 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 + 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 + +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') + +# 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 + cflib.crtp.init_drivers() + + 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: + +```python +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 +``` + +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! + + 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 new file mode 100644 index 000000000..a30164627 --- /dev/null +++ b/docs/user-guides/sbs_motion_commander.md @@ -0,0 +1,697 @@ +--- +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 flow deck. 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](/docs/user-guides/sbs_connect_log_param.md). + + +## Get the script started + +Since you should have installed cflib in the previous step by step tutorial, you are all ready to go now. Open up a new python script called `motion_flying.py`. First you will start by adding the following to 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') + +logging.basicConfig(level=logging.ERROR) + +if __name__ == '__main__': + + cflib.crtp.init_drivers() + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: +``` + +This probably all looks pretty familiar, except for one 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 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 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 + 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: +```python +def param_deck_flow(_, value_str): + value = int(value_str) + print(value) + if value: + deck_attached_event.set() + print('Deck is attached!') + else: + print('Deck is NOT attached!') +``` + +Note that the value type that the `param_deck_flow()` gets 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`: +```python +... +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 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 +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') + +deck_attached_event = Event() + +logging.basicConfig(level=logging.ERROR) + +def param_deck_flow(_, value_str): + value = int(value_str) + print(value) + if value: + deck_attached_event.set() + print('Deck is attached!') + else: + print('Deck is NOT attached!') + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + 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 + +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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + # Arm the Crazyflie + scf.cf.supervisor.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. + +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) +``` + +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 immediately taking off is that the Motion commander is initialized with a takeoff that starts 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 has 0.3 meters height as default but this can of course be changed. + +Change the following lines in `take_off_simple(...)`: +```python + with MotionCommander(scf) as mc: + time.sleep(3) + 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 +```python + with MotionCommander(scf, default_height = DEFAULT_HEIGHT) as mc: +``` + +Add the variable underneath `URI`: +```python +DEFAULT_HEIGHT = 0.5 +``` + + +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') + +DEFAULT_HEIGHT = 0.5 + +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) + + +def param_deck_flow(name, value_str): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + 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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + # Arm the Crazyflie + scf.cf.supervisor.send_arming_request(True) + time.sleep(1.0) + + 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)`: + +```python +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: + +```python + 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 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 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') + +DEFAULT_HEIGHT = 0.5 + +deck_attached_event = Event() +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_str): + ... + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + 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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + # Arm the Crazyflie + scf.cf.supervisor.send_arming_request(True) + time.sleep(1.0) + + 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` + + +```python + 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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + 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: + + + +```python +def log_pos_callback(timestamp, data, logconf): + print(data) +``` + +NOW: Make global variable which is a list called `position_estimate` and fill this 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 :)* +```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') + +DEFAULT_HEIGHT = 0.5 + +deck_attached_event = Event() + +logging.basicConfig(level=logging.ERROR) + +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 position_estimate + position_estimate[0] = data['stateEstimate.x'] + position_estimate[1] = data['stateEstimate.y'] + + +def param_deck_flow(name, value_str): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + 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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + # Arm the Crazyflie + scf.cf.supervisor.send_arming_request(True) + time.sleep(1.0) + + 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_linear_simple(scf)`: + +```python +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: + +```python +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + 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 its 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 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 +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') + +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +deck_attached_event = Event() + +logging.basicConfig(level=logging.ERROR) + +position_estimate = [0, 0] + + +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + 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_str): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + 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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + # Arm the Crazyflie + scf.cf.supervisor.send_arming_request(True) + time.sleep(1.0) + + 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 + + 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 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! +```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') + +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +deck_attached_event = Event() + +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): + ... + +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(_, value_str): + value = int(value_str) + print(value) + if value: + deck_attached_event.set() + print('Deck is attached!') + else: + print('Deck is NOT attached!') + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + 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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + # Arm the Crazyflie + scf.cf.supervisor.send_arming_request(True) + time.sleep(1.0) + + 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](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/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md new file mode 100644 index 000000000..3be574e97 --- /dev/null +++ b/docs/user-guides/sbs_swarm_interface.md @@ -0,0 +1,477 @@ +--- +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](https://www.bitcraze.io/documentation/system/positioning/) (Lighthouse, Loco or MoCap) that is able to provide data for the position estimation of the Crazyflies. You can also use the Flow Deck, 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 have completed the following steps before you start with the tutorial: + +* Have some basic experience with Python +* Followed the [Crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) +* Set up the positioning system of your choice +* 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're 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 + +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 + # URIs in a swarm using the same radio must also be on the same channel +} + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: +``` + +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. + +## 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 in all the LEDs of each copter lighting up simultaneously. + +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) + +def deactivate_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + activate_led_bit_mask(scf) + time.sleep(2) + deactivate_led_bit_mask(scf) +``` +`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 `light_check()` is going to be called through the `parallel_safe()` method which will execute it 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_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + activate_led_bit_mask(scf) + time.sleep(2) + deactivate_led_bit_mask(scf) + time.sleep(2) + + +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 + # URIs in a swarm using the same radio must also be on the same channel +} + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + swarm.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 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(light_check) + swarm.reset_estimators() +``` + +## Step 3: Taking off and Landing Sequentially +Make sure that your positioning system of your choice is set up and ready to go. Now we are going to execute the first 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 + + 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) +``` + +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) +``` +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_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + activate_led_bit_mask(scf) + time.sleep(2) + deactivate_led_bit_mask(scf) + time.sleep(2) + +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 + # URIs in a swarm using the same radio must also be on the same channel +} + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + print('Connected to Crazyflies') + swarm.parallel_safe(light_check) + 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. + +## 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 independently executes 4 relative movements (+X, +Y, -X, -Y) tracing a 1×1 m square and returning to its own starting position. Since the movements are relative, all copters fly the same square path simultaneously regardless of where they started, and you should see the entire swarm move in sync. + +```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_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + ... + +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 + # URIs in a swarm using the same radio must also be on the same channel +] + +if __name__ == '__main__': + cflib.crtp.init_drivers() + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + 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) +``` + +## Step 6: Performing a flight with different arguments +You can also feed different arguments to each Crazyflie in the swarm. The `parallel_safe()`, `parallel()`, and `sequential()` methods accept an optional `args_dict` parameter which is a dictionary that maps each URI to a list of extra arguments to pass to the per-Crazyflie function. + +You create this dictionary yourself, with the URI (radio address) as the key and a list of arguments as the value: + +```python +my_args = { + URI0: [optional_param0_cf0, optional_param1_cf0], + URI1: [optional_param0_cf1, optional_param1_cf1], + ... +} + +swarm.parallel_safe(my_function, args_dict=my_args) +``` + +The arguments in each list are appended to the `scf` argument when the function is called, so `my_function(scf, optional_param0, optional_param1)` is what gets executed for each copter. + +In the example below, four copters are placed in a square (pay attention to the order of the Crazyflies) and each one executes a different movement sequence. The dictionary `seq_args` maps each URI to its own sequence, which is then passed as the `sequence` argument to `run_sequence`. + + +```python +# The layout of the positions (1m apart from each other): +# <------ 1 m -----> +# 0 1 +# ^ ^ +# |Y | +# | | +# +------> X 1 m +# | +# | +# 3 2 . + + +uris = [ + 'radio://0/20/2M/E7E7E7E701', + 'radio://0/20/2M/E7E7E7E702', + 'radio://0/20/2M/E7E7E7E703', + 'radio://0/20/2M/E7E7E7E704', +] + +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.syncCrazyflie import SyncCrazyflie + + +def activate_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 255) + +def deactivate_led_bit_mask(scf): + scf.cf.param.set_value('led.bitmask', 0) + +def light_check(scf): + ... + +def take_off(scf): + ... + +def land(scf): + ... + + +def run_square_sequence(scf): + ... + +uris = ... + +# 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 = ... + +sequence1 = ... + +sequence2 = ... + +sequence3 = ... + +seq_args = ... + +def run_sequence(scf: 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(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 [crazyflie-demos](https://github.com/bitcraze/crazyflie-demos) repository. diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 000000000..985db5a25 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,16 @@ +# Examples + +This folder contains basic examples for getting started with the Crazyflie Python library. They cover the core building blocks: connecting, logging, parameters, and simple autonomous flight. + +## What's here + +| Folder | Description | +|--------|-------------| +| `logging/` | Read sensor data and log variables from the Crazyflie | +| `parameters/` | Read and write parameters, including persistent parameter storage | +| `autonomy/` | Basic autonomous flight using different commanders | +| `step-by-step/` | Step-by-step tutorial scripts that accompany the [user guides](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/user-guides/) | + +## More examples + +For more complete and advanced demos see the **[crazyflie-demos](https://github.com/bitcraze/crazyflie-demos)** repository. diff --git a/examples/autonomousSequence.py b/examples/autonomy/autonomous_sequence.py similarity index 55% rename from examples/autonomousSequence.py rename to examples/autonomy/autonomous_sequence.py index f4d37fac4..9ea47b186 100644 --- a/examples/autonomousSequence.py +++ b/examples/autonomy/autonomous_sequence.py @@ -19,18 +19,16 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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, 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 @@ -38,72 +36,38 @@ 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.reset_estimator import reset_estimator # 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 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), + (0.0, 0.0, 0.0, 0), ] -def wait_for_position_estimator(scf): - print('Waiting for estimator to find position...') +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 - 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') + print(f'take off at {position[2]}') - 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) + 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): @@ -127,6 +91,13 @@ def start_position_printing(scf): def run_sequence(scf, sequence): cf = scf.cf + # Arm the Crazyflie + cf.supervisor.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): @@ -137,15 +108,18 @@ 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) 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) - # start_position_printing(scf) + start_position_printing(scf) run_sequence(scf, sequence) diff --git a/examples/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py similarity index 68% rename from examples/autonomous_sequence_high_level.py rename to examples/autonomy/autonomous_sequence_high_level.py index 9132a7917..45365e0b6 100644 --- a/examples/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 @@ -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 @@ -30,18 +28,20 @@ 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 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.reset_estimator import reset_estimator # 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 trajectory to fly # See https://github.com/whoenig/uav_trajectories for a tool to generate @@ -62,81 +62,13 @@ ] -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') - - 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 = [] total_duration = 0 for row in trajectory: @@ -145,22 +77,28 @@ def upload_trajectory(cf, trajectory_id, trajectory): 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)) + trajectory_mem.trajectory.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)) + 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_mem.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 - commander.takeoff(1.0, 2.0) + # Arm the Crazyflie + cf.supervisor.send_arming_request(True) + time.sleep(1.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_position=True, relative_yaw=relative_yaw) time.sleep(duration) commander.land(0.0, 2.0) time.sleep(2) @@ -168,15 +106,24 @@ def run_sequence(cf, trajectory_id, duration): if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) + 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: 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)) reset_estimator(cf) - run_sequence(cf, trajectory_id, duration) + run_sequence(cf, trajectory_id, duration, relative_yaw=args.relative_yaw) diff --git a/examples/motion_commander_demo.py b/examples/autonomy/motion_commander_demo.py similarity index 87% rename from examples/motion_commander_demo.py rename to examples/autonomy/motion_commander_demo.py index 6040d1ffc..abb6194a6 100644 --- a/examples/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. @@ -41,18 +39,23 @@ 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) 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: + # Arm the Crazyflie + scf.cf.supervisor.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/basicLedmemSync.py b/examples/basicLedmemSync.py deleted file mode 100644 index e8155331b..000000000 --- a/examples/basicLedmemSync.py +++ /dev/null @@ -1,66 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, 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/250K' - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - # Set virtual mem effect effect - cf.param.set_value('ring.effect', '13') - - # Get LED memory and write to it - mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED) - if len(mem) > 0: - mem[0].leds[0].set(r=0, g=100, b=0) - mem[0].leds[3].set(r=0, g=0, b=100) - mem[0].leds[6].set(r=100, g=0, b=0) - mem[0].leds[9].set(r=100, g=100, b=100) - mem[0].write_data(None) - - time.sleep(2) diff --git a/examples/basicLedparamSync.py b/examples/basicLedparamSync.py deleted file mode 100644 index 654d88fb7..000000000 --- a/examples/basicLedparamSync.py +++ /dev/null @@ -1,74 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, 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 -parameters that control the LED-ring, -it has been tested with (and designed for) the LED-ring deck. - -Change the URI variable to your Crazyflie configuration. -""" -import logging -import time - -import cflib.crtp -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie - -URI = 'radio://0/80/250K' - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - - with SyncCrazyflie(URI) as scf: - cf = scf.cf - - # Set solid color effect - cf.param.set_value('ring.effect', '7') - # Set the RGB values - cf.param.set_value('ring.solidRed', '100') - cf.param.set_value('ring.solidGreen', '0') - cf.param.set_value('ring.solidBlue', '0') - time.sleep(2) - - # Set black color effect - cf.param.set_value('ring.effect', '0') - time.sleep(1) - - # Set fade to color effect - cf.param.set_value('ring.effect', '14') - # Set fade time i seconds - cf.param.set_value('ring.fadeTime', '1.0') - # Set the RGB values in one uint32 0xRRGGBB - cf.param.set_value('ring.fadeColor', '0x0000A0') - time.sleep(1) - cf.param.set_value('ring.fadeColor', '0x00A000') - time.sleep(1) - cf.param.set_value('ring.fadeColor', '0xA00000') - time.sleep(1) diff --git a/examples/cfbridge.py b/examples/cfbridge.py deleted file mode 100755 index 1d7677e6f..000000000 --- a/examples/cfbridge.py +++ /dev/null @@ -1,147 +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/Reciever) -modes. Thus, data is only recieved 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.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - print('Connecting to %s' % link_uri) - - # Try to connect to the Crazyflie - self._cf.open_link(link_uri) - - # Variable used to keep main loop occupied until disconnect - self.is_connected = True - - 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(' 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 - - link_uri = 'radio://0/' + str(channel) + '/2M' - le = RadioBridge(link_uri) # (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. - try: - while le.is_connected: - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/erase-ow.py b/examples/erase-ow.py deleted file mode 100644 index 705ceabe9..000000000 --- a/examples/erase-ow.py +++ /dev/null @@ -1,148 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects to the first Crazyflie found, looks for -EEPROM memories and lists its contents. -""" -import logging -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement - -# Only output errors from the logging framework -logging.basicConfig(level=logging.INFO) - - -class EEPROMExample: - """ - Simple example listing the EEPROMs found and lists its contents. - """ - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # Connect some callbacks from the Crazyflie API - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - print('Connecting to %s' % link_uri) - - # Try to connect to the Crazyflie - self._cf.open_link(link_uri) - - # Variable used to keep main loop occupied until disconnect - self.is_connected = True - - def _connected(self, link_uri): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - print('Connected to %s' % link_uri) - - mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) - print('Found {} 1-wire memories'.format(len(mems))) - if len(mems) > 0: - print('Erasing memory {}'.format(mems[0].id)) - mems[0].erase(self._data_written) - - def _data_written(self, mem, addr): - print('Data written, reading back...') - mem.update(self._data_updated) - - def _data_updated(self, mem): - print('Updated id={}'.format(mem.id)) - print('\tType : {}'.format(mem.type)) - print('\tSize : {}'.format(mem.size)) - print('\tValid : {}'.format(mem.valid)) - print('\tName : {}'.format(mem.name)) - print('\tVID : 0x{:02X}'.format(mem.vid)) - print('\tPID : 0x{:02X}'.format(mem.pid)) - print('\tPins : 0x{:02X}'.format(mem.pins)) - print('\tElements : ') - - for key in mem.elements: - print('\t\t{}={}'.format(key, mem.elements[key])) - - self._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 froma 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)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - self.is_connected = False - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - self.is_connected = False - - -if __name__ == '__main__': - 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) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = EEPROMExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') - - # The Crazyflie lib doesn't contain anything to keep the application alive, - # so this is where your application should do something. In our case we - # are just waiting until we are disconnected. - try: - while le.is_connected: - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/flash-memory.py b/examples/flash-memory.py deleted file mode 100644 index ff7551464..000000000 --- a/examples/flash-memory.py +++ /dev/null @@ -1,249 +0,0 @@ -# -*- coding: utf-8 -*- -# -# Copyright (C) 2015 Danilo Bargen -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Flash the DS28E05 EEPROM via CRTP. -""" -import datetime -import os -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement - - -class NotConnected(RuntimeError): - pass - - -class Flasher(object): - """ - A class that can flash the DS28E05 EEPROM via CRTP. - """ - - def __init__(self, link_uri): - self._cf = Crazyflie() - self._link_uri = link_uri - - # Add some callbacks from the Crazyflie API - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - # Initialize variables - self.connected = False - - # Public methods - - def connect(self): - """ - Connect to the crazyflie. - """ - print('Connecting to %s' % self._link_uri) - self._cf.open_link(self._link_uri) - - def disconnect(self): - print('Disconnecting from %s' % self._link_uri) - self._cf.close_link() - - def wait_for_connection(self, timeout=10): - """ - Busy loop until connection is established. - - Will abort after timeout (seconds). Return value is a boolean, whether - connection could be established. - - """ - start_time = datetime.datetime.now() - while True: - if self.connected: - return True - now = datetime.datetime.now() - if (now - start_time).total_seconds() > timeout: - return False - time.sleep(0.5) - - def search_memories(self): - """ - Search and return list of 1-wire memories. - """ - if not self.connected: - raise NotConnected() - return self._cf.mem.get_mems(MemoryElement.TYPE_1W) - - # Callbacks - - def _connected(self, link_uri): - print('Connected to %s' % link_uri) - self.connected = True - - def _disconnected(self, link_uri): - print('Disconnected from %s' % link_uri) - self.connected = False - - def _connection_failed(self, link_uri, msg): - print('Connection to %s failed: %s' % (link_uri, msg)) - self.connected = False - - def _connection_lost(self, link_uri, msg): - print('Connection to %s lost: %s' % (link_uri, msg)) - self.connected = False - - -def choose(items, title_text, question_text): - """ - Interactively choose one of the items. - """ - print(title_text) - - for i, item in enumerate(items, start=1): - print('%d) %s' % (i, item)) - print('%d) Abort' % (i + 1)) - - selected = input(question_text) - try: - index = int(selected) - except ValueError: - index = -1 - if not (index - 1) in range(len(items)): - print('Aborting.') - return None - - return items[index - 1] - - -def scan(): - """ - Scan for Crazyflie and return its URI. - """ - - # Initiate the low level drivers - cflib.crtp.init_drivers(enable_debug_driver=False) - - # Scan for Crazyflies - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - 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' - 'BLE support for this to work.\n' - 'See ' - 'https://github.com/bitcraze/crazyflie-clients-python/issues/166\n' - '###\n') - - # Initialize flasher - flasher = Flasher(radio_uri) - - def abort(): - flasher.disconnect() - sys.exit(1) - - # Connect to Crazyflie - flasher.connect() - connected = flasher.wait_for_connection() - if not connected: - print('Connection failed.') - abort() - - # Search for memories - mems = flasher.search_memories() - if not mems: - print('No memories found.') - abort() - mem = choose(mems, 'Available memories:', 'Select memory: ') - if mem is None: - print('Aborting.') - abort() - - # Print information about memory - print('You selected the following memory:') - print(' Name: %s' % mem.name) - print(' Vendor ID: 0x%X' % mem.vid) - print(' Memory ID: 0x%X' % mem.pid) - print(' Pins: 0x%X' % mem.pins) - print(' Elements: %s' % mem.elements) - - # Ask for new information - print('Please specify what information to write. If you just press enter, ' - 'the value will not be changed.') - - # Vendor ID - vid_input = input('New vendor ID: ') - if vid_input != '': - try: - vid = int(vid_input, 0) - if not 0 <= vid <= 0xff: - raise ValueError() - except ValueError: - print('Invalid vendor ID. Please specify a number between 0x00 ' - 'and 0xff.') - abort() - else: - mem.vid = vid - - # Memory ID - pid_input = input('New memory ID: ') - if pid_input != '': - try: - pid = int(pid_input, 0) - if not 0 <= pid <= 0xff: - raise ValueError() - except ValueError: - print('Invalid memory ID. Please specify a number between 0x00 ' - 'and 0xff.') - abort() - else: - mem.pid = pid - - # Callback function when data has been written - def data_written(mem, addr): - print('Data has been written to memory!') - flasher.disconnect() - - # We need to use os.kill because this is a callback - SIGTERM = 15 - os.kill(os.getpid(), SIGTERM) - - # Write data - sure = input('Are you sure? [y/n] ') - if sure != 'y': - print('Better safe than sorry!') - abort() - mem.write_data(data_written) - - # Timeout 10 seconds - for _ in range(10 * 2): - time.sleep(0.5) - print('Apparently data could not be written to memory... :(') - - flasher.disconnect() diff --git a/examples/flowsequenceSync.py b/examples/flowsequenceSync.py deleted file mode 100644 index fb88be857..000000000 --- a/examples/flowsequenceSync.py +++ /dev/null @@ -1,82 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2016 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, 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 runs a figure 8 -sequence. This script requires some kind of location system, it has been -tested with (and designed for) the flow deck. - -Change the URI variable to your Crazyflie configuration. -""" -import logging -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie - -URI = 'radio://0/80/250K' - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - cf.param.set_value('kalman.resetEstimation', '1') - time.sleep(0.1) - cf.param.set_value('kalman.resetEstimation', '0') - time.sleep(2) - - for y in range(10): - cf.commander.send_hover_setpoint(0, 0, 0, y / 25) - time.sleep(0.1) - - for _ in range(20): - cf.commander.send_hover_setpoint(0, 0, 0, 0.4) - time.sleep(0.1) - - for _ in range(50): - cf.commander.send_hover_setpoint(0.5, 0, 36 * 2, 0.4) - time.sleep(0.1) - - for _ in range(50): - cf.commander.send_hover_setpoint(0.5, 0, -36 * 2, 0.4) - time.sleep(0.1) - - for _ in range(20): - cf.commander.send_hover_setpoint(0, 0, 0, 0.4) - time.sleep(0.1) - - for y in range(10): - cf.commander.send_hover_setpoint(0, 0, 0, (10 - y) / 25) - time.sleep(0.1) - - cf.commander.send_stop_setpoint() diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py deleted file mode 100644 index bbba813c2..000000000 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ /dev/null @@ -1,176 +0,0 @@ -#!/usr/bin/env python3 -# Demo that makes one Crazyflie take off 30cm above the first controller found -# Using the controller trigger it is then possible to 'grab' the Crazyflie -# and to make it move. -import sys -import time - -import openvr - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger - -# URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' - -print('Opening') -vr = openvr.init(openvr.VRApplication_Other) -print('Opened') - -# Find first controller or tracker -controllerId = None -poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) -for i in range(openvr.k_unMaxTrackedDeviceCount): - if poses[i].bPoseIsValid: - device_class = vr.getTrackedDeviceClass(i) - if device_class == openvr.TrackedDeviceClass_Controller or \ - device_class == openvr.TrackedDeviceClass_GenericTracker: - controllerId = i - break - -if controllerId is None: - print('Cannot find controller or tracker, exiting') - sys.exit(1) - - -def 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 vector_substract(v0, v1): - return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] - - -def vector_add(v0, v1): - return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]] - - -def run_sequence(scf): - cf = scf.cf - - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount) - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - setpoint = [-1*pose[2][3], -1*pose[0][3], pose[1][3] + 0.3] - - grabbed = False - grab_controller_start = [0, 0, 0] - grab_setpoint_start = [0, 0, 0] - - while True: - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) - controller_state = vr.getControllerState(controllerId)[1] - - trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - - if not grabbed and trigger: - print('Grab started') - grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - grab_setpoint_start = setpoint - - if grabbed and not trigger: - print('Grab ended') - - grabbed = trigger - - if trigger: - curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - setpoint = vector_add(grab_setpoint_start, - vector_substract(curr, - grab_controller_start)) - - cf.commander.send_position_setpoint(setpoint[0], - setpoint[1], - setpoint[2], - 0) - time.sleep(0.02) - - cf.commander.send_setpoint - (0, 0, 0, 0) - # Make sure that the last packet leaves before the link is closed - # since the message queue is not flushed before closing - time.sleep(0.1) - - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - reset_estimator(scf) - run_sequence(scf) - - openvr.shutdown() diff --git a/examples/lighthouse/lighthouse_openvr_grab_color.py b/examples/lighthouse/lighthouse_openvr_grab_color.py deleted file mode 100644 index 9c5a4bfdf..000000000 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ /dev/null @@ -1,208 +0,0 @@ -#!/usr/bin/env python3 -# Demo that makes one Crazyflie take off 30cm above the first controller found -# Using the controller trigger it is then possible to 'grab' the Crazyflie -# and to make it move. -# If the Crazyflie has a ledring attached, the touchpad of the controller can -# be used to change the color of the led-ring LEDs -import colorsys -import math -import sys -import time - -import openvr - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger - -# URI to the Crazyflie to connect to -uri = 'radio://0/80/2M' - -print('Openning') -vr = openvr.init(openvr.VRApplication_Other) -print('Oppened') - -# Find first controller or tracker -controllerId = None -poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) -for i in range(openvr.k_unMaxTrackedDeviceCount): - if poses[i].bPoseIsValid: - device_class = vr.getTrackedDeviceClass(i) - if device_class == openvr.TrackedDeviceClass_Controller or\ - device_class == openvr.TrackedDeviceClass_GenericTracker: - controllerId = i - break - -if controllerId is None: - print('Cannot find controller or tracker, exiting') - sys.exit(1) - - -def 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 vector_substract(v0, v1): - return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] - - -def vector_add(v0, v1): - return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]] - - -def run_sequence(scf): - cf = scf.cf - - cf.param.set_value('ring.effect', '7') - - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - # setpoint = [-1*pose[2][3], -1*pose[0][3], pose[1][3] + 0.3] - setpoint = [0, 0, 0.5] - - grabbed = False - grab_controller_start = [0, 0, 0] - grab_setpoint_start = [0, 0, 0] - - while True: - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) - controller_state = vr.getControllerState(controllerId)[1] - - trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0) - trackpad_touched = ((controller_state.ulButtonTouched & 0x100000000) != 0) # noqa - - trackpad = (0, 0) - for i in range(openvr.k_unControllerStateAxisCount): - axis_type = vr.getInt32TrackedDeviceProperty( - controllerId, openvr.Prop_Axis0Type_Int32 + i)[0] - if axis_type == openvr.k_eControllerAxis_TrackPad: - trackpad = (controller_state.rAxis[i].x, - controller_state.rAxis[i].y) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - - if trackpad_touched: - angle = math.atan2(trackpad[1], trackpad[0]) - hue = (angle + math.pi) / (2*math.pi) - rgb = colorsys.hsv_to_rgb(hue, 1, 0.3) - - print(rgb) - - cf.param.set_value('ring.solidRed', - '{}'.format(int(rgb[0] * 255))) - cf.param.set_value('ring.solidGreen', - '{}'.format(int(rgb[1] * 255))) - cf.param.set_value('ring.solidBlue', - '{}'.format(int(rgb[2] * 255))) - - if not grabbed and trigger: - print('Grab started') - grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - grab_setpoint_start = setpoint - - if grabbed and not trigger: - print('Grab ended') - - grabbed = trigger - - if trigger: - curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - setpoint = vector_add(grab_setpoint_start, - vector_substract(curr, grab_controller_start) - ) - - cf.commander.send_position_setpoint(setpoint[0], - setpoint[1], - setpoint[2], - 0) - time.sleep(0.02) - - cf.commander.send_setpoint - (0, 0, 0, 0) - # Make sure that the last packet leaves before the link is closed - # since the message queue is not flushed before closing - time.sleep(0.1) - - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - reset_estimator(scf) - # start_position_printing(scf) - run_sequence(scf) - - openvr.shutdown() diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py deleted file mode 100644 index 8d30f053d..000000000 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ /dev/null @@ -1,204 +0,0 @@ -#!/usr/bin/env python3 -# Demo that makes two Crazyflie take off 30cm above the first controller found -# Using the controller trigger it is then possible to 'grab' the closest -# Crazyflie and to make it move. -import math -import sys -import time - -import openvr - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger - -# URI to the Crazyflie to connect to -uri0 = 'radio://0/80/2M' -uri1 = 'radio://0/80/2M/E7E7E7E701' - -print('Opening') -vr = openvr.init(openvr.VRApplication_Other) -print('Opened') - -# Find first controller or tracker -controllerId = None -poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) -for i in range(openvr.k_unMaxTrackedDeviceCount): - if poses[i].bPoseIsValid: - device_class = vr.getTrackedDeviceClass(i) - if device_class == openvr.TrackedDeviceClass_Controller or \ - device_class == openvr.TrackedDeviceClass_GenericTracker: - controllerId = i - break - -if controllerId is None: - print('Cannot find controller or tracker, exiting') - sys.exit(1) - - -def 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 vector_substract(v0, v1): - return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] - - -def vector_add(v0, v1): - return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]] - - -def vector_norm(v0): - return math.sqrt((v0[0] * v0[0]) + (v0[1] * v0[1]) + (v0[2] * v0[2])) - - -def run_sequence(scf0, scf1): - cf0 = scf0.cf - cf1 = scf1.cf - - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount) - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - setpoints = [[-1*pose[2][3], -1*pose[0][3] - 0.5, pose[1][3] + 0.3], - [-1*pose[2][3], -1*pose[0][3] + 0.5, pose[1][3] + 0.3]] - - closest = 0 - - grabbed = False - grab_controller_start = [0, 0, 0] - grab_setpoint_start = [0, 0, 0] - - while True: - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) - controller_state = vr.getControllerState(controllerId)[1] - - trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - - if not grabbed and trigger: - print('Grab started') - grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - - dist0 = vector_norm(vector_substract(grab_controller_start, - setpoints[0])) - dist1 = vector_norm(vector_substract(grab_controller_start, - setpoints[1])) - - if dist0 < dist1: - closest = 0 - else: - closest = 1 - - grab_setpoint_start = setpoints[closest] - - if grabbed and not trigger: - print('Grab ended') - - grabbed = trigger - - if trigger: - curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - setpoints[closest] = vector_add( - grab_setpoint_start, vector_substract(curr, - grab_controller_start)) - - cf0.commander.send_position_setpoint(setpoints[0][0], - setpoints[0][1], - setpoints[0][2], - 0) - cf1.commander.send_position_setpoint(setpoints[1][0], - setpoints[1][1], - setpoints[1][2], - 0) - - time.sleep(0.02) - - cf0.commander.send_setpoint(0, 0, 0, 0) - cf1.commander.send_setpoint(0, 0, 0, 0) - # Make sure that the last packet leaves before the link is closed - # since the message queue is not flushed before closing - time.sleep(0.1) - - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - - with SyncCrazyflie(uri0, cf=Crazyflie(rw_cache='./cache')) as scf0: - reset_estimator(scf0) - with SyncCrazyflie(uri1, cf=Crazyflie(rw_cache='./cache')) as scf1: - reset_estimator(scf1) - run_sequence(scf0, scf1) - - openvr.shutdown() diff --git a/examples/lighthouse/read-geometry-mem.py b/examples/lighthouse/read-geometry-mem.py deleted file mode 100644 index d6cd5b12b..000000000 --- a/examples/lighthouse/read-geometry-mem.py +++ /dev/null @@ -1,69 +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, write to the Free Software -# 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 -""" -import logging -import time - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -# Only output errors from the logging framework - -logging.basicConfig(level=logging.ERROR) - - -class ReadMem: - def __init__(self, uri): - self.got_data = False - - 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) - - print('Rquesting data') - mems[0].update(self._data_updated) - - while not self.got_data: - time.sleep(1) - - def _data_updated(self, mem): - mem.dump() - self.got_data = True - - -if __name__ == '__main__': - # URI to the Crazyflie to connect to - uri = 'radio://0/80/2M' - - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - - ReadMem(uri) diff --git a/examples/lighthouse/write-geometry-mem.py b/examples/lighthouse/write-geometry-mem.py deleted file mode 100644 index 65fd3161c..000000000 --- a/examples/lighthouse/write-geometry-mem.py +++ /dev/null @@ -1,88 +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, write to the Free Software -# 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 -""" -import logging -import time - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import LighthouseBsGeometry -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -# Only output errors from the logging framework - -logging.basicConfig(level=logging.ERROR) - - -class WriteMem: - def __init__(self, uri, bs1, bs2): - self.data_written = False - - 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) - - mems[0].geometry_data = [bs1, bs2] - - print('Writing data') - mems[0].write_data(self._data_written) - - while not self.data_written: - time.sleep(1) - - def _data_written(self, mem, addr): - self.data_written = True - print('Data written') - - -if __name__ == '__main__': - # URI to the Crazyflie to connect to - 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 = [ - [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 = [ - [24.0, 25.0, 26.0], - [27.0, 28.0, 29.0], - [30.0, 31.0, 32.0], - ] - - WriteMem(uri, bs1, bs2) diff --git a/examples/basiclog.py b/examples/logging/basiclog.py similarity index 83% rename from examples/basiclog.py rename to examples/logging/basiclog.py index e8c118c89..bf2a22b68 100644 --- a/examples/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. @@ -34,6 +32,9 @@ import cflib.crtp # noqa from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig +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) @@ -70,10 +71,15 @@ def _connected(self, link_uri): print('Connected to %s' % link_uri) # The definition of the logconfig can be made before connecting - self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=100) + 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') + # The fetch-as argument can be set to FP16 to save space in the log packet + self._lg_stab.add_variable('pm.vbat', 'FP16') # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we @@ -101,12 +107,15 @@ 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""" - print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + """Callback from a the log API when data arrives""" + print(f'[{timestamp}][{logconf.name}]: ', end='') + for name, value in data.items(): + print(f'{name}: {value:3.3f} ', end='') + print() 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 @@ -122,19 +131,10 @@ 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) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = LoggingExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + 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/logging/basiclog_sync.py similarity index 50% rename from examples/basiclogSync.py rename to examples/logging/basiclog_sync.py index cf10f01a3..529709c40 100644 --- a/examples/basiclogSync.py +++ b/examples/logging/basiclog_sync.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. @@ -37,40 +35,38 @@ 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 = 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 (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) + # Initialize the low-level drivers + cflib.crtp.init_drivers() - 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: - 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/lps_reboot_to_bootloader.py b/examples/lps_reboot_to_bootloader.py deleted file mode 100644 index 3dd4f17be..000000000 --- a/examples/lps_reboot_to_bootloader.py +++ /dev/null @@ -1,108 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects to the first Crazyflie found, and then sends the -reboot signals to 6 anchors ID from 0 to 5. The reset signals is sent -10 times in a row to make sure all anchors are reset to bootloader. -""" -import logging -import time -from threading import Thread - -import cflib -from cflib.crazyflie import Crazyflie -from lpslib.lopoanchor import LoPoAnchor - -logging.basicConfig(level=logging.ERROR) - - -class LpsRebootToBootloader: - """Example that connects to a Crazyflie and ramps the motors up/down and - the disconnects""" - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - self._cf = Crazyflie() - - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - self._cf.open_link(link_uri) - - print('Connecting to %s' % link_uri) - - def _connected(self, link_uri): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - - # Start a separate thread to do the motor test. - # Do not hijack the calling thread! - Thread(target=self._reboot_thread).start() - - def _connection_failed(self, link_uri, msg): - """Callback when connection initial connection fails (i.e no Crazyflie - at the specified address)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - - def _reboot_thread(self): - - anchors = LoPoAnchor(self._cf) - - print('Sending reboot signal to all anchors 10 times in a row ...') - for retry in range(10): - for anchor_id in range(6): - anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER) - time.sleep(0.1) - - self._cf.close_link() - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = LpsRebootToBootloader(available[0][0]) - else: - print('No Crazyflies found, cannot run example') diff --git a/examples/multiramp.py b/examples/multiramp.py deleted file mode 100644 index 076820c5e..000000000 --- a/examples/multiramp.py +++ /dev/null @@ -1,116 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects 2 Crazyflies, ramp up-down the motors and -disconnects. -""" -import logging -import time -from threading import Thread - -import cflib -from cflib.crazyflie import Crazyflie - -logging.basicConfig(level=logging.ERROR) - - -class MotorRampExample: - """Example that connects to a Crazyflie and ramps the motors up/down and - the disconnects""" - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - self._cf = Crazyflie(rw_cache='./cache') - - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - self._cf.open_link(link_uri) - - self.connected = True - - print('Connecting to %s' % link_uri) - - def _connected(self, link_uri): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - - # Start a separate thread to do the motor test. - # Do not hijack the calling thread! - Thread(target=self._ramp_motors).start() - - def _connection_failed(self, link_uri, msg): - """Callback when connection initial connection fails (i.e no Crazyflie - at the specified address)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - self.connected = False - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - self.connected = False - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - self.connected = False - - def _ramp_motors(self): - thrust_mult = 1 - thrust_step = 500 - thrust = 20000 - pitch = 0 - roll = 0 - yawrate = 0 - - # Unlock startup thrust protection - self._cf.commander.send_setpoint(0, 0, 0, 0) - - while thrust >= 20000: - self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) - time.sleep(0.1) - if thrust >= 25000: - thrust_mult = -1 - thrust += thrust_step * thrust_mult - 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) - self._cf.close_link() - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - # Connect the two Crazyflies and ramps them up-down - le0 = MotorRampExample('radio://0/70/2M') - le1 = MotorRampExample('radio://1/80/250K') - while(le0.connected or le1.connected): - time.sleep(0.1) diff --git a/examples/multiranger_pointcloud.py b/examples/multiranger_pointcloud.py deleted file mode 100644 index ddd0354e3..000000000 --- a/examples/multiranger_pointcloud.py +++ /dev/null @@ -1,365 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2018 Bitcraze AB -# -# Crazyflie Python Library -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Example script that plots the output ranges from the Multiranger and Flow -deck in a 3D plot. - -When the application is started the Crazyflie will hover at 0.3 m. The -Crazyflie can then be controlled by using keyboard input: - * Move by using the arrow keys (left/right/forward/backwards) - * Adjust the right with w/s (0.1 m for each keypress) - * Yaw slowly using a/d (CCW/CW) - * Yaw fast using z/x (CCW/CW) - -There's additional setting for (see constants below): - * Plotting the downwards sensor - * Plotting the estimated Crazyflie postition - * Max threashold for sensors - * Speed factor that set's how fast the Crazyflie moves - -The demo is ended by either closing the graph window. - -For the example to run the following hardware is needed: - * Crazyflie 2.0 - * Crazyradio PA - * Flow deck - * Multiranger deck -""" -import logging -import math -import sys - -import numpy as np -from vispy import scene -from vispy.scene import visuals -from vispy.scene.cameras import TurntableCamera - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig - -try: - from sip import setapi - setapi('QVariant', 2) - setapi('QString', 2) -except ImportError: - pass - -from PyQt5 import QtCore, QtWidgets - -logging.basicConfig(level=logging.INFO) - -URI = 'radio://0/80/2M' - -if len(sys.argv) > 1: - URI = sys.argv[1] - -# Enable plotting of Crazyflie -PLOT_CF = False -# Enable plotting of down sensor -PLOT_SENSOR_DOWN = False -# Set the sensor threashold (in mm) -SENSOR_TH = 2000 -# Set the speed factor for moving and rotating -SPEED_FACTOR = 0.3 - - -class MainWindow(QtWidgets.QMainWindow): - - def __init__(self, URI): - QtWidgets.QMainWindow.__init__(self) - - self.resize(700, 500) - self.setWindowTitle('Multi-ranger point cloud') - - self.canvas = Canvas(self.updateHover) - self.canvas.create_native() - self.canvas.native.setParent(self) - - self.setCentralWidget(self.canvas.native) - - cflib.crtp.init_drivers(enable_debug_driver=False) - self.cf = Crazyflie(ro_cache=None, rw_cache='cache') - - # Connect callbacks from the Crazyflie API - self.cf.connected.add_callback(self.connected) - self.cf.disconnected.add_callback(self.disconnected) - - # Connect to the Crazyflie - self.cf.open_link(URI) - - self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} - - self.hoverTimer = QtCore.QTimer() - self.hoverTimer.timeout.connect(self.sendHoverCommand) - self.hoverTimer.setInterval(100) - self.hoverTimer.start() - - def sendHoverCommand(self): - self.cf.commander.send_hover_setpoint( - self.hover['x'], self.hover['y'], self.hover['yaw'], - self.hover['height']) - - def updateHover(self, k, v): - if (k != 'height'): - self.hover[k] = v * SPEED_FACTOR - else: - self.hover[k] += v - - def disconnected(self, URI): - print('Disconnected') - - def connected(self, URI): - print('We are now connected to {}'.format(URI)) - - # The definition of the logconfig can be made before connecting - lpos = LogConfig(name='Position', period_in_ms=100) - lpos.add_variable('stateEstimate.x') - lpos.add_variable('stateEstimate.y') - lpos.add_variable('stateEstimate.z') - - try: - self.cf.log.add_config(lpos) - lpos.data_received_cb.add_callback(self.pos_data) - lpos.start() - except KeyError as e: - print('Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - print('Could not add Position log config, bad configuration.') - - lmeas = LogConfig(name='Meas', period_in_ms=100) - lmeas.add_variable('range.front') - lmeas.add_variable('range.back') - lmeas.add_variable('range.up') - lmeas.add_variable('range.left') - lmeas.add_variable('range.right') - lmeas.add_variable('range.zrange') - lmeas.add_variable('stabilizer.roll') - lmeas.add_variable('stabilizer.pitch') - lmeas.add_variable('stabilizer.yaw') - - try: - self.cf.log.add_config(lmeas) - lmeas.data_received_cb.add_callback(self.meas_data) - lmeas.start() - except KeyError as e: - print('Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - print('Could not add Measurement log config, bad configuration.') - - def pos_data(self, timestamp, data, logconf): - position = [ - data['stateEstimate.x'], - data['stateEstimate.y'], - data['stateEstimate.z'] - ] - self.canvas.set_position(position) - - def meas_data(self, timestamp, data, logconf): - measurement = { - 'roll': data['stabilizer.roll'], - 'pitch': data['stabilizer.pitch'], - 'yaw': data['stabilizer.yaw'], - 'front': data['range.front'], - 'back': data['range.back'], - 'up': data['range.up'], - 'down': data['range.zrange'], - 'left': data['range.left'], - 'right': data['range.right'] - } - self.canvas.set_measurement(measurement) - - def closeEvent(self, event): - if (self.cf is not None): - self.cf.close_link() - - -class Canvas(scene.SceneCanvas): - def __init__(self, keyupdateCB): - scene.SceneCanvas.__init__(self, keys=None) - self.size = 800, 600 - self.unfreeze() - self.view = self.central_widget.add_view() - self.view.bgcolor = '#ffffff' - self.view.camera = TurntableCamera( - fov=10.0, distance=30.0, up='+z', center=(0.0, 0.0, 0.0)) - self.last_pos = [0, 0, 0] - self.pos_markers = visuals.Markers() - self.meas_markers = visuals.Markers() - self.pos_data = np.array([0, 0, 0], ndmin=2) - self.meas_data = np.array([0, 0, 0], ndmin=2) - self.lines = [] - - self.view.add(self.pos_markers) - self.view.add(self.meas_markers) - for i in range(6): - line = visuals.Line() - self.lines.append(line) - self.view.add(line) - - self.keyCB = keyupdateCB - - self.freeze() - - scene.visuals.XYZAxis(parent=self.view.scene) - - def on_key_press(self, event): - if (not event.native.isAutoRepeat()): - if (event.native.key() == QtCore.Qt.Key_Left): - self.keyCB('y', 1) - if (event.native.key() == QtCore.Qt.Key_Right): - self.keyCB('y', -1) - if (event.native.key() == QtCore.Qt.Key_Up): - self.keyCB('x', 1) - if (event.native.key() == QtCore.Qt.Key_Down): - self.keyCB('x', -1) - if (event.native.key() == QtCore.Qt.Key_A): - self.keyCB('yaw', -70) - if (event.native.key() == QtCore.Qt.Key_D): - self.keyCB('yaw', 70) - if (event.native.key() == QtCore.Qt.Key_Z): - self.keyCB('yaw', -200) - if (event.native.key() == QtCore.Qt.Key_X): - self.keyCB('yaw', 200) - if (event.native.key() == QtCore.Qt.Key_W): - self.keyCB('height', 0.1) - if (event.native.key() == QtCore.Qt.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): - self.keyCB('y', 0) - if (event.native.key() == QtCore.Qt.Key_Right): - self.keyCB('y', 0) - if (event.native.key() == QtCore.Qt.Key_Up): - self.keyCB('x', 0) - if (event.native.key() == QtCore.Qt.Key_Down): - self.keyCB('x', 0) - if (event.native.key() == QtCore.Qt.Key_A): - self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key_D): - self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key_W): - self.keyCB('height', 0) - if (event.native.key() == QtCore.Qt.Key_S): - self.keyCB('height', 0) - if (event.native.key() == QtCore.Qt.Key_Z): - self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key_X): - self.keyCB('yaw', 0) - - def set_position(self, pos): - self.last_pos = pos - if (PLOT_CF): - self.pos_data = np.append(self.pos_data, [pos], axis=0) - self.pos_markers.set_data(self.pos_data, face_color='red', size=5) - - def rot(self, roll, pitch, yaw, origin, point): - cosr = math.cos(math.radians(roll)) - cosp = math.cos(math.radians(pitch)) - cosy = math.cos(math.radians(yaw)) - - sinr = math.sin(math.radians(roll)) - sinp = math.sin(math.radians(pitch)) - siny = math.sin(math.radians(yaw)) - - roty = np.array([[cosy, -siny, 0], - [siny, cosy, 0], - [0, 0, 1]]) - - rotp = np.array([[cosp, 0, sinp], - [0, 1, 0], - [-sinp, 0, cosp]]) - - rotr = np.array([[1, 0, 0], - [0, cosr, -sinr], - [0, sinr, cosr]]) - - rotFirst = np.dot(rotr, rotp) - - rot = np.array(np.dot(rotFirst, roty)) - - tmp = np.subtract(point, origin) - tmp2 = np.dot(rot, tmp) - return np.add(tmp2, origin) - - def rotate_and_create_points(self, m): - data = [] - o = self.last_pos - roll = m['roll'] - pitch = -m['pitch'] - yaw = m['yaw'] - - if (m['up'] < SENSOR_TH): - up = [o[0], o[1], o[2] + m['up'] / 1000.0] - data.append(self.rot(roll, pitch, yaw, o, up)) - - if (m['down'] < SENSOR_TH and PLOT_SENSOR_DOWN): - down = [o[0], o[1], o[2] - m['down'] / 1000.0] - data.append(self.rot(roll, pitch, yaw, o, down)) - - if (m['left'] < SENSOR_TH): - left = [o[0], o[1] + m['left'] / 1000.0, o[2]] - data.append(self.rot(roll, pitch, yaw, o, left)) - - if (m['right'] < SENSOR_TH): - right = [o[0], o[1] - m['right'] / 1000.0, o[2]] - data.append(self.rot(roll, pitch, yaw, o, right)) - - if (m['front'] < SENSOR_TH): - front = [o[0] + m['front'] / 1000.0, o[1], o[2]] - data.append(self.rot(roll, pitch, yaw, o, front)) - - if (m['back'] < SENSOR_TH): - back = [o[0] - m['back'] / 1000.0, o[1], o[2]] - data.append(self.rot(roll, pitch, yaw, o, back)) - - return data - - def set_measurement(self, measurements): - data = self.rotate_and_create_points(measurements) - o = self.last_pos - for i in range(6): - if (i < len(data)): - o = self.last_pos - self.lines[i].set_data(np.array([o, data[i]])) - else: - self.lines[i].set_data(np.array([o, o])) - - if (len(data) > 0): - self.meas_data = np.append(self.meas_data, data, axis=0) - self.meas_markers.set_data(self.meas_data, face_color='blue', size=5) - - -if __name__ == '__main__': - appQt = QtWidgets.QApplication(sys.argv) - win = MainWindow(URI) - win.show() - appQt.exec_() diff --git a/examples/multiranger_push.py b/examples/multiranger_push.py deleted file mode 100755 index a63e01f86..000000000 --- a/examples/multiranger_push.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017 Bitcraze AB -# -# Crazyflie Python Library -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# 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 -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.multiranger import Multiranger - -URI = 'radio://0/80/2M' - -if len(sys.argv) > 1: - URI = sys.argv[1] - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -def is_close(range): - MIN_DISTANCE = 0.2 # m - - if range is None: - return False - else: - return range < MIN_DISTANCE - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - - cf = Crazyflie(rw_cache='./cache') - with SyncCrazyflie(URI, cf=cf) as scf: - with MotionCommander(scf) as motion_commander: - with Multiranger(scf) as multiranger: - keep_flying = True - - while keep_flying: - VELOCITY = 0.5 - velocity_x = 0.0 - velocity_y = 0.0 - - if is_close(multiranger.front): - velocity_x -= VELOCITY - if is_close(multiranger.back): - velocity_x += VELOCITY - - if is_close(multiranger.left): - velocity_y -= VELOCITY - if is_close(multiranger.right): - velocity_y += VELOCITY - - if is_close(multiranger.up): - keep_flying = False - - motion_commander.start_linear_motion( - velocity_x, velocity_y, 0) - - time.sleep(0.1) - - print('Demo terminated!') diff --git a/examples/basicparam.py b/examples/parameters/basicparam.py similarity index 66% rename from examples/basicparam.py rename to examples/parameters/basicparam.py index d2b76bf67..b391ae22b 100644 --- a/examples/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 @@ -34,6 +32,9 @@ import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.utils import uri_helper + +address = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -52,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) @@ -63,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 = [] @@ -71,7 +74,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 @@ -83,8 +86,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', @@ -92,6 +94,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)) @@ -100,36 +117,21 @@ 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) - 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 @@ -149,21 +151,16 @@ 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) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - pe = ParamExample(available[0][0]) - # The Crazyflie lib doesn't contain anything to keep the application - # alive, so this is where your application should do something. In our - # case we are just waiting until we are disconnected. - while pe.is_connected: - time.sleep(1) - else: - print('No Crazyflies found, cannot run example') + import logging + logging.getLogger().setLevel(logging.INFO) + # Initialize the low-level drivers + cflib.crtp.init_drivers() + 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: + if pe.should_disconnect: + pe._cf.close_link() + time.sleep(1) diff --git a/examples/parameters/persistent_params.py b/examples/parameters/persistent_params.py new file mode 100644 index 000000000..338557b7b --- /dev/null +++ b/examples/parameters/persistent_params.py @@ -0,0 +1,135 @@ +# -*- 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. +* 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 +""" +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='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 + + +if __name__ == '__main__': + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + cf = Crazyflie(rw_cache='./cache') + + with SyncCrazyflie(uri, cf=cf) as scf: + # Get the names of all parameters that can be persisted + persistent_params = get_all_persistent_param_names(scf.cf) + + if not persistent_params: + print('No persistent parameters') + sys.exit(0) + + # Get the state of all persistent parameters + print('-- All persistent parameters --') + for complete_name in persistent_params: + get_persistent_state(scf.cf, complete_name) + + print() + param_name = 'ring.effect' + param_value = 10 + + print(f'Set parameter {param_name} to {param_value}') + scf.cf.param.set_value(param_name, param_value) + + print() + print('Store the new value in persistent memory') + persist_parameter(scf.cf, param_name) + + 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) + + print('The new state is:') + get_persistent_state(scf.cf, param_name) 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/examples/position_commander_demo.py b/examples/position_commander_demo.py deleted file mode 100644 index e00a28f0b..000000000 --- a/examples/position_commander_demo.py +++ /dev/null @@ -1,82 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2018 Bitcraze AB -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -This script shows the basic use of the PositionHlCommander class. - -Simple example that connects to the crazyflie at `URI` and runs a -sequence. This script requires some kind of location system. - -The PositionHlCommander uses position setpoints. - -Change the URI variable to your Crazyflie configuration. -""" -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.positioning.position_hl_commander import PositionHlCommander - -# URI to the Crazyflie to connect to -uri = 'radio://0/80/2M/E7E7E7E7E7' - - -def slightly_more_complex_usage(): - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - with PositionHlCommander( - scf, - x=0.0, y=0.0, z=0.0, - default_velocity=0.3, - default_height=0.5, - controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: - # Go to a coordinate - pc.go_to(1.0, 1.0, 1.0) - - # Move relative to the current position - pc.right(1.0) - - # Go to a coordinate and use default height - pc.go_to(0.0, 0.0) - - # Go slowly to a coordinate - pc.go_to(1.0, 1.0, velocity=0.2) - - # Set new default velocity and height - pc.set_default_velocity(0.3) - pc.set_default_height(1.0) - pc.go_to(0.0, 0.0) - - -def simple_sequence(): - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - with PositionHlCommander(scf) as pc: - pc.forward(1.0) - pc.left(1.0) - pc.back(1.0) - pc.go_to(0.0, 0.0, 1.0) - - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - - simple_sequence() - # slightly_more_complex_usage() diff --git a/examples/positioning/bezier_trajectory.py b/examples/positioning/bezier_trajectory.py deleted file mode 100644 index 9b26ad943..000000000 --- a/examples/positioning/bezier_trajectory.py +++ /dev/null @@ -1,425 +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, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -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%C3%A9zier_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() diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py deleted file mode 100644 index 29ed5bfa2..000000000 --- a/examples/positioning/initial_position.py +++ /dev/null @@ -1,157 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects to one crazyflie, sets the initial position/yaw -and flies a trajectory. - -The initial pose (x, y, z, yaw) is configured in a number of variables and -the trajectory is flown relative to this position, using the initial yaw. - -This example is intended to work with any absolute positioning system. -It aims at documenting how to take off with the Crazyflie in an orientation -that is different from the standard positive X orientation and how to set the -initial position of the kalman estimator. -""" -import math -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.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' - -# Change the sequence according to your setup -# x y z -sequence = [ - (0, 0, 0.7), - (-0.7, 0, 0.7), - (0, 0, 0.7), - (0, 0, 0.2), -] - - -def 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) - scf.cf.param.set_value('kalman.initialZ', z) - - yaw_radians = math.radians(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 - - for position in sequence: - print('Setting position {}'.format(position)) - - x = position[0] + base_x - y = position[1] + base_y - z = position[2] + base_z - - for i in range(50): - cf.commander.send_position_setpoint(x, y, z, yaw) - time.sleep(0.1) - - cf.commander.send_stop_setpoint() - # Make sure that the last packet leaves before the link is closed - # since the message queue is not flushed before closing - time.sleep(0.1) - - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - - # Set these to the position and yaw based on how your Crazyflie is placed - # on the floor - initial_x = 1.0 - initial_y = 1.0 - initial_z = 0.0 - initial_yaw = 90 # In degrees - # 0: positive X direction - # 90: positive Y direction - # 180: negative X direction - # 270: negative Y direction - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - set_initial_position(scf, initial_x, initial_y, initial_z, initial_yaw) - reset_estimator(scf) - run_sequence(scf, sequence, - initial_x, initial_y, initial_z, initial_yaw) diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py deleted file mode 100644 index cd62017e8..000000000 --- a/examples/positioning/matrix_light_printer.py +++ /dev/null @@ -1,115 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2018 Bitcraze AB -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -This script implements a simple matrix light printer to be used with a -camera with open shutter in a dark room. - -It requires a Crazyflie capable of controlling its position and with -a Led ring attached to it. A piece of sicky paper can be attached on -the led ring to orient the ring light toward the front. - -To control it position, Crazyflie requires an absolute positioning -system such as the Lighthouse. -""" -import time - -import matplotlib.pyplot as plt - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.positioning.position_hl_commander import PositionHlCommander - -# URI to the Crazyflie to connect to -uri = 'radio://0/80' - - -class ImageDef: - def __init__(self, file_name): - self._image = plt.imread(file_name) - - self.x_pixels = self._image.shape[1] - self.y_pixels = self._image.shape[0] - - width = 1.0 - height = self.y_pixels * width / self.x_pixels - - self.x_start = -width / 2.0 + 0.5 - self.y_start = 0.7 - - self.x_step = width / self.x_pixels - self.y_step = height / self.y_pixels - - def get_color(self, x_index, y_index): - rgba = self._image[self.y_pixels - y_index - 1, x_index] - rgb = [int(rgba[0] * 90), int(rgba[1] * 90), int(rgba[2] * 90)] - return rgb - - -BLACK = [0, 0, 0] - - -def set_led_ring_solid(cf, rgb): - cf.param.set_value('ring.effect', 7) - print(rgb[0], rgb[1], rgb[2]) - cf.param.set_value('ring.solidRed', rgb[0]) - cf.param.set_value('ring.solidGreen', rgb[1]) - cf.param.set_value('ring.solidBlue', rgb[2]) - - -def matrix_print(cf, pc, image_def): - set_led_ring_solid(cf, BLACK) - time.sleep(3) - - for y_index in range(image_def.y_pixels): - y = image_def.y_start + image_def.y_step * y_index - - pc.go_to(0, image_def.x_start - 0.1, y) - time.sleep(1.0) - - scan_range = range(image_def.x_pixels) - - for x_index in scan_range: - x = image_def.x_start + image_def.x_step * x_index - - color = image_def.get_color(x_index, y_index) - - print(x, y, color) - - pc.go_to(0, x, y) - set_led_ring_solid(cf, color) - - set_led_ring_solid(cf, BLACK) - - print('---') - - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - - image_def = ImageDef('monalisa.png') - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - with PositionHlCommander(scf, default_height=0.5) as pc: - matrix_print(scf.cf, pc, image_def) diff --git a/examples/positioning/monalisa.png b/examples/positioning/monalisa.png deleted file mode 100644 index f39be376e..000000000 Binary files a/examples/positioning/monalisa.png and /dev/null differ diff --git a/examples/radio-test.py b/examples/radio-test.py deleted file mode 100644 index d4c90e284..000000000 --- a/examples/radio-test.py +++ /dev/null @@ -1,152 +0,0 @@ -# Eric Yihan Chen -# The Automatic Coordination of Teams (ACT) Lab -# University of Southern California -# ericyihc@usc.edu -''' - Simple example that connects to the first Crazyflie found, triggers - reading of rssi data and acknowledgement rate for every channel (0 to 125). - It finally sets the Crazyflie channel back to default, plots link - quality data, and offers good channel suggestion. - - Better used when the Crazyflie2-nrf-firmware is compiled with bluetooth - disabled. -''' -import argparse - -import matplotlib.pyplot as plt -import numpy as np - -import cflib.drivers.crazyradio as crazyradio - -radio = crazyradio.Crazyradio() - -# optional user input -parser = argparse.ArgumentParser(description='Key variables') -parser.add_argument( - '-try', '--try', dest='TRY', type=int, default=100, - help='the time to send data for each channel' -) -# by default my crazyflie uses channel 80 -parser.add_argument( - '-channel', '--channel', dest='channel', type=int, - default=80, help='the default channel in crazyflie' -) -# by default my crazyflie uses datarate 2M -parser.add_argument( - '-rate', '--rate', dest='rate', type=int, default=2, - help='the default datarate in crazyflie' -) -parser.add_argument( - '-frac', '--fraction', dest='fraction', type=float, - default=0.25, help='top fraction of suggested channels' -) -args = parser.parse_args() - -init_channel = args.channel -TRY = args.TRY -Fraction = args.fraction -data_rate = args.rate - -radio.set_channel(init_channel) -radio.set_data_rate(data_rate) -SET_RADIO_CHANNEL = 1 - -rssi_std = [] -rssi = [] -ack = [] -radio.set_arc(0) - -for channel in range(0, 126, 1): - - # change Crazyflie channel - for x in range(50): - radio.send_packet((0xff, 0x03, SET_RADIO_CHANNEL, channel)) - - count = 0 - temp = [] - - # change radio channel - radio.set_channel(channel) - - for i in range(TRY): - pk = radio.send_packet((0xff, )) - if pk.ack: - count += 1 - if pk.ack and len(pk.data) > 2 and \ - pk.data[0] & 0xf3 == 0xf3 and pk.data[1] == 0x01: - # append rssi data - temp.append(pk.data[2]) - - ack_rate = count / TRY - rssi_avg = np.mean(temp) - std = np.std(temp) - - rssi.append(rssi_avg) - ack.append(ack_rate) - rssi_std.append(std) - - print('Channel', channel, 'ack_rate:', ack_rate, - 'rssi average:', rssi_avg, 'rssi std:', std) - -# change channel back to default -for x in range(50): - radio.send_packet((0xff, 0x03, SET_RADIO_CHANNEL, init_channel)) - -# divide each std by 2 for plotting convenience -rssi_std = [x / 2 for x in rssi_std] -rssi_std = np.array(rssi_std) -rssi = np.array(rssi) -ack = np.array(ack) - -rssi_rank = [] -ack_rank = [] - -# suggestion for rssi -order = rssi.argsort() -ranks = order.argsort() -for x in range(int(125 * Fraction)): - for y in range(126): - if ranks[y] == x: - rssi_rank.append(y) - -# suggestion for ack -order = ack.argsort() -ranks = order.argsort() -for x in range(126, 126 - int(125 * Fraction) - 1, -1): - for y in range(126): - if ranks[y] == x: - ack_rank.append(y) - -rssi_set = set(rssi_rank[0:int(125 * Fraction)]) -ack_set = set(ack_rank[0:int(125 * Fraction)]) -final_rank = rssi_set.intersection(ack_rank) -print('\nSuggested Channels:') -for x in final_rank: - print('\t', x) - -# graph 1 for ack -x = np.arange(0, 126, 1) -fig, ax1 = plt.subplots() -ax1.axis([0, 125, 0, 1.25]) -ax1.plot(x, ack, 'b') -ax1.set_xlabel('Channel') -ax1.set_ylabel('Ack Rate', color='b') -for tl in ax1.get_yticklabels(): - tl.set_color('b') - -# graph 2 for rssi & rssi_std -ax2 = ax1.twinx() -ax2.grid(True) -ax2.errorbar(x, rssi, yerr=rssi_std, fmt='r-') -ax2.fill_between(x, rssi + rssi_std, rssi - rssi_std, - facecolor='orange', edgecolor='k') -ax2.axis([0, 125, 0, 90]) -plt.ylabel('RSSI Average', color='r') -for tl in ax2.get_yticklabels(): - tl.set_color('r') -points = np.ones(100) -for x in final_rank: - ax2.plot((x, x), (0, 100), linestyle='-', - color='cornflowerblue', linewidth=1) - -plt.show() diff --git a/examples/ramp.py b/examples/ramp.py deleted file mode 100644 index d5d9808da..000000000 --- a/examples/ramp.py +++ /dev/null @@ -1,117 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects to the first Crazyflie found, ramps up/down -the motors and disconnects. -""" -import logging -import time -from threading import Thread - -import cflib -from cflib.crazyflie import Crazyflie - -logging.basicConfig(level=logging.ERROR) - - -class MotorRampExample: - """Example that connects to a Crazyflie and ramps the motors up/down and - the disconnects""" - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - self._cf = Crazyflie(rw_cache='./cache') - - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - self._cf.open_link(link_uri) - - print('Connecting to %s' % link_uri) - - def _connected(self, link_uri): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - - # Start a separate thread to do the motor test. - # Do not hijack the calling thread! - Thread(target=self._ramp_motors).start() - - def _connection_failed(self, link_uri, msg): - """Callback when connection initial connection fails (i.e no Crazyflie - at the specified address)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - - def _ramp_motors(self): - thrust_mult = 1 - thrust_step = 500 - thrust = 20000 - pitch = 0 - roll = 0 - yawrate = 0 - - # Unlock startup thrust protection - self._cf.commander.send_setpoint(0, 0, 0, 0) - - while thrust >= 20000: - self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) - time.sleep(0.1) - if thrust >= 25000: - thrust_mult = -1 - thrust += thrust_step * thrust_mult - 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) - self._cf.close_link() - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = MotorRampExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') diff --git a/examples/read-eeprom.py b/examples/read-eeprom.py deleted file mode 100644 index eea58f901..000000000 --- a/examples/read-eeprom.py +++ /dev/null @@ -1,139 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects to the first Crazyflie found, looks for -EEPROM memories and lists its contents. -""" -import logging -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class EEPROMExample: - """ - Simple example listing the EEPROMs found and lists its contents. - """ - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # Connect some callbacks from the Crazyflie API - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - print('Connecting to %s' % link_uri) - - # Try to connect to the Crazyflie - self._cf.open_link(link_uri) - - # Variable used to keep main loop occupied until disconnect - self.is_connected = True - - def _connected(self, link_uri): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - print('Connected to %s' % link_uri) - - mems = self._cf.mem.get_mems(MemoryElement.TYPE_I2C) - print('Found {} EEPOM(s)'.format(len(mems))) - self._mems_to_update = len(mems) - for m in mems: - print('Updating id={}'.format(m.id)) - m.update(self._data_updated) - - def _data_updated(self, mem): - print('Updated id={}'.format(mem.id)) - print('\tType : {}'.format(mem.type)) - print('\tSize : {}'.format(mem.size)) - print('\tValid : {}'.format(mem.valid)) - print('\tElements : ') - for key in mem.elements: - print('\t\t{}={}'.format(key, mem.elements[key])) - - self._mems_to_update -= 1 - if self._mems_to_update == 0: - self._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 froma 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)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - self.is_connected = False - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - self.is_connected = False - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = EEPROMExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') - - # The Crazyflie lib doesn't contain anything to keep the application alive, - # so this is where your application should do something. In our case we - # are just waiting until we are disconnected. - try: - while le.is_connected: - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/read-ow.py b/examples/read-ow.py deleted file mode 100644 index 235e9eae3..000000000 --- a/examples/read-ow.py +++ /dev/null @@ -1,146 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects to the first Crazyflie found, looks for -1-wire memories and lists its contents. -""" -import logging -import sys -import time - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -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 """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # Connect some callbacks from the Crazyflie API - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - print('Connecting to %s' % link_uri) - - # Try to connect to the Crazyflie - self._cf.open_link(link_uri) - - # Variable used to keep main loop occupied until disconnect - self.is_connected = True - self._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))) - 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._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 froma 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)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - self.is_connected = False - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - self.is_connected = False - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = OWExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') - - # The Crazyflie lib doesn't contain anything to keep the application alive, - # so this is where your application should do something. In our case we - # are just waiting until we are disconnected. - try: - while le.is_connected: - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/step-by-step/sbs_connect_log_param.py b/examples/step-by-step/sbs_connect_log_param.py new file mode 100644 index 000000000..98cb4a4dd --- /dev/null +++ b/examples/step-by-step/sbs_connect_log_param.py @@ -0,0 +1,113 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2017 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +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 +from cflib.utils import uri_helper + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + + +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, logconf) 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 + cflib.crtp.init_drivers() + + 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) 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..7189db192 --- /dev/null +++ b/examples/step-by-step/sbs_motion_commander.py @@ -0,0 +1,135 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2017 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +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') + +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +deck_attached_event = Event() + +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): + time.sleep(3) + + +def log_pos_callback(timestamp, data, logconf): + print(data) + position_estimate[0] = data['stateEstimate.x'] + position_estimate[1] = data['stateEstimate.y'] + + +def param_deck_flow(_, value_str): + value = int(value_str) + print(value) + if value: + deck_attached_event.set() + print('Deck is attached!') + else: + print('Deck is NOT attached!') + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + 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 not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + # Arm the Crazyflie + scf.cf.supervisor.send_arming_request(True) + time.sleep(1.0) + + logconf.start() + + take_off_simple(scf) + # move_linear_simple(scf) + # move_box_limit(scf) + logconf.stop() diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py new file mode 100644 index 000000000..ae098e0f0 --- /dev/null +++ b/examples/step-by-step/sbs_swarm.py @@ -0,0 +1,174 @@ +# -*- 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_led_bit_mask(scf: SyncCrazyflie): + scf.cf.param.set_value('led.bitmask', 255) + + +def deactivate_led_bit_mask(scf: SyncCrazyflie): + scf.cf.param.set_value('led.bitmask', 0) + + +def light_check(scf: SyncCrazyflie): + activate_led_bit_mask(scf) + time.sleep(2) + deactivate_led_bit_mask(scf) + time.sleep(2) + + +def arm(scf: SyncCrazyflie): + scf.cf.supervisor.send_arming_request(True) + time.sleep(1.0) + + +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 + # 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): +# <------ 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(arm) + swarm.parallel_safe(take_off) + # swarm.parallel_safe(run_square_sequence) + swarm.parallel_safe(run_sequence, args_dict=seq_args) + swarm.parallel_safe(land) diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py deleted file mode 100644 index c1685342a..000000000 --- a/examples/swarm/hl-commander-swarm.py +++ /dev/null @@ -1,145 +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, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -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 -be at any position on the floor when the script is started. - -This example is intended to work with any absolute positioning system. -It aims at documenting how to use the High Level Commander together with -the Swarm class. -""" -import time - -import cflib.crtp -from cflib.crazyflie.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): - scf.cf.param.set_value('commander.enHighLevel', '1') - - -def activate_mellinger_controller(scf, use_mellinger): - controller = 1 - if use_mellinger: - controller = 2 - scf.cf.param.set_value('stabilizer.controller', controller) - - -def run_shared_sequence(scf): - activate_mellinger_controller(scf, False) - - box_size = 1 - flight_time = 2 - - commander = scf.cf.high_level_commander - - commander.takeoff(1.0, 2.0) - time.sleep(3) - - commander.go_to(box_size, 0, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(0, box_size, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(-box_size, 0, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(0, -box_size, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.land(0.0, 2.0) - time.sleep(2) - - commander.stop() - - -uris = { - 'radio://0/30/2M/E7E7E7E711', - 'radio://0/30/2M/E7E7E7E712', - # Add more URIs if you want more copters in the swarm -} - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - 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.parallel_safe(run_shared_sequence) diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py deleted file mode 100644 index 2b442987a..000000000 --- a/examples/swarm/swarmSequence.py +++ /dev/null @@ -1,296 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017-2018 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Version of the AutonomousSequence.py example connecting to 10 Crazyflies. -The Crazyflies go straight up, hover a while and land but the code is fairly -generic and each Crazyflie has its own sequence of setpoints that it files -to. - -The layout of the positions: - x2 x1 x0 - -y3 10 4 - - ^ Y - | -y2 9 6 3 - | - +------> X - -y1 8 5 2 - - - -y0 7 1 - -""" -import time - -import cflib.crtp -from cflib.crazyflie.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' -URI2 = 'radio://0/70/2M/E7E7E7E702' -URI3 = 'radio://0/70/2M/E7E7E7E703' -URI4 = 'radio://0/70/2M/E7E7E7E704' -URI5 = 'radio://0/70/2M/E7E7E7E705' -URI6 = 'radio://0/70/2M/E7E7E7E706' -URI7 = 'radio://0/70/2M/E7E7E7E707' -URI8 = 'radio://0/70/2M/E7E7E7E708' -URI9 = 'radio://0/70/2M/E7E7E7E709' -URI10 = 'radio://0/70/2M/E7E7E7E70A' - - -z0 = 0.4 -z = 1.0 - -x0 = 0.7 -x1 = 0 -x2 = -0.7 - -y0 = -1.0 -y1 = -0.4 -y2 = 0.4 -y3 = 1.0 - -# x y z time -sequence1 = [ - (x0, y0, z0, 3.0), - (x0, y0, z, 30.0), - (x0, y0, z0, 3.0), -] - -sequence2 = [ - (x0, y1, z0, 3.0), - (x0, y1, z, 30.0), - (x0, y1, z0, 3.0), -] - -sequence3 = [ - (x0, y2, z0, 3.0), - (x0, y2, z, 30.0), - (x0, y2, z0, 3.0), -] - -sequence4 = [ - (x0, y3, z0, 3.0), - (x0, y3, z, 30.0), - (x0, y3, z0, 3.0), -] - -sequence5 = [ - (x1, y1, z0, 3.0), - (x1, y1, z, 30.0), - (x1, y1, z0, 3.0), -] - -sequence6 = [ - (x1, y2, z0, 3.0), - (x1, y2, z, 30.0), - (x1, y2, z0, 3.0), -] - -sequence7 = [ - (x2, y0, z0, 3.0), - (x2, y0, z, 30.0), - (x2, y0, z0, 3.0), -] - -sequence8 = [ - (x2, y1, z0, 3.0), - (x2, y1, z, 30.0), - (x2, y1, z0, 3.0), -] - -sequence9 = [ - (x2, y2, z0, 3.0), - (x2, y2, z, 30.0), - (x2, y2, z0, 3.0), -] - -sequence10 = [ - (x2, y3, z0, 3.0), - (x2, y3, z, 30.0), - (x2, y3, z0, 3.0), -] - -seq_args = { - URI1: [sequence1], - URI2: [sequence2], - URI3: [sequence3], - URI4: [sequence4], - URI5: [sequence5], - URI6: [sequence6], - URI7: [sequence7], - URI8: [sequence8], - URI9: [sequence9], - URI10: [sequence10], -} - -# List of URIs, comment the one you do not want to fly -uris = { - URI1, - URI2, - URI3, - URI4, - URI5, - URI6, - URI7, - URI8, - URI9, - URI10 -} - - -def wait_for_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 - steps = int(take_off_time / sleep_time) - vz = position[2] / take_off_time - - print(vz) - - for i in range(steps): - cf.commander.send_velocity_world_setpoint(0, 0, vz, 0) - time.sleep(sleep_time) - - -def land(cf, position): - landing_time = 1.0 - sleep_time = 0.1 - steps = int(landing_time / sleep_time) - vz = -position[2] / landing_time - - print(vz) - - for _ in range(steps): - cf.commander.send_velocity_world_setpoint(0, 0, vz, 0) - time.sleep(sleep_time) - - cf.commander.send_stop_setpoint() - # Make sure that the last packet leaves before the link is closed - # since the message queue is not flushed before closing - time.sleep(0.1) - - -def run_sequence(scf, sequence): - try: - cf = scf.cf - - take_off(cf, sequence[0]) - for position in sequence: - print('Setting position {}'.format(position)) - end_time = time.time() + position[3] - while time.time() < end_time: - cf.commander.send_position_setpoint(position[0], - position[1], - position[2], 0) - time.sleep(0.1) - land(cf, sequence[-1]) - except Exception as e: - print(e) - - -if __name__ == '__main__': - # logging.basicConfig(level=logging.DEBUG) - cflib.crtp.init_drivers(enable_debug_driver=False) - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - # If the copters are started in their correct positions this is - # probably not needed. The Kalman filter will have time to converge - # any way since it takes a while to start them all up and connect. We - # keep the code here to illustrate how to do it. - # swarm.parallel(reset_estimator) - - # The current values of all parameters are downloaded as a part of the - # connections sequence. Since we have 10 copters this is clogging up - # communication and we have to wait for it to finish before we start - # flying. - print('Waiting for parameters to be downloaded...') - swarm.parallel(wait_for_param_download) - - swarm.parallel(run_sequence, args_dict=seq_args) diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py deleted file mode 100644 index d0c637809..000000000 --- a/examples/swarm/swarmSequenceCircle.py +++ /dev/null @@ -1,150 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# 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 -Flow deck. -The starting positions are vital and should be oriented like this - - > - -^ + v - - < - -The distance from the center to the perimeter of the circle is around 0.5 m - -""" -import math -import time - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm - -# Change uris according to your setup -URI0 = 'radio://0/70/2M/E7E7E7E7E7' -URI1 = 'radio://0/110/2M/E7E7E7E702' -URI2 = 'radio://0/94/2M/E7E7E7E7E7' -URI3 = 'radio://0/5/2M/E7E7E7E702' -URI4 = 'radio://0/110/2M/E7E7E7E703' - -# d: diameter of circle -# z: altituce -params0 = {'d': 1.0, 'z': 0.3} -params1 = {'d': 1.0, 'z': 0.3} -params2 = {'d': 0.0, 'z': 0.5} -params3 = {'d': 1.0, 'z': 0.3} -params4 = {'d': 1.0, 'z': 0.3} - - -uris = { - URI0, - URI1, - URI2, - URI3, - URI4, -} - -params = { - URI0: [params0], - URI1: [params1], - URI2: [params2], - URI3: [params3], - URI4: [params4], -} - - -def 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 - - for r in range(steps): - cf.commander.send_hover_setpoint(0, 0, 0, z) - time.sleep(0.1) - - -def run_sequence(scf, params): - cf = scf.cf - - # Number of setpoints sent per second - fs = 4 - fsi = 1.0 / fs - - # Compensation for unknown error :-( - comp = 1.3 - - # Base altitude in meters - base = 0.15 - - d = params['d'] - z = params['z'] - - poshold(cf, 2, base) - - ramp = fs * 2 - for r in range(ramp): - cf.commander.send_hover_setpoint(0, 0, 0, base + r * (z - base) / ramp) - time.sleep(fsi) - - poshold(cf, 2, z) - - for _ in range(2): - # The time for one revolution - circle_time = 8 - - steps = circle_time * fs - for _ in range(steps): - cf.commander.send_hover_setpoint(d * comp * math.pi / circle_time, - 0, 360.0 / circle_time, z) - time.sleep(fsi) - - poshold(cf, 2, z) - - for r in range(ramp): - cf.commander.send_hover_setpoint(0, 0, 0, - base + (ramp - r) * (z - base) / ramp) - time.sleep(fsi) - - poshold(cf, 1, base) - - cf.commander.send_stop_setpoint() - - -if __name__ == '__main__': - cflib.crtp.init_drivers(enable_debug_driver=False) - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - swarm.parallel(reset_estimator) - swarm.parallel(run_sequence, args_dict=params) diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py deleted file mode 100755 index fc9e6b601..000000000 --- a/examples/swarm/synchronizedSequence.py +++ /dev/null @@ -1,250 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example of a synchronized swarm choreography using the High level -commander. - -The swarm takes off and flies a synchronous choreography before landing. -The take-of is relative to the start position but the Goto are absolute. -The sequence contains a list of commands to be executed at each step. - -This example is intended to work with any absolute positioning system. -It aims at documenting how to use the High Level Commander together with -the Swarm class to achieve synchronous sequences. -""" -import threading -import time -from collections import namedtuple -from queue import Queue - -import cflib.crtp -from cflib.crazyflie.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 - -# Possible commands, all times are in seconds -Takeoff = namedtuple('Takeoff', ['height', 'time']) -Land = namedtuple('Land', ['time']) -Goto = namedtuple('Goto', ['x', 'y', 'z', 'time']) -# RGB [0-255], Intensity [0.0-1.0] -Ring = namedtuple('Ring', ['r', 'g', 'b', 'intensity', 'time']) -# Reserved for the control loop, do not use in sequence -Quit = namedtuple('Quit', []) - -uris = [ - 'radio://0/10/2M/E7E7E7E701', # cf_id 0, startup position [-0.5, -0.5] - 'radio://0/10/2M/E7E7E7E702', # cf_id 1, startup position [ 0, 0] - 'radio://0/10/2M/E7E7E7E703', # cf_id 3, startup position [0.5, 0.5] - # Add more URIs if you want more copters in the swarm -] - -sequence = [ - # Step, CF_id, action - (0, 0, Takeoff(0.5, 2)), - (0, 2, Takeoff(0.5, 2)), - - (1, 1, Takeoff(1.0, 2)), - - (2, 0, Goto(-0.5, -0.5, 0.5, 1)), - (2, 2, Goto(0.5, 0.5, 0.5, 1)), - - (3, 1, Goto(0, 0, 1, 1)), - - (4, 0, Ring(255, 255, 255, 0.2, 0)), - (4, 1, Ring(255, 0, 0, 0.2, 0)), - (4, 2, Ring(255, 255, 255, 0.2, 0)), - - (5, 0, Goto(0.5, -0.5, 0.5, 2)), - (5, 2, Goto(-0.5, 0.5, 0.5, 2)), - - (7, 0, Goto(0.5, 0.5, 0.5, 2)), - (7, 2, Goto(-0.5, -0.5, 0.5, 2)), - - (9, 0, Goto(-0.5, 0.5, 0.5, 2)), - (9, 2, Goto(0.5, -0.5, 0.5, 2)), - - (11, 0, Goto(-0.5, -0.5, 0.5, 2)), - (11, 2, Goto(0.5, 0.5, 0.5, 2)), - - (13, 0, Land(2)), - (13, 1, Land(2)), - (13, 2, Land(2)), - - (15, 0, Ring(0, 0, 0, 0, 5)), - (15, 1, Ring(0, 0, 0, 0, 5)), - (15, 2, Ring(0, 0, 0, 0, 5)), -] - - -def 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') - - -def activate_mellinger_controller(scf, use_mellinger): - controller = 1 - if use_mellinger: - controller = 2 - scf.cf.param.set_value('stabilizer.controller', str(controller)) - - -def set_ring_color(cf, r, g, b, intensity, time): - cf.param.set_value('ring.fadeTime', str(time)) - - r *= intensity - g *= intensity - b *= intensity - - color = (int(r) << 16) | (int(g) << 8) | int(b) - - cf.param.set_value('ring.fadeColor', str(color)) - - -def crazyflie_control(scf): - cf = scf.cf - control = controlQueues[uris.index(cf.link_uri)] - - activate_mellinger_controller(scf, True) - - commander = scf.cf.high_level_commander - - # Set fade to color effect and reset to Led-ring OFF - set_ring_color(cf, 0, 0, 0, 0, 0) - cf.param.set_value('ring.effect', '14') - - while True: - command = control.get() - if type(command) is Quit: - return - elif type(command) is Takeoff: - commander.takeoff(command.height, command.time) - elif type(command) is Land: - commander.land(0.0, command.time) - elif type(command) is Goto: - commander.go_to(command.x, command.y, command.z, 0, command.time) - elif type(command) is Ring: - set_ring_color(cf, command.r, command.g, command.b, - command.intensity, command.time) - pass - else: - print('Warning! unknown command {} for uri {}'.format(command, - cf.uri)) - - -def control_thread(): - pointer = 0 - step = 0 - stop = False - - while not stop: - print('Step {}:'.format(step)) - while sequence[pointer][0] <= step: - cf_id = sequence[pointer][1] - command = sequence[pointer][2] - - print(' - Running: {} on {}'.format(command, cf_id)) - controlQueues[cf_id].put(command) - pointer += 1 - - if pointer >= len(sequence): - print('Reaching the end of the sequence, stopping!') - stop = True - break - - step += 1 - time.sleep(STEP_TIME) - - for ctrl in controlQueues: - ctrl.put(Quit()) - - -if __name__ == '__main__': - controlQueues = [Queue() for _ in range(len(uris))] - - cflib.crtp.init_drivers(enable_debug_driver=False) - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - swarm.parallel_safe(activate_high_level_commander) - swarm.parallel_safe(reset_estimator) - - print('Starting sequence!') - - threading.Thread(target=control_thread).start() - - swarm.parallel_safe(crazyflie_control) - - time.sleep(1) diff --git a/examples/write-eeprom.py b/examples/write-eeprom.py deleted file mode 100644 index 0dfe09bbc..000000000 --- a/examples/write-eeprom.py +++ /dev/null @@ -1,151 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -Simple example that connects to the first Crazyflie found, looks for -EEPROM memories and writes the default values in it. -""" -import logging -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class EEPROMExample: - """ - Simple example listing the EEPROMs found and writes the default values - in it. - """ - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # Connect some callbacks from the Crazyflie API - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - print('Connecting to %s' % link_uri) - - # Try to connect to the Crazyflie - self._cf.open_link(link_uri) - - # Variable used to keep main loop occupied until disconnect - self.is_connected = True - - def _connected(self, link_uri): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - print('Connected to %s' % link_uri) - - mems = self._cf.mem.get_mems(MemoryElement.TYPE_I2C) - print('Found {} EEPOM(s)'.format(len(mems))) - if len(mems) > 0: - print('Writing default configuration to' - ' memory {}'.format(mems[0].id)) - - elems = mems[0].elements - elems['version'] = 1 - elems['pitch_trim'] = 0.0 - elems['roll_trim'] = 0.0 - elems['radio_channel'] = 80 - elems['radio_speed'] = 0 - elems['radio_address'] = 0xE7E7E7E7E7 - - mems[0].write_data(self._data_written) - - def _data_written(self, mem, addr): - print('Data written, reading back...') - mem.update(self._data_updated) - - def _data_updated(self, mem): - print('Updated id={}'.format(mem.id)) - print('\tType : {}'.format(mem.type)) - print('\tSize : {}'.format(mem.size)) - print('\tValid : {}'.format(mem.valid)) - print('\tElements : ') - for key in mem.elements: - print('\t\t{}={}'.format(key, mem.elements[key])) - - self._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 froma 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)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - self.is_connected = False - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - self.is_connected = False - - -if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = EEPROMExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') - - # The Crazyflie lib doesn't contain anything to keep the application alive, - # so this is where your application should do something. In our case we - # are just waiting until we are disconnected. - try: - while le.is_connected: - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/write-ow.py b/examples/write-ow.py deleted file mode 100644 index 17ab60eed..000000000 --- a/examples/write-ow.py +++ /dev/null @@ -1,161 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2014 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, -# MA 02110-1301, USA. -""" -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 -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.mem import OWElement - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class WriteOwExample: - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # Connect some callbacks from the Crazyflie API - self._cf.connected.add_callback(self._connected) - self._cf.disconnected.add_callback(self._disconnected) - self._cf.connection_failed.add_callback(self._connection_failed) - self._cf.connection_lost.add_callback(self._connection_lost) - - print('Connecting to %s' % link_uri) - - # Try to connect to the Crazyflie - self._cf.open_link(link_uri) - - # Variable used to keep main loop occupied until disconnect - self.is_connected = True - - def _connected(self, link_uri): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - print('Connected to %s' % link_uri) - - mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) - print('Found {} 1-wire memories'.format(len(mems))) - if len(mems) > 0: - print('Writing test configuration to' - ' memory {}'.format(mems[0].id)) - - mems[0].vid = 0xBC - mems[0].pid = 0xFF - - 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_rev_id] = 'A' - - mems[0].write_data(self._data_written) - - def _data_written(self, mem, addr): - print('Data written, reading back...') - mem.update(self._data_updated) - - def _data_updated(self, mem): - print('Updated id={}'.format(mem.id)) - print('\tType : {}'.format(mem.type)) - print('\tSize : {}'.format(mem.size)) - print('\tValid : {}'.format(mem.valid)) - print('\tName : {}'.format(mem.name)) - print('\tVID : 0x{:02X}'.format(mem.vid)) - print('\tPID : 0x{:02X}'.format(mem.pid)) - print('\tPins : 0x{:02X}'.format(mem.pins)) - print('\tElements : ') - - for key in mem.elements: - print('\t\t{}={}'.format(key, mem.elements[key])) - - self._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 froma 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)""" - print('Connection to %s failed: %s' % (link_uri, msg)) - self.is_connected = False - - def _connection_lost(self, link_uri, msg): - """Callback when disconnected after a connection has been made (i.e - Crazyflie moves out of range)""" - print('Connection to %s lost: %s' % (link_uri, msg)) - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - self.is_connected = False - - -if __name__ == '__main__': - 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) - # Scan for Crazyflies and use the first one found - print('Scanning interfaces for Crazyflies...') - available = cflib.crtp.scan_interfaces() - print('Crazyflies found:') - for i in available: - print(i[0]) - - if len(available) > 0: - le = WriteOwExample(available[0][0]) - else: - print('No Crazyflies found, cannot run example') - - # The Crazyflie lib doesn't contain anything to keep the application alive, - # so this is where your application should do something. In our case we - # are just waiting until we are disconnected. - try: - while le.is_connected: - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/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/module.json b/module.json index 3d8af4559..600e47cc6 100644 --- a/module.json +++ b/module.json @@ -1,4 +1,7 @@ { - "version": "1.0", - "environmentReq": ["python2", "python3"] + "version": "2.0", + "environmentReqs": { + "build": ["python3"], + "build-docs": ["doxygen"] + } } diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..a2c3a5dc7 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,66 @@ +[build-system] +requires = ["setuptools>=61.0", "wheel", "setuptools_scm"] +build-backend = "setuptools.build_meta" + +[project] +name = "cflib" +dynamic = ["version"] +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 :: 5 - Production/Stable", + "License :: OSI Approved :: GNU General Public License v3 (GPLv3)", + "Topic :: System :: Hardware :: Hardware Drivers", + "Topic :: Scientific/Engineering", + "Intended Audience :: Science/Research", + "Intended Audience :: Education", + "Intended Audience :: Developers", + "Operating System :: POSIX :: Linux", + "Operating System :: MacOS", + "Operating System :: Microsoft :: Windows", + + # 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.2", + "libusb-package~=1.0", + "scipy~=1.14", + "numpy~=2.2", + "packaging~=25.0", +] + +[project.urls] +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" + +[project.optional-dependencies] +dev = ["pre-commit"] +qualisys = ["qtm-rt~=3.0.2"] +motioncapture = ["motioncapture~=1.0a4"] + +[tool.setuptools] +include-package-data = true + +[tool.setuptools.packages] +find = { exclude = ["examples", "test"] } + +[tool.setuptools.package-data] +"cflib.resources.binaries" = ["cflib/resources/binaries/*.bin"] + +[tool.setuptools_scm] +version_scheme = "no-guess-dev" 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 diff --git a/setup.cfg b/setup.cfg deleted file mode 100644 index 3c6e79cf3..000000000 --- a/setup.cfg +++ /dev/null @@ -1,2 +0,0 @@ -[bdist_wheel] -universal=1 diff --git a/setup.py b/setup.py deleted file mode 100644 index 7a2e8c933..000000000 --- a/setup.py +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/env python3 -from setuptools import find_packages -from setuptools import setup - -setup( - name='cflib', - version='0.1.8', - packages=find_packages(exclude=['examples', 'tests']), - - description='Crazyflie python driver', - url='https://github.com/bitcraze/crazyflie-lib-python', - - 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 :: 2', - 'Programming Language :: Python :: 3' - ], - - keywords='driver crazyflie quadcopter', - - install_requires='pyusb>=1.0.0b2', -) 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 diff --git a/sys_test/single_cf_grounded/README.md b/sys_test/single_cf_grounded/README.md new file mode 100644 index 000000000..b9eb4e9ca --- /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..b11234630 --- /dev/null +++ b/sys_test/single_cf_grounded/single_cf_grounded.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, see . +import unittest + +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 = uri_helper.uri_from_env(default='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..a3115251e --- /dev/null +++ b/sys_test/single_cf_grounded/test_bootloader.py @@ -0,0 +1,50 @@ +# -*- 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, see . +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..c9b4ffc52 --- /dev/null +++ b/sys_test/single_cf_grounded/test_link.py @@ -0,0 +1,154 @@ +# -*- 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, see . +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('. +import time +import unittest + +from single_cf_grounded import TestSingleCfGrounded + +from cflib.utils.power_switch import PowerSwitch + + +class TestPowerSwitch(TestSingleCfGrounded): + + def test_reboot(self): + self.assertTrue(self.is_stm_connected()) + s = PowerSwitch(self.radioUri) + s.stm_power_down() + s.close() + self.assertFalse(self.is_stm_connected()) + s = PowerSwitch(self.radioUri) + s.stm_power_up() + s.close() + time.sleep(2) + self.assertTrue(self.is_stm_connected()) + + +if __name__ == '__main__': + unittest.main() 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/__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/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/sys_test/swarm_test_rig/rig_support.py b/sys_test/swarm_test_rig/rig_support.py index 9fec7a73e..7940373bc 100644 --- a/sys_test/swarm_test_rig/rig_support.py +++ b/sys_test/swarm_test_rig/rig_support.py @@ -17,14 +17,11 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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.crtp import RadioDriver -from cflib.drivers.crazyradio import Crazyradio +from cflib.utils.power_switch import PowerSwitch class RigSupport: @@ -43,35 +40,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() diff --git a/sys_test/swarm_test_rig/test_connection.py b/sys_test/swarm_test_rig/test_connection.py index 14c4b2117..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 @@ -34,7 +32,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..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 @@ -35,7 +33,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..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 @@ -37,7 +35,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 159e2c14f..40158239c 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 @@ -34,7 +32,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 = [] @@ -108,7 +106,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() @@ -130,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._radio_link_statistics_cb, self._link_error_cb) self.assertIsNotNone(link) self.links.append(link) return link - def _link_quality_cb(self, percentage): + def _radio_link_statistics_cb(self, radoi_link_statistics): pass def _link_error_cb(self, errmsg): 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..042bd4c7d --- /dev/null +++ b/test/crazyflie/mem/test_lighthouse_memory.py @@ -0,0 +1,110 @@ +# -*- 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.crazyflie.mem import LighthouseBsCalibration +from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry + + +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/crazyflie/test_localization.py b/test/crazyflie/test_localization.py new file mode 100644 index 000000000..7306f4b6a --- /dev/null +++ b/test/crazyflie/test_localization.py @@ -0,0 +1,135 @@ +# -*- 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 . +import struct +import unittest +from unittest.mock import MagicMock + +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.localization import Localization +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort + + +class LocalizationTest(unittest.TestCase): + + def setUp(self): + self.cf_mock = MagicMock(spec=Crazyflie) + self.sut = Localization(crazyflie=self.cf_mock) + + def test_that_lighthouse_persist_data_is_correctly_encoded(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_persist_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( + '. 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 22e233740..579c769d2 100644 --- a/test/crazyflie/test_syncCrazyflie.py +++ b/test/crazyflie/test_syncCrazyflie.py @@ -19,39 +19,47 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 sys +# 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 from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper 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): 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() 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, - 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 + + # 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, self.cf_mock) + self.sut = SyncCrazyflie(self.uri, cf=self.cf_mock) def test_different_underlying_cf_instances(self): # Fixture @@ -102,6 +110,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() @@ -110,7 +138,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() @@ -136,7 +164,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 @@ -144,10 +172,67 @@ 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_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 + + # 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 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.cf_mock.fully_connected.callbacks)) + + def _connected_callback(self, uri): + AsyncCallbackCaller( + cb=self.cf_mock.fully_connected, + args=[self.uri], + delay=0.2 + ).trigger() diff --git a/test/crazyflie/test_syncLogger.py b/test/crazyflie/test_syncLogger.py index 0f5fe8bdc..5a81b8571 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 # @@ -19,13 +19,12 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 sys +# 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 +from unittest.mock import MagicMock from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import Log @@ -34,11 +33,6 @@ from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils.callbacks import Caller -if sys.version_info < (3, 3): - from mock import MagicMock -else: - from unittest.mock import MagicMock - class SyncLoggerTest(unittest.TestCase): @@ -52,7 +46,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 +62,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 +83,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 +126,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() @@ -167,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()) @@ -194,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 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/localization/__init__.py b/test/localization/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/test/localization/fixtures/parameters.yaml b/test/localization/fixtures/parameters.yaml new file mode 100644 index 000000000..b61abc437 --- /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: persistent_param_state +version: '1' diff --git a/test/localization/fixtures/system_config.yaml b/test/localization/fixtures/system_config.yaml new file mode 100644 index 000000000..3c18ad13b --- /dev/null +++ b/test/localization/fixtures/system_config.yaml @@ -0,0 +1,69 @@ +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 +systemType: 1 +type: lighthouse_system_configuration +version: '1' 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..557cd9437 --- /dev/null +++ b/test/localization/lighthouse_test_base.py @@ -0,0 +1,59 @@ +# -*- 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 scipy.spatial.transform import Rotation + +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}') + + _expected_rot = Rotation.from_rotvec(expected.rot_vec) + _actual_rot = Rotation.from_rotvec(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 + + angle_diff = _relative_rot.magnitude() + self.assertAlmostEqual(0.0, angle_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 new file mode 100644 index 000000000..8d490cd91 --- /dev/null +++ b/test/localization/test_lighthouse_bs_vector.py @@ -0,0 +1,164 @@ +# -*- 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 test.localization.lighthouse_test_base import LighthouseTestBase + +import numpy as np + +from cflib.localization import LighthouseBsVector +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors + + +class TestLighthouseBsVector(LighthouseTestBase): + 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) + + 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()) + + 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_config_manager.py b/test/localization/test_lighthouse_config_manager.py new file mode 100644 index 000000000..1658715a1 --- /dev/null +++ b/test/localization/test_lighthouse_config_manager.py @@ -0,0 +1,141 @@ +# -*- 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 unittest.mock import ANY +from unittest.mock import mock_open +from unittest.mock import patch + +import yaml + +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_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, 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): + # Fixture + fixture_file = 'test/localization/fixtures/system_config.yaml' + + file = open(fixture_file, 'r') + expected = yaml.safe_load(file) + file.close() + + # Test + 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, system_type=system_type) + + # 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') diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py new file mode 100644 index 000000000..ad2f2fd29 --- /dev/null +++ b/test/localization/test_lighthouse_geometry_solver.py @@ -0,0 +1,93 @@ +# -*- 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_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, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, + LhDeck4SensorPositions.positions) + + # Test + actual = LighthouseGeometrySolver.solve( + initial_guess, cleaned_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 = 7 + bs_id1 = 2 + bs_id2 = 9 + bs_id3 = 3 + 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, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, + LhDeck4SensorPositions.positions) + + # Test + actual = LighthouseGeometrySolver.solve( + initial_guess, cleaned_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) diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py new file mode 100644 index 000000000..b011558b5 --- /dev/null +++ b/test/localization/test_lighthouse_initial_estimator.py @@ -0,0 +1,179 @@ +# -*- 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 LhException +from cflib.localization.lighthouse_types import Pose + + +class TestLighthouseInitialEstimator(LighthouseTestBase): + def setUp(self): + self.fixtures = LighthouseFixtures() + + 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 + samples = [ + LhCfPoseSample(angles_calibrated={bs_id: self.fixtures.angles_cf_origin_bs0}), + ] + + # Test + # Assert + with self.assertRaises(LhException): + LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + + 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, cleaned_samples = 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) + + 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 = 7 + bs_id1 = 1 + 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, cleaned_samples = 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={ + 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, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) + + # Assert + 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 + # 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, cleaned_samples = 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_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_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_poses[bs_id2], places=3) + + def test_that_raises_for_isolated_bs(self): + # Fixture + bs_id0 = 3 + bs_id1 = 1 + bs_id2 = 2 + bs_id3 = 4 + 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, + bs_id3: self.fixtures.angles_cf2_bs2, + }), + ] + + # Test + # Assert + with self.assertRaises(LhException): + LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions) diff --git a/test/localization/test_lighthouse_sample_matcher.py b/test/localization/test_lighthouse_sample_matcher.py new file mode 100644 index 000000000..225be6f85 --- /dev/null +++ b/test/localization/test_lighthouse_sample_matcher.py @@ -0,0 +1,82 @@ +# -*- 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_out(self): + # Fixture + + # Test + 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)) diff --git a/test/localization/test_lighthouse_system_aligner.py b/test/localization/test_lighthouse_system_aligner.py new file mode 100644 index 000000000..0e5cea781 --- /dev/null +++ b/test/localization/test_lighthouse_system_aligner.py @@ -0,0 +1,117 @@ +# -*- 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_test_base import LighthouseTestBase + +import numpy as np + +from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner +from cflib.localization.lighthouse_types import Pose + + +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, transform = 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, transform = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses) + + # 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, transform = 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) diff --git a/test/localization/test_lighthouse_types.py b/test/localization/test_lighthouse_types.py new file mode 100644 index 000000000..dacc2e27b --- /dev/null +++ b/test/localization/test_lighthouse_types.py @@ -0,0 +1,98 @@ +# -*- 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_test_base import LighthouseTestBase + +import numpy as np + +from cflib.localization.lighthouse_types import Pose + + +class TestLighthouseTypes(LighthouseTestBase): + 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 + 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 = transform.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 + 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 = 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) diff --git a/test/localization/test_param_io.py b/test/localization/test_param_io.py new file mode 100644 index 000000000..1154e8a85 --- /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': 'persistent_param_state', + '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') diff --git a/test/positioning/test_motion_commander.py b/test/positioning/test_motion_commander.py index 7fd3d7ce1..a8b9855dd 100644 --- a/test/positioning/test_motion_commander.py +++ b/test/positioning/test_motion_commander.py @@ -17,13 +17,13 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 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 +31,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', @@ -260,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): @@ -275,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( @@ -294,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( @@ -313,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): @@ -330,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) @@ -349,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) @@ -373,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) @@ -397,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) diff --git a/test/positioning/test_position_hl_commander.py b/test/positioning/test_position_hl_commander.py index a347824d9..dd5ea929b 100644 --- a/test/positioning/test_position_hl_commander.py +++ b/test/positioning/test_position_hl_commander.py @@ -17,24 +17,19 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 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): @@ -48,43 +43,14 @@ def setUp(self): self.sut = PositionHlCommander(self.cf_mock) - def test_that_the_estimator_is_reset_on_take_off( - 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( + def test_that_controller_is_selected_on_creation( 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( - 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([ @@ -126,7 +92,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) @@ -149,6 +115,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) + + # 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 @@ -180,13 +160,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 +176,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 +188,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 +197,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 +208,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 +216,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 +227,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 +235,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 +246,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 +254,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 +265,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 +273,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 +284,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 +292,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 +303,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 +311,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 +322,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 +339,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 +347,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) diff --git a/test/support/asyncCallbackCaller.py b/test/support/asyncCallbackCaller.py index ecf896432..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 @@ -32,8 +30,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): 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/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_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_encoding.py b/test/utils/test_encoding.py new file mode 100644 index 000000000..57d97e72a --- /dev/null +++ b/test/utils/test_encoding.py @@ -0,0 +1,69 @@ +# -*- 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 +from cflib.utils.encoding import 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) diff --git a/test/utils/test_multiranger.py b/test/utils/test_multiranger.py index f4a514b47..60bfcc505 100644 --- a/test/utils/test_multiranger.py +++ b/test/utils/test_multiranger.py @@ -17,12 +17,12 @@ # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU 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 sys +# 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 unittest.mock import patch from cflib.crazyflie import Crazyflie from cflib.crazyflie import Log @@ -30,11 +30,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/test/utils/test_param_file_helper.py b/test/utils/test_param_file_helper.py new file mode 100644 index 000000000..4f8666cb2 --- /dev/null +++ b/test/utils/test_param_file_helper.py @@ -0,0 +1,120 @@ +# -*- 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 threading import Event +from unittest.mock import MagicMock +from unittest.mock import patch + +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils.param_file_helper import ParamFileHelper + + +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: + 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): + # 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', 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) + # 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.assertEqual(5, len(mock_Param.set_value.mock_calls)) + self.assertEqual(5, len(mock_Param.persistent_store.mock_calls)) diff --git a/tools/build-docs/build-docs b/tools/build-docs/build-docs new file mode 100755 index 000000000..7060ccb0d --- /dev/null +++ b/tools/build-docs/build-docs @@ -0,0 +1,46 @@ +#!/usr/bin/env bash + +set -e + +scriptDir=$(dirname $0) +cflibDir=$scriptDir/../../cflib +apiRefDir=$scriptDir/../../docs/api +templateDir=$apiRefDir/template +tempDir=$scriptDir/../../.tmp + +mkdir -p $apiRefDir +mkdir -p $tempDir + +[ -n "$API_REF_BASE" ] || { + export API_REF_BASE=$(readlink -fn $tempDir) +} + +# +# 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. +# + +# Make a temporary install in the temp dir +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 + +# 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 diff --git a/tools/build/bdist b/tools/build/bdist index 0aaf41489..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(['python', 'setup.py', 'bdist_wheel'], cwd=root) + subprocess.check_call(['python3', '-m', 'build', '--wheel'], cwd=root) print('Wheel built') except subprocess.CalledProcessError as e: diff --git a/tools/build/sys-test b/tools/build/sys-test index 924d6abdc..5ba60f83d 100755 --- a/tools/build/sys-test +++ b/tools/build/sys-test @@ -5,10 +5,10 @@ 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]) + subprocess.check_call(['python3', '-m', 'unittest', 'discover', root, '-v']) print('Unit tests pass') except subprocess.CalledProcessError as e: 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]) diff --git a/tools/crtp-dissector.lua b/tools/crtp-dissector.lua new file mode 100644 index 000000000..051010dff --- /dev/null +++ b/tools/crtp-dissector.lua @@ -0,0 +1,701 @@ +-- 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") +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") + +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") + +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") + +local f_crtp_echo_data = ProtoField.uint32("crtp.echo_data", "Echo Data") + +-- 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_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, + 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_echo_data, + f_crtp_undecoded, +} + +local param_toc = {} +local log_toc = {} + +local Links = { + UNKNOWN = 0, + RADIO = 1, + USB = 2, +} + +local link = Links.UNKNOWN +local undecoded = 0 +local crtp_start = 0 + +-- 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, + LinkControl = 0xF, + ALL = 0xFF +} + +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 == 0 then + channel_name = "Echo" + elseif 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() + + addr = addr:gsub("..", ":%0"):sub(2) + port = tostring(port) + + 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 +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 +end + +function append_param_type(str, type) + if #str == 0 then + return type + else + return 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 = "" + + 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_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) + + -- 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.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 + + 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 + + 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") +wtap_encap:add(wtap.USER15, crtp) 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