├── docs ├── build.py ├── multiwii.config.rst ├── multiwii.commands.rst ├── multiwii.messaging.rst ├── multiwii.rst ├── index.rst ├── multiwii.data.rst └── conf.py ├── examples ├── arm_disarm_vehicle_example.py ├── print_rc_channels_example.py ├── print_vehicle_info_example.py └── print_module_members_example.py ├── pyproject.toml ├── src └── multiwii │ ├── data │ ├── __init__.py │ ├── _types.py │ ├── motor.py │ ├── servo.py │ ├── rc.py │ ├── pid.py │ ├── telemetry.py │ ├── box.py │ ├── navigation.py │ └── info.py │ ├── config.py │ ├── messaging.py │ ├── commands.py │ ├── _command.py │ └── __init__.py ├── README.md ├── poetry.lock ├── LICENSE ├── .gitignore └── tests └── unit ├── test_box.py ├── test_telemetry.py ├── test_servo.py ├── test_motor.py ├── test_rc.py ├── test_navigation.py ├── test_info.py ├── test_pid.py ├── test_command.py ├── test_multiwii.py └── test_messaging.py /docs/build.py: -------------------------------------------------------------------------------- 1 | from subprocess import run 2 | 3 | run('sphinx-build -a -b html . _build/html', shell=True) 4 | -------------------------------------------------------------------------------- /docs/multiwii.config.rst: -------------------------------------------------------------------------------- 1 | Config 2 | ====== 3 | 4 | .. automodule:: multiwii.config 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/multiwii.commands.rst: -------------------------------------------------------------------------------- 1 | Commands 2 | ======== 3 | 4 | .. automodule:: multiwii.commands 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/multiwii.messaging.rst: -------------------------------------------------------------------------------- 1 | Messaging 2 | ========= 3 | 4 | .. automodule:: multiwii.messaging 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /docs/multiwii.rst: -------------------------------------------------------------------------------- 1 | multiwii API 2 | ============ 3 | 4 | .. toctree:: 5 | :maxdepth: 4 6 | 7 | multiwii.commands 8 | multiwii.config 9 | multiwii.data 10 | multiwii.messaging 11 | 12 | .. autoclass:: multiwii.MultiWii 13 | :members: 14 | :undoc-members: 15 | :show-inheritance: 16 | -------------------------------------------------------------------------------- /examples/arm_disarm_vehicle_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from multiwii import MultiWii 4 | from serial import Serial 5 | from time import sleep 6 | 7 | serial_port = Serial('/dev/ttyUSB0', baudrate=115200) 8 | 9 | multiwii = MultiWii(serial_port) 10 | 11 | multiwii.arm() 12 | 13 | sleep(2) 14 | 15 | multiwii.disarm() -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | Welcome to multiwii-proxy-python's documentation 2 | ================================================ 3 | 4 | - `Project page `_ 5 | - `multiwii-firmware `_ 6 | - `MultiWi Serial Protocol v1 `_ 7 | 8 | Contents 9 | -------- 10 | 11 | .. toctree:: 12 | :maxdepth: 4 13 | 14 | multiwii 15 | -------------------------------------------------------------------------------- /examples/print_rc_channels_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from multiwii import MultiWii 4 | from multiwii.commands import MSP_RC 5 | from serial import Serial 6 | 7 | serial_port = Serial('/dev/ttyUSB0', baudrate=115200) 8 | 9 | multiwii = MultiWii(serial_port) 10 | 11 | rc = multiwii.get_data(MSP_RC) 12 | 13 | print(rc.roll) 14 | print(rc.pitch) 15 | print(rc.yaw) 16 | print(rc.throttle) 17 | print(rc.aux1) 18 | print(rc.aux2) 19 | print(rc.aux3) 20 | print(rc.aux4) -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "multiwii-proxy-python" 3 | version = "3.0" 4 | description = "A simple and user-friendly Python 3 module for MultiWii-based drones." 5 | authors = ["BluDay"] 6 | license = "MIT" 7 | readme = "README.md" 8 | packages = [ 9 | { include = "multiwii", from = "src" } 10 | ] 11 | 12 | [tool.poetry.dependencies] 13 | python = "^3.10" 14 | pyserial = "^3.4" 15 | 16 | [build-system] 17 | requires = ["poetry-core"] 18 | build-backend = "poetry.core.masonry.api" 19 | -------------------------------------------------------------------------------- /examples/print_vehicle_info_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from multiwii import MultiWii 4 | 5 | from multiwii.commands import ( 6 | MSP_ANALOG, 7 | MSP_IDENT, 8 | MSP_MISC, 9 | MSP_STATUS 10 | ) 11 | 12 | from serial import Serial 13 | 14 | serial_port = Serial('/dev/ttyUSB0', baudrate=115200) 15 | 16 | multiwii = MultiWii(serial_port) 17 | 18 | print(multiwii.get_data(MSP_ANALOG)) 19 | print(multiwii.get_data(MSP_IDENT)) 20 | print(multiwii.get_data(MSP_MISC)) 21 | print(multiwii.get_data(MSP_STATUS)) -------------------------------------------------------------------------------- /src/multiwii/data/__init__.py: -------------------------------------------------------------------------------- 1 | from ._types import Coordinates, Pid, Point2D, Point3D 2 | 3 | from .box import MspBox, MspBoxIds, MspBoxItem, MspBoxNames 4 | 5 | from .info import MspAnalog, MspIdent, MspMisc, MspSetMisc, MspStatus 6 | 7 | from .motor import MspMotor, MspMotorPins 8 | 9 | from .navigation import MspCompGps, MspRawGps, MspWaypoint 10 | 11 | from .pid import MspPid, MspPidNames 12 | 13 | from .rc import MspRc, MspRcTuning 14 | 15 | from .servo import MspServo, MspServoConf, MspServoConfItem 16 | 17 | from .telemetry import MspAltitude, MspAttitude, MspRawImu -------------------------------------------------------------------------------- /examples/print_module_members_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from multiwii.commands import MSP_IDENT, MSP_MISC, MSP_RC 4 | from multiwii.config import MultiWiiMultitype 5 | from multiwii.data import MspRc 6 | 7 | from multiwii.messaging import ( 8 | MESSAGE_ERROR_HEADER, 9 | MESSAGE_INCOMING_HEADER, 10 | MESSAGE_OUTGOING_HEADER 11 | ) 12 | 13 | print(f'Error header : {MESSAGE_ERROR_HEADER}') 14 | print(f'Incoming header : {MESSAGE_INCOMING_HEADER}') 15 | print(f'Outgoing header : {MESSAGE_OUTGOING_HEADER}') 16 | 17 | print(repr(MSP_IDENT)) 18 | print(repr(MSP_MISC)) 19 | print(repr(MSP_RC)) 20 | 21 | print(repr(MultiWiiMultitype.QuadX)) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # multiwii-proxy-python 2 | 3 | Simple and user-friendly Python 3 module for controlling drones via the MultiWii Serial Protocol (MSP) v1. 4 | 5 | This module does not support MSP v2—or any of the newer versions. 6 | 7 | ## Documentation 8 | 9 | The API documentation can be found on the [documentation site](https://bluday.github.io/multiwii-proxy-python/). 10 | 11 | ## Installation 12 | 13 | Run either `pip install .` or `poetry build` to install the package: 14 | 15 | ## Usage 16 | 17 | Check the `examples` directory. 18 | 19 | ## Licensing 20 | 21 | This project is licensed under the MIT license. See [LICENSE](https://github.com/bluday/multiwii-proxy-python/blob/master/LICENSE) for more details. 22 | -------------------------------------------------------------------------------- /poetry.lock: -------------------------------------------------------------------------------- 1 | # This file is automatically @generated by Poetry 1.8.3 and should not be changed by hand. 2 | 3 | [[package]] 4 | name = "pyserial" 5 | version = "3.5" 6 | description = "Python Serial Port Extension" 7 | optional = false 8 | python-versions = "*" 9 | files = [ 10 | {file = "pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0"}, 11 | {file = "pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb"}, 12 | ] 13 | 14 | [package.extras] 15 | cp2110 = ["hidapi"] 16 | 17 | [metadata] 18 | lock-version = "2.0" 19 | python-versions = "^3.10" 20 | content-hash = "75bf5a6ba9c2dcc43a4922b2f7d88e400451a32f9a15a9777cca3f53e1b03d78" 21 | -------------------------------------------------------------------------------- /docs/multiwii.data.rst: -------------------------------------------------------------------------------- 1 | Data structures 2 | =============== 3 | 4 | .. automodule:: multiwii.data.box 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | .. automodule:: multiwii.data.info 10 | :members: 11 | :undoc-members: 12 | :show-inheritance: 13 | 14 | .. automodule:: multiwii.data.motor 15 | :members: 16 | :undoc-members: 17 | :show-inheritance: 18 | 19 | .. automodule:: multiwii.data.navigation 20 | :members: 21 | :undoc-members: 22 | :show-inheritance: 23 | 24 | .. automodule:: multiwii.data.pid 25 | :members: 26 | :undoc-members: 27 | :show-inheritance: 28 | 29 | .. automodule:: multiwii.data.rc 30 | :members: 31 | :undoc-members: 32 | :show-inheritance: 33 | 34 | .. automodule:: multiwii.data.servo 35 | :members: 36 | :undoc-members: 37 | :show-inheritance: 38 | 39 | .. automodule:: multiwii.data.telemetry 40 | :members: 41 | :undoc-members: 42 | :show-inheritance: 43 | 44 | Value types 45 | ----------- 46 | 47 | .. automodule:: multiwii.data._types 48 | :members: 49 | :undoc-members: 50 | :show-inheritance: 51 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # Text files 7 | *.txt 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | wheels/ 26 | *.egg-info/ 27 | .installed.cfg 28 | *.egg 29 | MANIFEST 30 | 31 | # PyInstaller 32 | # Usually these files are written by a python script from a template 33 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 34 | *.manifest 35 | *.spec 36 | 37 | # Installer logs 38 | pip-log.txt 39 | pip-delete-this-directory.txt 40 | 41 | # Unit test / coverage reports 42 | htmlcov/ 43 | .tox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | 53 | # Translations 54 | *.mo 55 | *.pot 56 | 57 | # Vim 58 | *.swp 59 | 60 | # pyenv 61 | .python-version 62 | 63 | # Environments 64 | .env 65 | .venv 66 | env/ 67 | venv/ 68 | ENV/ 69 | env.bak/ 70 | venv.bak/ 71 | 72 | # mkdocs documentation 73 | /site 74 | 75 | # Sphinx 76 | docs/_build 77 | docs/_static 78 | docs/_templates 79 | -------------------------------------------------------------------------------- /src/multiwii/data/_types.py: -------------------------------------------------------------------------------- 1 | from typing import Generic, NamedTuple, TypeVar 2 | 3 | T = TypeVar('T') 4 | 5 | class Coordinates(NamedTuple): 6 | """ 7 | Represents 2D geographics coordinates with longitude and latitude values. 8 | """ 9 | latitude: float 10 | """float: The latitude value of the coordinate.""" 11 | 12 | longitude: float 13 | """float: The longitude value of the coordinate.""" 14 | 15 | class Pid(NamedTuple, Generic[T]): 16 | """ 17 | Represents PID values. 18 | """ 19 | proportional: T 20 | """T: The proportional (P) component of the PID controller.""" 21 | 22 | integral: T 23 | """T: The integral (I) component of the PID controller.""" 24 | 25 | derivative: T 26 | """T: The derivative (D) component of the PID controller.""" 27 | 28 | class Point2D(NamedTuple, Generic[T]): 29 | """ 30 | Represents a 2D point. 31 | """ 32 | x: T 33 | """T: The x-coordinate of the point.""" 34 | 35 | y: T 36 | """T: The y-coordinate of the point.""" 37 | 38 | class Point3D(NamedTuple, Generic[T]): 39 | """ 40 | Represents a 3D point. 41 | """ 42 | x: T 43 | """T: The x-coordinate of the point.""" 44 | 45 | y: T 46 | """T: The y-coordinate of the point.""" 47 | 48 | z: T 49 | """T: The z-coordinate of the point.""" -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # For the full list of built-in configuration values, see the documentation: 4 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 5 | 6 | import os, sys 7 | 8 | sys.path.insert(0, os.path.abspath('../src/')) 9 | 10 | # -- Project information ----------------------------------------------------- 11 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information 12 | 13 | project = 'multiwii-proxy-python' 14 | 15 | copyright = '2024, BluDay' 16 | 17 | author = 'BluDay' 18 | 19 | release = '3.0' 20 | 21 | # -- General configuration --------------------------------------------------- 22 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration 23 | 24 | extensions = [ 25 | 'myst_parser', 26 | 'sphinx.ext.autodoc', 27 | 'sphinx.ext.autosummary', 28 | 'sphinx.ext.doctest', 29 | 'sphinx.ext.githubpages', 30 | 'sphinx.ext.napoleon', 31 | 'sphinx.ext.todo', 32 | 'sphinx.ext.viewcode' 33 | ] 34 | 35 | templates_path = ['_templates'] 36 | 37 | source_suffix = '.rst' 38 | 39 | root_doc = 'index' 40 | 41 | # -- Options for HTML output ------------------------------------------------- 42 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output 43 | 44 | html_theme = 'sphinx_rtd_theme' 45 | 46 | html_static_path = ['_static'] 47 | 48 | # -- Napoleon options -------------------------------------------------------- 49 | 50 | napoleon_google_docstring = False 51 | 52 | napoleon_numpy_docstring = True 53 | 54 | napoleon_include_init_with_doc = False 55 | 56 | napoleon_include_private_with_doc = False 57 | 58 | napoleon_include_special_with_doc = True 59 | 60 | napoleon_use_admonition_for_examples = False 61 | 62 | napoleon_use_admonition_for_notes = False 63 | 64 | napoleon_use_admonition_for_references = False 65 | 66 | napoleon_use_ivar = False 67 | 68 | napoleon_use_param = True 69 | 70 | napoleon_use_rtype = True 71 | 72 | napoleon_preprocess_types = False 73 | 74 | napoleon_type_aliases = None 75 | 76 | napoleon_attr_annotations = True 77 | -------------------------------------------------------------------------------- /src/multiwii/data/motor.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | from typing import Self 3 | 4 | @dataclass 5 | class MspMotor: 6 | """ 7 | Represents data values for the MSP_MOTOR command. 8 | 9 | This class encapsulates the motor speed values for up to eight motors in a MultiWii flight 10 | controller. Each motor's speed is represented as an integer value. 11 | """ 12 | motor1: int 13 | """int: The speed value for motor 1.""" 14 | 15 | motor2: int 16 | """int: The speed value for motor 2.""" 17 | 18 | motor3: int 19 | """int: The speed value for motor 3.""" 20 | 21 | motor4: int 22 | """int: The speed value for motor 4.""" 23 | 24 | motor5: int 25 | """int: The speed value for motor 5.""" 26 | 27 | motor6: int 28 | """int: The speed value for motor 6.""" 29 | 30 | motor7: int 31 | """int: The speed value for motor 7.""" 32 | 33 | motor8: int 34 | """int: The speed value for motor 8.""" 35 | 36 | @classmethod 37 | def parse(cls, data: tuple) -> Self: 38 | """ 39 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 40 | the `MspMotor` class. 41 | 42 | Parameters 43 | ---------- 44 | data : tuple 45 | A tuple containing unpacked data values. 46 | 47 | Returns 48 | ------- 49 | MspMotor 50 | An instance of the `MspMotor` class populated with the parsed data. 51 | """ 52 | return cls(*data) 53 | 54 | def as_serializable(self) -> tuple[int]: 55 | """ 56 | Returns a tuple with integer values to be used for serialization. 57 | 58 | Returns 59 | ------- 60 | tuple[int] 61 | A tuple with serializable integer values. 62 | """ 63 | return (motor1, motor2, motor3, motor4, motor5, motor6, motor7, motor8) 64 | 65 | @dataclass 66 | class MspMotorPins(MspMotor): 67 | """ 68 | Represents data values for the MSP_MOTOR_PINS command. 69 | 70 | This class extends `MspMotor` to provide the motor pin values for up to eight motors 71 | in a MultiWii flight controller. Each motor's pin value is represented as an integer 72 | value. 73 | """ 74 | @classmethod 75 | def parse(cls, data: tuple) -> Self: 76 | """ 77 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 78 | the `MspMotorPins` class. 79 | 80 | Parameters 81 | ---------- 82 | data : tuple 83 | A tuple containing unpacked data values. 84 | 85 | Returns 86 | ------- 87 | MspMotorPins 88 | An instance of the `MspMotorPins` class populated with the parsed data. 89 | """ 90 | return cls(*data) -------------------------------------------------------------------------------- /src/multiwii/data/servo.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | from typing import Self 3 | 4 | @dataclass 5 | class MspServo: 6 | """ 7 | Represents data values for the MSP_SERVO command. 8 | 9 | This class encapsulates the servo values for channels in the MultiWii flight controller. 10 | """ 11 | values: tuple[int] 12 | """tuple[int]: The servo output values for each channel.""" 13 | 14 | @classmethod 15 | def parse(cls, data: tuple) -> Self: 16 | """ 17 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 18 | the `MspServo` class. 19 | 20 | Parameters 21 | ---------- 22 | data : tuple 23 | A tuple containing unpacked data values. 24 | 25 | Returns 26 | ------- 27 | MspServo 28 | An instance of the `MspServo` class populated with the parsed data. 29 | """ 30 | return cls(*data) 31 | 32 | @dataclass 33 | class MspServoConfItem: 34 | """ 35 | Represents data values for the MSP_SET_SERVO_CONF command. 36 | 37 | This class encapsulates the configuration values for a single servo channel in the 38 | MultiWii flight controller. 39 | """ 40 | min: int 41 | """int: The minimum value for the servo endpoint.""" 42 | 43 | max: int 44 | """int: The maximum value for the servo endpoint.""" 45 | 46 | middle: int 47 | """int: The middle value for the servo endpoint.""" 48 | 49 | rate: int 50 | """int: The rate vlaue for the servo channel.""" 51 | 52 | @dataclass 53 | class MspServoConf: 54 | """ 55 | Represents data values for the MSP_SERVO_CONF command. 56 | 57 | This class encapsulates the servo configuration values for setting up servo endpoints, 58 | middle points, and rates in the MultiWii flight controller. 59 | """ 60 | values: tuple[MspServoConfItem] 61 | """tuple[MspServoConfItem]: The servo configuration values for each servo channel.""" 62 | 63 | @classmethod 64 | def parse(cls, data: tuple) -> Self: 65 | """ 66 | Parses a tuple of data values obtained from `struct.unpack` and returns aninstance of 67 | the `MspServoConf` class. 68 | 69 | Parameters 70 | ---------- 71 | data : tuple 72 | A tuple containing unpacked data values. 73 | 74 | Returns 75 | ------- 76 | MspServoConf 77 | An instance of the `MspServoConf` class populated with the parsed data. 78 | """ 79 | item_count = len(data) / 4 80 | 81 | values = () 82 | 83 | for index in range(item_count, step=4): 84 | item = ServoConfItem( 85 | min=data[index], 86 | max=data[index + 1], 87 | middle=data[index + 2], 88 | rate=data[index + 3] 89 | ) 90 | 91 | values += (item,) 92 | 93 | return cls(values) 94 | 95 | def as_serializable(self) -> tuple[int]: 96 | """ 97 | Returns a tuple with integer values to be used for serialization. 98 | 99 | Returns 100 | ------- 101 | tuple[int] 102 | A tuple with serializable integer values. 103 | """ 104 | values = () 105 | 106 | for item in self.values: 107 | values += ( 108 | item.min, 109 | item.max, 110 | item.middle, 111 | item.rate 112 | ) 113 | 114 | return values -------------------------------------------------------------------------------- /tests/unit/test_box.py: -------------------------------------------------------------------------------- 1 | from multiwii.config import MultiWiiBoxState 2 | from multiwii.data import MspBox, MspBoxIds, MspBoxItem, MspBoxNames 3 | 4 | import pytest 5 | 6 | @pytest.mark.parametrize("value, expected_aux1, expected_aux2, expected_aux3, expected_aux4", [ 7 | ( 8 | 0b101010101, 9 | MultiWiiBoxState.Low, 10 | MultiWiiBoxState.Mid, 11 | MultiWiiBoxState.High, 12 | MultiWiiBoxState.Empty 13 | ), 14 | ]) 15 | def test_msp_box_item_parse_valid(value, expected_aux1, expected_aux2, expected_aux3, expected_aux4): 16 | result = MspBoxItem.parse(value) 17 | 18 | assert isinstance(result, MspBoxItem) 19 | 20 | assert result.aux1 == expected_aux1 21 | assert result.aux2 == expected_aux2 22 | assert result.aux3 == expected_aux3 23 | assert result.aux4 == expected_aux4 24 | 25 | @pytest.mark.parametrize("item, expected_value", [ 26 | ( 27 | MspBoxItem( 28 | aux1=MultiWiiBoxState.Low, 29 | aux2=MultiWiiBoxState.Mid, 30 | aux3=MultiWiiBoxState.High, 31 | aux4=MultiWiiBoxState.Empty, 32 | ), 33 | 0b101010101 34 | ), 35 | ]) 36 | def test_msp_box_item_compile(item, expected_value): 37 | assert item.compile() == expected_value 38 | 39 | @pytest.mark.parametrize("data", [((0b101010101, 0b110101010))]) 40 | def test_msp_box_parse(data): 41 | result = MspBox.parse(data) 42 | 43 | assert isinstance(result, MspBox) 44 | assert len(result.items) == 2 45 | assert all(isinstance(item, MspBoxItem) for item in result.items) 46 | 47 | @pytest.mark.parametrize("item1, item2", [ 48 | ( 49 | MspBoxItem( 50 | aux1=MultiWiiBoxState.Low, 51 | aux2=MultiWiiBoxState.Mid, 52 | aux3=MultiWiiBoxState.High, 53 | aux4=MultiWiiBoxState.Empty 54 | ), 55 | MspBoxItem( 56 | aux1=MultiWiiBoxState.High, 57 | aux2=MultiWiiBoxState.Low, 58 | aux3=MultiWiiBoxState.Mid, 59 | aux4=MultiWiiBoxState.Empty 60 | ) 61 | ), 62 | ]) 63 | def test_msp_box_as_serializable(item1, item2): 64 | box = MspBox(items=(item1, item2)) 65 | 66 | serializable_data = box.as_serializable() 67 | 68 | assert isinstance(serializable_data, tuple) 69 | assert len(serializable_data) == 2 70 | assert all(isinstance(value, int) for value in serializable_data) 71 | 72 | @pytest.mark.parametrize("data", [((1, 2, 3))]) 73 | def test_msp_box_ids_parse(data): 74 | result = MspBoxIds.parse(data) 75 | 76 | assert isinstance(result, MspBoxIds) 77 | assert len(result.values) == 3 78 | assert all(isinstance(box, MultiWiiBox) for box in result.values) 79 | 80 | @pytest.mark.parametrize("data", [(b'Box1;Box2;Box3',)]) 81 | def test_msp_box_names_parse(data): 82 | result = MspBoxNames.parse(data) 83 | 84 | assert isinstance(result, MspBoxNames) 85 | assert len(result.names) == 3 86 | assert all(isinstance(name, str) for name in result.names) 87 | 88 | @pytest.mark.parametrize("data", [()]) 89 | def test_msp_box_names_parse_empty(data): 90 | result = MspBoxNames.parse(data) 91 | 92 | assert isinstance(result, MspBoxNames) 93 | assert len(result.names) == 0 94 | 95 | @pytest.mark.parametrize("value", [("invalid_value",)]) 96 | def test_msp_box_item_invalid_parse(value): 97 | with pytest.raises(ValueError): 98 | MspBoxItem.parse(value) 99 | 100 | @pytest.mark.parametrize("item", [MspBox(items=())]) 101 | def test_msp_box_as_serializable_empty(item): 102 | assert item.as_serializable() == () 103 | -------------------------------------------------------------------------------- /src/multiwii/config.py: -------------------------------------------------------------------------------- 1 | from enum import IntEnum, unique 2 | 3 | from typing import Self 4 | 5 | @unique 6 | class MultiWiiBox(IntEnum): 7 | """ 8 | Represents the various boxes that can be checked in a MultiWii flight controller. 9 | 10 | Each box corresponds to a specific function or mode that can be activated in the 11 | flight controller's configuration. 12 | """ 13 | Arm = 0 14 | Angle = 1 15 | Horizon = 2 16 | Baro = 3 17 | Vario = 4 18 | Mag = 5 19 | HeadFree = 6 20 | HeadAdj = 7 21 | CamStab = 8 22 | CamTrig = 9 23 | GpsHome = 10 24 | GpsHold = 11 25 | Passthru = 12 26 | Beeper = 13 27 | LedMax = 14 28 | LedLow = 15 29 | LLights = 16 30 | Calib = 17 31 | Governor = 18 32 | OsdSwitch = 19 33 | Mission = 20 34 | Land = 21 35 | 36 | @unique 37 | class MultiWiiBoxState(IntEnum): 38 | """ 39 | Represents the state of an auxiliary (aux) control box in MultiWii flight controller. 40 | 41 | The state indicates whether the box is unselected (Empty), or selected at a LOW (Low), 42 | MID (Mid), or HIGH (High) position. 43 | """ 44 | Empty = 0b000 45 | Low = 0b001 46 | Mid = 0b010 47 | High = 0b100 48 | 49 | @classmethod 50 | def parse(cls, value: int) -> Self: 51 | """ 52 | Gets a parsed value from the provided integer value. 53 | 54 | Parameters 55 | ---------- 56 | value : int 57 | The integer value to evaluate the first three bits from. 58 | 59 | Returns 60 | ------- 61 | A parsed value or `Empty` if unsuccessful. 62 | """ 63 | value = value & 0x7 64 | 65 | if value & cls.Low.value: 66 | return cls.Low 67 | 68 | if value & cls.Mid.value: 69 | return cls.Mid 70 | 71 | if value & cls.High.value: 72 | return cls.High 73 | 74 | return cls.Empty 75 | 76 | @unique 77 | class MultiWiiCapability(IntEnum): 78 | """ 79 | Represents various capabilities of a MultiWii flight controller. 80 | 81 | Each capability indicates a specific feature that the flight controller supports. 82 | """ 83 | Bind = 0b00001 84 | Dynbal = 0b00010 85 | Flap = 0b00100 86 | Nav = 0b01000 87 | ExtAux = 0b10000 88 | 89 | @unique 90 | class MultiWiiMultitype(IntEnum): 91 | """ 92 | Enumeration of vehicle configurations available in MultiWii. 93 | 94 | These configurations represents different types of aircraft or vehicle setups 95 | that the flight controller can be configured to control. 96 | """ 97 | Unidentified = 0 98 | Tri = 1 99 | QuadP = 2 100 | QuadX = 3 101 | Bi = 4 102 | Gimbal = 5 103 | Y6 = 6 104 | Hex6 = 7 105 | FlyingWing = 8 106 | Y4 = 9 107 | Hex6X = 10 108 | OctoX8 = 11 109 | OctoflatP = 12 110 | OctoflatX = 13 111 | Airplane = 14 112 | Heli120CCPM = 15 113 | Heli90Deg = 16 114 | VTail4 = 17 115 | Hex6H = 18 116 | Singlecopter = 21 117 | Dualcopter = 20 118 | 119 | @unique 120 | class MultiWiiSensor(IntEnum): 121 | """ 122 | Enumeration of sensor types supported by MultiWii. 123 | 124 | These sensor types indicate the various sensors that can be used with the 125 | MultiWii flight controller to provide additional data and functionality. 126 | """ 127 | Acc = 0 128 | Baro = 1 129 | Mag = 2 130 | Gps = 3 131 | Sonar = 4 -------------------------------------------------------------------------------- /tests/unit/test_telemetry.py: -------------------------------------------------------------------------------- 1 | from multiwii.data import MspAltitude, MspAttitude, MspRawImu, Point3D 2 | 3 | import pytest 4 | 5 | @pytest.mark.parametrize( 6 | "data, expected_estimation, expected_pressure_variation", 7 | [ 8 | ( 9 | (12345, 678), 10 | 12345, 11 | 678 12 | ), 13 | ] 14 | ) 15 | def test_msp_altitude_parse(data, expected_estimation, expected_pressure_variation): 16 | result = MspAltitude.parse(data) 17 | 18 | assert isinstance(result, MspAltitude) 19 | assert result.estimation == expected_estimation 20 | assert result.pressure_variation == expected_pressure_variation 21 | 22 | @pytest.mark.parametrize( 23 | "data", 24 | [ 25 | (12345,), 26 | ] 27 | ) 28 | def test_msp_altitude_parse_invalid_data(data): 29 | with pytest.raises(TypeError): 30 | MspAltitude.parse(data) 31 | 32 | @pytest.mark.parametrize( 33 | "data, expected_pitch, expected_roll, expected_yaw", 34 | [ 35 | ( 36 | (2000, 1500, 0, 180), 37 | 200.0, 38 | 150.0, 39 | 180 40 | ), 41 | ] 42 | ) 43 | def test_msp_attitude_parse(data, expected_pitch, expected_roll, expected_yaw): 44 | result = MspAttitude.parse(data) 45 | 46 | assert isinstance(result, MspAttitude) 47 | assert result.pitch_angle == expected_pitch 48 | assert result.roll_angle == expected_roll 49 | assert result.yaw_angle == expected_yaw 50 | 51 | @pytest.mark.parametrize( 52 | "data", 53 | [ 54 | (2000, 1500), 55 | ] 56 | ) 57 | def test_msp_attitude_parse_invalid_data(data): 58 | with pytest.raises(TypeError): 59 | MspAttitude.parse(data) 60 | 61 | @pytest.mark.parametrize( 62 | "data, expected_accelerometer, expected_gyroscope, expected_magnetometer", 63 | [ 64 | ( 65 | ( 66 | 1000, 1500, 2000, 67 | 3000, 3500, 4000, 68 | 5000, 5500, 6000 69 | ), 70 | Point3D(1000, 1500, 2000), 71 | Point3D(3000, 3500, 4000), 72 | Point3D(5000, 5500, 6000), 73 | ), 74 | ] 75 | ) 76 | def test_msp_raw_imu_parse(data, expected_accelerometer, expected_gyroscope, expected_magnetometer): 77 | result = MspRawImu.parse(data) 78 | 79 | assert isinstance(result, MspRawImu) 80 | assert result.accelerometer == expected_accelerometer 81 | assert result.gyroscope == expected_gyroscope 82 | assert result.magnetometer == expected_magnetometer 83 | 84 | @pytest.mark.parametrize( 85 | "data, expected_accelerometer, expected_gyroscope, expected_magnetometer", 86 | [ 87 | ( 88 | ( 89 | 1000, 1500, 2000, 90 | 3000, 3500, 4000, 91 | 5000, 5500, 6000 92 | ), 93 | Point3D(500.0, 750.0, 1000.0), 94 | Point3D(1500.0, 1750.0, 2000.0), 95 | Point3D(2500.0, 2750.0, 3000.0), 96 | ), 97 | ] 98 | ) 99 | def test_msp_raw_imu_parse_with_units(data, expected_accelerometer, expected_gyroscope, expected_magnetometer): 100 | result = MspRawImu.parse( 101 | data, 102 | accelerometer_unit=2.0, 103 | gyroscope_unit=2.0, 104 | magnetometer_unit=2.0 105 | ) 106 | 107 | assert isinstance(result, MspRawImu) 108 | assert result.accelerometer == expected_accelerometer 109 | assert result.gyroscope == expected_gyroscope 110 | assert result.magnetometer == expected_magnetometer 111 | 112 | @pytest.mark.parametrize( 113 | "data", 114 | [ 115 | ( 116 | 1000, 1500, 2000, 117 | 3000, 3500, 4000, 118 | 5000, 5500 119 | ), 120 | ] 121 | ) 122 | def test_msp_raw_imu_parse_invalid_data(data): 123 | with pytest.raises(TypeError): 124 | MspRawImu.parse(data) 125 | -------------------------------------------------------------------------------- /tests/unit/test_servo.py: -------------------------------------------------------------------------------- 1 | from multiwii.data import MspServo, MspServoConf, MspServoConfItem 2 | 3 | import pytest 4 | 5 | @pytest.mark.parametrize( 6 | "data, expected_values", 7 | [ 8 | ((1000, 1500, 2000, 2500), (1000, 1500, 2000, 2500)), 9 | ] 10 | ) 11 | def test_msp_servo_parse(data, expected_values): 12 | result = MspServo.parse(data) 13 | 14 | assert isinstance(result, MspServo) 15 | 16 | assert result.values == expected_values 17 | 18 | @pytest.mark.parametrize( 19 | "data", 20 | [ 21 | (1000, 1500), 22 | ] 23 | ) 24 | def test_msp_servo_parse_invalid_data(data): 25 | with pytest.raises(TypeError): 26 | MspServo.parse(data) 27 | 28 | @pytest.mark.parametrize( 29 | "values, expected_serializable", 30 | [ 31 | ((1000, 1500, 2000, 2500), (1000, 1500, 2000, 2500)), 32 | ] 33 | ) 34 | def test_msp_servo_as_serializable(values, expected_serializable): 35 | servo = MspServo(values=values) 36 | 37 | serializable_data = servo.as_serializable() 38 | 39 | assert isinstance(serializable_data, tuple) 40 | assert serializable_data == expected_serializable 41 | 42 | @pytest.mark.parametrize( 43 | "values, expected_serializable", 44 | [ 45 | ((), ()), 46 | ] 47 | ) 48 | def test_msp_servo_as_serializable_empty(values, expected_serializable): 49 | servo = MspServo(values=()) 50 | 51 | serializable_data = servo.as_serializable() 52 | 53 | assert serializable_data == expected_serializable 54 | 55 | @pytest.mark.parametrize( 56 | "data, expected_values", 57 | [ 58 | ( 59 | (1000, 1500, 2000, 2500, 1100, 1600, 2100, 2600), 60 | [ 61 | MspServoConfItem(min=1000, max=1500, middle=2000, rate=2500), 62 | MspServoConfItem(min=1100, max=1600, middle=2100, rate=2600), 63 | ] 64 | ), 65 | ] 66 | ) 67 | def test_msp_servo_conf_parse(data, expected_values): 68 | result = MspServoConf.parse(data) 69 | 70 | assert isinstance(result, MspServoConf) 71 | 72 | assert len(result.values) == len(expected_values) 73 | 74 | for i, item in enumerate(result.values): 75 | assert item.min == expected_values[i].min 76 | assert item.max == expected_values[i].max 77 | assert item.middle == expected_values[i].middle 78 | assert item.rate == expected_values[i].rate 79 | 80 | @pytest.mark.parametrize( 81 | "data", 82 | [ 83 | (1000, 1500, 2000, 2500, 1100, 1600), 84 | ] 85 | ) 86 | def test_msp_servo_conf_parse_invalid_data(data): 87 | with pytest.raises(TypeError): 88 | MspServoConf.parse(data) 89 | 90 | @pytest.mark.parametrize( 91 | "conf_item_1, conf_item_2, expected_serializable", 92 | [ 93 | ( 94 | MspServoConfItem(min=1000, max=1500, middle=2000, rate=2500), 95 | MspServoConfItem(min=1100, max=1600, middle=2100, rate=2600), 96 | (1000, 1500, 2000, 2500, 1100, 1600, 2100, 2600), 97 | ), 98 | ] 99 | ) 100 | def test_msp_servo_conf_as_serializable(conf_item_1, conf_item_2, expected_serializable): 101 | servo_conf = MspServoConf(values=(conf_item_1, conf_item_2)) 102 | 103 | serializable_data = servo_conf.as_serializable() 104 | 105 | assert isinstance(serializable_data, tuple) 106 | assert serializable_data == expected_serializable 107 | 108 | @pytest.mark.parametrize( 109 | "conf_item, expected_serializable", 110 | [ 111 | (MspServoConfItem(min=0, max=0, middle=0, rate=0), (0, 0, 0, 0)), 112 | ] 113 | ) 114 | def test_msp_servo_conf_as_serializable_empty(conf_item, expected_serializable): 115 | servo_conf = MspServoConf(values=(conf_item,)) 116 | 117 | serializable_data = servo_conf.as_serializable() 118 | 119 | assert serializable_data == expected_serializable 120 | -------------------------------------------------------------------------------- /src/multiwii/data/rc.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | from typing import Self 3 | 4 | @dataclass 5 | class MspRc: 6 | """ 7 | Represents data values for the MSP_RC command. 8 | 9 | This class encapsulates the RC (Remote Control) input values for various control axes in 10 | the MultiWii flight controller. 11 | """ 12 | roll: int 13 | """int: The input value for the roll axis.""" 14 | 15 | pitch: int 16 | """int: The input value for the pitch axis.""" 17 | 18 | yaw: int 19 | """int: The input value for the yaw axis.""" 20 | 21 | throttle: int 22 | """int: The input value for the throttle.""" 23 | 24 | aux1: int 25 | """int: The input value for the first auxiliary channel.""" 26 | 27 | aux2: int 28 | """int: The input value for the second auxiliary channel.""" 29 | 30 | aux3: int 31 | """int: The input value for the third auxiliary channel.""" 32 | 33 | aux4: int 34 | """int: The input value for the fourth auxiliary channel.""" 35 | 36 | @classmethod 37 | def parse(cls, data: tuple) -> Self: 38 | """ 39 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 40 | the `MspRc` class. 41 | 42 | Parameters 43 | ---------- 44 | data : tuple 45 | A tuple containing unpacked data values. 46 | 47 | Returns 48 | ------- 49 | MspRc 50 | An instance of the `MspRc` class populated with the parsed data. 51 | """ 52 | return cls(*data) 53 | 54 | def as_serializable(self) -> tuple[int]: 55 | """ 56 | Returns a tuple with integer values to be used for serialization. 57 | 58 | Returns 59 | ------- 60 | tuple[int] 61 | A tuple with serializable integer values. 62 | """ 63 | return (roll, pitch, yaw, throttle, aux1, aux2, aux3, aux4) 64 | 65 | @dataclass 66 | class MspRcTuning: 67 | """ 68 | Represents data values for the MSP_RC_TUNING command. 69 | 70 | This class encapsulates the tuning parameters for the RC (Remote Control) inputs 71 | in the MultiWii flight controller. It provides information about the rates, expo, 72 | and throttle settings. 73 | """ 74 | rate: int 75 | """int: The RC rate.""" 76 | 77 | expo: int 78 | """int: The RC expo.""" 79 | 80 | roll_pitch_rate: int 81 | """int: The roll and pitch rate.""" 82 | 83 | yaw_rate: int 84 | """int: The yaw rate.""" 85 | 86 | dynamic_throttle_pid: int 87 | """int: The dynamic throttle PID.""" 88 | 89 | throttle_mid: int 90 | """int: The throttle mid-point value.""" 91 | 92 | throttle_expo: int 93 | """int: The throttle expo value.""" 94 | 95 | @classmethod 96 | def parse(cls, data: tuple) -> Self: 97 | """ 98 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 99 | the `MspRcTuning` class. 100 | 101 | Parameters 102 | ---------- 103 | data : tuple 104 | A tuple containing unpacked data values. 105 | 106 | Returns 107 | ------- 108 | MspRcTuning 109 | An instance of the `MspRcTuning` class populated with the parsed data. 110 | """ 111 | return cls(*data) 112 | 113 | def as_serializable(self) -> tuple[int]: 114 | """ 115 | Returns a tuple with integer values to be used for serialization. 116 | 117 | Returns 118 | ------- 119 | tuple[int] 120 | A tuple with serializable integer values. 121 | """ 122 | return ( 123 | rate, 124 | expo, 125 | roll_pitch_rate, 126 | yaw_rate, 127 | dynamic_throttle_pid, 128 | throttle_mid, 129 | throttle_expo 130 | ) -------------------------------------------------------------------------------- /src/multiwii/data/pid.py: -------------------------------------------------------------------------------- 1 | from . import Pid 2 | 3 | from ..messaging import _decode_names 4 | 5 | from dataclasses import dataclass 6 | from typing import Self 7 | 8 | @dataclass 9 | class MspPid: 10 | """ 11 | Represents data values for the MSP_PID command. 12 | 13 | This class encapsulates the PID controller settings for various control axes and functions 14 | in the MultiWii flight controller. 15 | """ 16 | roll: Pid[int] 17 | """Pid[int]: PID values for the roll axis.""" 18 | 19 | pitch: Pid[int] 20 | """Pid[int]: PID values for the pitch axis.""" 21 | 22 | yaw: Pid[int] 23 | """Pid[int]: PID values for the yaw axis.""" 24 | 25 | altitude_hold: Pid[int] 26 | """Pid[int]: PID values for the altitude hold.""" 27 | 28 | position_hold: Pid[int] 29 | """Pid[int]: PID values for the position hold.""" 30 | 31 | position_rate: Pid[int] 32 | """Pid[int]: PID values for the position rate.""" 33 | 34 | navigation_rate: Pid[int] 35 | """Pid[int]: PID values for the navigation rate.""" 36 | 37 | level_mode: Pid[int] 38 | """Pid[int]: PID values for the level mode.""" 39 | 40 | magnetometer: Pid[int] 41 | """Pid[int]: PID values for the magnetometer.""" 42 | 43 | velocity: Pid[int] 44 | """Pid[int]: PID values for the velocity.""" 45 | 46 | @classmethod 47 | def parse(cls, data: tuple) -> Self: 48 | """ 49 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 50 | the `MspPid` class. 51 | 52 | Parameters 53 | ---------- 54 | data : tuple 55 | A tuple containing unpacked data values. 56 | 57 | Returns 58 | ------- 59 | MspPid 60 | An instance of the `MspPid` class populated with the parsed data. 61 | """ 62 | pid_collection = () 63 | 64 | for index in range(len(data), step=3): 65 | pid = Pid( 66 | p=data[index], 67 | i=data[index + 1], 68 | d=data[index + 2] 69 | ) 70 | 71 | pid_collection += (pid,) 72 | 73 | return cls(*pid_collection) 74 | 75 | def as_serializable(self) -> tuple[int]: 76 | """ 77 | Returns a tuple with integer values to be used for serialization. 78 | 79 | Returns 80 | ------- 81 | tuple[int] 82 | A tuple with serializable integer values. 83 | """ 84 | return ( 85 | roll.p, 86 | roll.i, 87 | roll.d, 88 | 89 | pitch.p, 90 | pitch.i, 91 | pitch.d, 92 | 93 | yaw.p, 94 | yaw.i, 95 | yaw.d, 96 | 97 | altitude_hold.p, 98 | altitude_hold.i, 99 | altitude_hold.d, 100 | 101 | position_hold.p, 102 | position_hold.i, 103 | position_hold.d, 104 | 105 | position_rate.p, 106 | position_rate.i, 107 | position_rate.d, 108 | 109 | navigation_rate.p, 110 | navigation_rate.i, 111 | navigation_rate.d, 112 | 113 | level_mode.p, 114 | level_mode.i, 115 | level_mode.d, 116 | 117 | magnetometer.p, 118 | magnetometer.i, 119 | magnetometer.d, 120 | 121 | velocity.p, 122 | velocity.i, 123 | velocity.d 124 | ) 125 | 126 | @dataclass 127 | class MspPidNames: 128 | """ 129 | Represents data values for the MSP_PIDNAMES command. 130 | 131 | This class is used to store the names of various PID controllers used in the MultiWii 132 | flight controller. Each name corresponds to a specific PID controller setting. 133 | """ 134 | names: tuple[str] 135 | """tuple[str]: The names of the PID controllers.""" 136 | 137 | @classmethod 138 | def parse(cls, data: tuple) -> Self: 139 | """ 140 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 141 | the `MspPidNames` class. 142 | 143 | Parameters 144 | ---------- 145 | data : tuple 146 | A tuple containing unpacked data values. 147 | 148 | Returns 149 | ------- 150 | MspPidNames 151 | An instance of the `MspPidNames` class populated with the parsed data. 152 | """ 153 | return cls(_decode_names(data)) -------------------------------------------------------------------------------- /src/multiwii/messaging.py: -------------------------------------------------------------------------------- 1 | from ._command import _MspCommand 2 | 3 | from typing import Final, NamedTuple 4 | from struct import pack, unpack 5 | 6 | MESSAGE_ERROR_HEADER: Final[bytes] = b'$M!' 7 | """bytes: The serialized error message header. (0x24, 0x4d, 0x21)""" 8 | 9 | MESSAGE_INCOMING_HEADER: Final[bytes] = b'$M<' 10 | """bytes: The serialized incoming message header. (0x24, 0x4d, 0x3c)""" 11 | 12 | MESSAGE_OUTGOING_HEADER: Final[bytes] = b'$M>' 13 | """bytes: The serialized outgoing message header. (0x24, 0x4d, 0x3e)""" 14 | 15 | class _MspResponseMessage(NamedTuple): 16 | """ 17 | Represents a tuple with the data size and values for a received MSP message. 18 | 19 | Attributes 20 | ---------- 21 | command : _MspCommand 22 | An instance of `_MspCommand` of the targeted MSP command. 23 | data : tuple[int] 24 | A tuple of parsed data values. 25 | data_size : int 26 | The size of the unserialized data values. 27 | """ 28 | command: _MspCommand 29 | 30 | data: tuple[int] 31 | 32 | data_size: int 33 | 34 | class MspMessageError(Exception): 35 | """Represents a specific errors related to MSP messages.""" 36 | pass 37 | 38 | def _crc8_xor(payload: bytes) -> int: 39 | """ 40 | Calculates the checksum for the payload using an XOR CRC. 41 | 42 | Parameters 43 | ---------- 44 | payload : bytes 45 | Bytes of the full payload (including the command code and size). 46 | 47 | Returns 48 | ------- 49 | int 50 | The checksum for the provided payload. 51 | """ 52 | checksum = 0 53 | 54 | for byte in payload: checksum ^= byte 55 | 56 | return checksum & 0xff 57 | 58 | def _create_request_message(command: _MspCommand, data: tuple[int]) -> bytes: 59 | """ 60 | Constructs a serialized message for a provided command and data values. 61 | 62 | Attributes 63 | ---------- 64 | command : _MspCommand 65 | An instance of `_MspCommand` representing the MSP command used to create the 66 | message. 67 | data : tuple[int] 68 | The data values to serialize and include in the payload. 69 | 70 | Returns 71 | ------- 72 | bytes 73 | The full message in bytes. 74 | """ 75 | data_size = 0 76 | 77 | payload_content = bytes() 78 | 79 | if data: 80 | if command.has_variable_size: 81 | data_size = len(data) / command.data_field_count 82 | else: 83 | data_size = command.data_size 84 | 85 | payload_content = pack(command.data_struct_format, *data) 86 | 87 | payload_header = pack('<2B', data_size, command.code) 88 | 89 | payload = payload_header + payload_content 90 | 91 | checksum = pack(' tuple[str]: 96 | """ 97 | Decodes the deserialized string value and splits it to a tuple. 98 | 99 | Parameters 100 | ---------- 101 | data : tuple 102 | The `struct` unpacked payload tuple to decode. 103 | 104 | Returns 105 | ------- 106 | tuple[str] 107 | A tuple of decoded names. 108 | """ 109 | return tuple(data[0].decode('ascii').split(';')) 110 | 111 | def _parse_response_message(command: _MspCommand, payload: bytes) -> _MspResponseMessage: 112 | """ 113 | Parses the payload of a response message for a given command. 114 | 115 | Attributes 116 | ---------- 117 | command : _MspCommand 118 | An instance of `_MspCommand` representing the MSP command for the response message. 119 | payload : bytes 120 | The received payload buffer from a response message. 121 | 122 | Raises 123 | ------ 124 | ValueError 125 | If the command code in the payload does not match the code of the provided 126 | command. 127 | 128 | Returns 129 | ------- 130 | _MspResponseMessage 131 | A named tuple with the command, parsed data and additional information. 132 | """ 133 | command_code = payload[1] 134 | 135 | if command_code != command.code: 136 | raise ValueError( 137 | 'Payload with an invalid command code detected. ({}, {})'.format( 138 | command.code, 139 | command_code 140 | ) 141 | ) 142 | 143 | data = unpack(command.data_struct_format, payload[2:]) 144 | 145 | data_size = payload[0] 146 | 147 | return _MspResponseMessage(command, data, data_size) -------------------------------------------------------------------------------- /src/multiwii/data/telemetry.py: -------------------------------------------------------------------------------- 1 | from . import Point3D 2 | 3 | from dataclasses import dataclass 4 | from typing import Self 5 | 6 | @dataclass 7 | class MspAltitude: 8 | """ 9 | Represents data values for the MSP_ALTITUDE command. 10 | 11 | This class encapsulates altitude-related data from a MultiWii flight controller. 12 | """ 13 | estimation: int 14 | """int: The estimated altitude.""" 15 | 16 | pressure_variation: int 17 | """int: The variation in pressure.""" 18 | 19 | @classmethod 20 | def parse(cls, data: tuple) -> Self: 21 | """ 22 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 23 | the `MspAltitude` class. 24 | 25 | Parameters 26 | ---------- 27 | data : tuple 28 | A tuple containing unpacked data values. 29 | 30 | Returns 31 | ------- 32 | MspAltitude 33 | An instance of the `MspAltitude` class populated with the parsed data. 34 | """ 35 | return cls( 36 | estimation=((data[3] << 24) | (data[2] << 16) | (data[1] << 8) | data[0]), 37 | pressure_variation=((data[5] << 8) | data[4]) 38 | ) 39 | 40 | @dataclass 41 | class MspAttitude: 42 | """ 43 | Represents data values for the MSP_ATTITUDE command. 44 | 45 | This class encapsulates attitude-related data from a MultiWii flight controller. 46 | """ 47 | pitch_angle: float 48 | """float: The pitch angle of the aircraft in degrees, ranging from -180.0 to 180.0.""" 49 | 50 | roll_angle: float 51 | """float: The roll angle of the aircraft in degrees, ranging from -90.0 to 90.0.""" 52 | 53 | yaw_angle: int 54 | """int: The heading angle of the aircraft in degrees, ranging from 0 to 360.""" 55 | 56 | @classmethod 57 | def parse(cls, data: tuple) -> Self: 58 | """ 59 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 60 | the `MspAttitude` class. 61 | 62 | Parameters 63 | ---------- 64 | data : tuple 65 | A tuple containing unpacked data values. 66 | 67 | Returns 68 | ------- 69 | MspAttitude 70 | An instance of the `MspAttitude` class populated with the parsed data. 71 | """ 72 | return cls( 73 | pitch_angle=data[0] / 10.0, 74 | roll_angle=data[1] / 10.0, 75 | yaw_angle=data[3] 76 | ) 77 | 78 | @dataclass 79 | class MspRawImu: 80 | """ 81 | Represents data values for the MSP_RAW_IMU command. 82 | 83 | This class encapsulates raw IMU (Intertial Measurement Unit) data from a MultiWii 84 | flight controller. 85 | """ 86 | accelerometer: Point3D[float] 87 | """Point3D[float]: The accelerometer data.""" 88 | 89 | gyroscope: Point3D[float] 90 | """Point3D[float]: The gyroscope data.""" 91 | 92 | magnetometer: Point3D[float] 93 | """Point3D[float]: The magnetometer data.""" 94 | 95 | @classmethod 96 | def parse( 97 | cls, 98 | data: tuple, 99 | accelerometer_unit: int = 1.0, 100 | gyroscope_unit: int = 1.0, 101 | magnetometer_unit: int = 1.0 102 | ) -> Self: 103 | """ 104 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 105 | the `MspRawImu` class. 106 | 107 | Parameters 108 | ---------- 109 | data : tuple 110 | A tuple containing unpacked data values. 111 | accelerometer_unit : int, optional 112 | The unit conversion factor for the accelerometer data (default is 1.0). 113 | gyroscope_unit : int, optional 114 | The unit conversion factor for the gyroscope data (default is 1.0). 115 | magnetometer_unit : int, optional 116 | The unit conversion factor for the magnetometer data (default is 1.0). 117 | 118 | Returns 119 | ------- 120 | MspRawImu 121 | An instance of the `MspRawImu` class populated with the parsed data. 122 | """ 123 | return cls( 124 | accelerometer=Point3D( 125 | x=data[0] / accelerometer_unit, 126 | y=data[1] / accelerometer_unit, 127 | z=data[2] / accelerometer_unit 128 | ), 129 | gyroscope=Point3D( 130 | x=data[3] / gyroscope_unit, 131 | y=data[4] / gyroscope_unit, 132 | z=data[5] / gyroscope_unit 133 | ), 134 | magnetometer=Point3D( 135 | x=data[6] / magnetometer_unit, 136 | y=data[7] / magnetometer_unit, 137 | z=data[8] / magnetometer_unit 138 | ) 139 | ) -------------------------------------------------------------------------------- /tests/unit/test_motor.py: -------------------------------------------------------------------------------- 1 | from multiwii.data import MspMotor, MspMotorPins 2 | 3 | import pytest 4 | 5 | @pytest.mark.parametrize("data, expected_motor_values", [ 6 | ( 7 | (1000, 1200, 1300, 1400, 1500, 1600, 1700, 1800), 8 | (1000, 1200, 1300, 1400, 1500, 1600, 1700, 1800) 9 | ), 10 | ( 11 | (2000, 2100, 2200, 2300, 2400, 2500, 2600, 2700), 12 | (2000, 2100, 2200, 2300, 2400, 2500, 2600, 2700) 13 | ), 14 | ( 15 | (3000, 3100, 3200, 3300, 3400, 3500, 3600, 3700), 16 | (3000, 3100, 3200, 3300, 3400, 3500, 3600, 3700) 17 | ) 18 | ]) 19 | def test_msp_motor_parse(data, expected_motor_values): 20 | result = MspMotor.parse(data) 21 | 22 | assert isinstance(result, MspMotor) 23 | assert result.motor1 == expected_motor_values[0] 24 | assert result.motor2 == expected_motor_values[1] 25 | assert result.motor3 == expected_motor_values[2] 26 | assert result.motor4 == expected_motor_values[3] 27 | assert result.motor5 == expected_motor_values[4] 28 | assert result.motor6 == expected_motor_values[5] 29 | assert result.motor7 == expected_motor_values[6] 30 | assert result.motor8 == expected_motor_values[7] 31 | 32 | @pytest.mark.parametrize("motor_data, expected_serializable", [ 33 | ( 34 | (1000, 1200, 1300, 1400, 1500, 1600, 1700, 1800), 35 | (1000, 1200, 1300, 1400, 1500, 1600, 1700, 1800) 36 | ), 37 | ( 38 | (2000, 2100, 2200, 2300, 2400, 2500, 2600, 2700), 39 | (2000, 2100, 2200, 2300, 2400, 2500, 2600, 2700) 40 | ), 41 | ( 42 | (3000, 3100, 3200, 3300, 3400, 3500, 3600, 3700), 43 | (3000, 3100, 3200, 3300, 3400, 3500, 3600, 3700) 44 | ) 45 | ]) 46 | def test_msp_motor_as_serializable(motor_data, expected_serializable): 47 | motor = MspMotor(*motor_data) 48 | 49 | serializable_data = motor.as_serializable() 50 | 51 | assert isinstance(serializable_data, tuple) 52 | assert serializable_data == expected_serializable 53 | 54 | @pytest.mark.parametrize("invalid_data", [ 55 | (1000, 1200, 1300, 1400, 1500), 56 | (1000, 1200), 57 | (1000, 1200, 1300, 1400), 58 | (1000,) 59 | ]) 60 | def test_msp_motor_parse_invalid_data(invalid_data): 61 | with pytest.raises(TypeError): 62 | MspMotor.parse(invalid_data) 63 | 64 | @pytest.mark.parametrize("data, expected_motor_pin_values", [ 65 | ( 66 | (1, 2, 3, 4, 5, 6, 7, 8), 67 | (1, 2, 3, 4, 5, 6, 7, 8) 68 | ), 69 | ( 70 | (9, 10, 11, 12, 13, 14, 15, 16), 71 | (9, 10, 11, 12, 13, 14, 15, 16) 72 | ), 73 | ( 74 | (17, 18, 19, 20, 21, 22, 23, 24), 75 | (17, 18, 19, 20, 21, 22, 23, 24) 76 | ) 77 | ]) 78 | def test_msp_motor_pins_parse(data, expected_motor_pin_values): 79 | result = MspMotorPins.parse(data) 80 | 81 | assert isinstance(result, MspMotorPins) 82 | assert result.motor1 == expected_motor_pin_values[0] 83 | assert result.motor2 == expected_motor_pin_values[1] 84 | assert result.motor3 == expected_motor_pin_values[2] 85 | assert result.motor4 == expected_motor_pin_values[3] 86 | assert result.motor5 == expected_motor_pin_values[4] 87 | assert result.motor6 == expected_motor_pin_values[5] 88 | assert result.motor7 == expected_motor_pin_values[6] 89 | assert result.motor8 == expected_motor_pin_values[7] 90 | 91 | @pytest.mark.parametrize("motor_pin_data, expected_serializable", [ 92 | ( 93 | (1, 2, 3, 4, 5, 6, 7, 8), 94 | (1, 2, 3, 4, 5, 6, 7, 8) 95 | ), 96 | ( 97 | (9, 10, 11, 12, 13, 14, 15, 16), 98 | (9, 10, 11, 12, 13, 14, 15, 16) 99 | ), 100 | ( 101 | (17, 18, 19, 20, 21, 22, 23, 24), 102 | (17, 18, 19, 20, 21, 22, 23, 24) 103 | ) 104 | ]) 105 | def test_msp_motor_pins_as_serializable(motor_pin_data, expected_serializable): 106 | motor_pins = MspMotorPins(*motor_pin_data) 107 | 108 | serializable_data = motor_pins.as_serializable() 109 | 110 | assert isinstance(serializable_data, tuple) 111 | assert serializable_data == expected_serializable 112 | 113 | @pytest.mark.parametrize("invalid_pin_data", [ 114 | (1, 2, 3, 4, 5), 115 | (1, 2), 116 | (1, 2, 3, 4), 117 | (1,) 118 | ]) 119 | def test_msp_motor_pins_parse_invalid_data(invalid_pin_data): 120 | with pytest.raises(TypeError): 121 | MspMotorPins.parse(invalid_pin_data) 122 | 123 | @pytest.mark.parametrize("motor_data, expected_serializable", [ 124 | ( 125 | (0, 0, 0, 0, 0, 0, 0, 0), 126 | (0, 0, 0, 0, 0, 0, 0, 0) 127 | ), 128 | ]) 129 | def test_msp_motor_as_serializable_empty(motor_data, expected_serializable): 130 | motor = MspMotor(*motor_data) 131 | 132 | serializable_data = motor.as_serializable() 133 | 134 | assert serializable_data == expected_serializable 135 | 136 | @pytest.mark.parametrize("motor_pin_data, expected_serializable", [ 137 | ( 138 | (0, 0, 0, 0, 0, 0, 0, 0), 139 | (0, 0, 0, 0, 0, 0, 0, 0) 140 | ), 141 | ]) 142 | def test_msp_motor_pins_as_serializable_empty(motor_pin_data, expected_serializable): 143 | motor_pins = MspMotorPins(*motor_pin_data) 144 | 145 | serializable_data = motor_pins.as_serializable() 146 | 147 | assert serializable_data == expected_serializable 148 | -------------------------------------------------------------------------------- /tests/unit/test_rc.py: -------------------------------------------------------------------------------- 1 | from multiwii.data import MspRc, MspRcTuning 2 | 3 | import pytest 4 | 5 | @pytest.mark.parametrize( 6 | "data, expected_values", 7 | [ 8 | ( 9 | (1000, 2000, 1500, 1200, 1800, 1600, 1400, 1300), 10 | (1000, 2000, 1500, 1200, 1800, 1600, 1400, 1300), 11 | ), 12 | ] 13 | ) 14 | def test_msp_rc_parse(data, expected_values): 15 | result = MspRc.parse(data) 16 | 17 | assert isinstance(result, MspRc) 18 | assert result.roll == expected_values[0] 19 | assert result.pitch == expected_values[1] 20 | assert result.yaw == expected_values[2] 21 | assert result.throttle == expected_values[3] 22 | assert result.aux1 == expected_values[4] 23 | assert result.aux2 == expected_values[5] 24 | assert result.aux3 == expected_values[6] 25 | assert result.aux4 == expected_values[7] 26 | 27 | @pytest.mark.parametrize( 28 | "data", 29 | [ 30 | (1000, 2000, 1500, 1200, 1800), 31 | ] 32 | ) 33 | def test_msp_rc_parse_invalid_data(data): 34 | with pytest.raises(TypeError): 35 | MspRc.parse(data) 36 | 37 | @pytest.mark.parametrize( 38 | "rc_values, expected_serializable", 39 | [ 40 | ( 41 | (1000, 2000, 1500, 1200, 1800, 1600, 1400, 1300), 42 | (1000, 2000, 1500, 1200, 1800, 1600, 1400, 1300), 43 | ), 44 | ] 45 | ) 46 | def test_msp_rc_as_serializable(rc_values, expected_serializable): 47 | rc = MspRc( 48 | roll=rc_values[0], 49 | pitch=rc_values[1], 50 | yaw=rc_values[2], 51 | throttle=rc_values[3], 52 | aux1=rc_values[4], 53 | aux2=rc_values[5], 54 | aux3=rc_values[6], 55 | aux4=rc_values[7] 56 | ) 57 | 58 | serializable_data = rc.as_serializable() 59 | 60 | assert isinstance(serializable_data, tuple) 61 | assert serializable_data == expected_serializable 62 | 63 | @pytest.mark.parametrize( 64 | "rc_values, expected_serializable", 65 | [ 66 | ( 67 | (0, 0, 0, 0, 0, 0, 0, 0), 68 | (0, 0, 0, 0, 0, 0, 0, 0), 69 | ), 70 | ] 71 | ) 72 | def test_msp_rc_as_serializable_empty(rc_values, expected_serializable): 73 | rc = MspRc( 74 | roll=rc_values[0], 75 | pitch=rc_values[1], 76 | yaw=rc_values[2], 77 | throttle=rc_values[3], 78 | aux1=rc_values[4], 79 | aux2=rc_values[5], 80 | aux3=rc_values[6], 81 | aux4=rc_values[7] 82 | ) 83 | 84 | serializable_data = rc.as_serializable() 85 | 86 | assert serializable_data == expected_serializable 87 | 88 | @pytest.mark.parametrize( 89 | "data, expected_values", 90 | [ 91 | ( 92 | (100, 200, 300, 400, 500, 600, 700), 93 | [ 94 | 100, 200, 300, 400, 500, 600, 700 95 | ] 96 | ), 97 | ] 98 | ) 99 | def test_msp_rc_tuning_parse(data, expected_values): 100 | result = MspRcTuning.parse(data) 101 | 102 | assert isinstance(result, MspRcTuning) 103 | assert result.rate == expected_values[0] 104 | assert result.expo == expected_values[1] 105 | assert result.roll_pitch_rate == expected_values[2] 106 | assert result.yaw_rate == expected_values[3] 107 | assert result.dynamic_throttle_pid == expected_values[4] 108 | assert result.throttle_mid == expected_values[5] 109 | assert result.throttle_expo == expected_values[6] 110 | 111 | @pytest.mark.parametrize( 112 | "data", 113 | [ 114 | (100, 200, 300, 400, 500), 115 | ] 116 | ) 117 | def test_msp_rc_tuning_parse_invalid_data(data): 118 | with pytest.raises(TypeError): 119 | MspRcTuning.parse(data) 120 | 121 | @pytest.mark.parametrize( 122 | "tuning_values, expected_serializable", 123 | [ 124 | ( 125 | (100, 200, 300, 400, 500, 600, 700), 126 | (100, 200, 300, 400, 500, 600, 700), 127 | ), 128 | ] 129 | ) 130 | def test_msp_rc_tuning_as_serializable(tuning_values, expected_serializable): 131 | rc_tuning = MspRcTuning( 132 | rate=tuning_values[0], 133 | expo=tuning_values[1], 134 | roll_pitch_rate=tuning_values[2], 135 | yaw_rate=tuning_values[3], 136 | dynamic_throttle_pid=tuning_values[4], 137 | throttle_mid=tuning_values[5], 138 | throttle_expo=tuning_values[6] 139 | ) 140 | 141 | serializable_data = rc_tuning.as_serializable() 142 | 143 | assert isinstance(serializable_data, tuple) 144 | assert serializable_data == expected_serializable 145 | 146 | @pytest.mark.parametrize( 147 | "tuning_values, expected_serializable", 148 | [ 149 | ( 150 | (0, 0, 0, 0, 0, 0, 0), 151 | (0, 0, 0, 0, 0, 0, 0), 152 | ), 153 | ] 154 | ) 155 | def test_msp_rc_tuning_as_serializable_empty(tuning_values, expected_serializable): 156 | rc_tuning = MspRcTuning( 157 | rate=tuning_values[0], 158 | expo=tuning_values[1], 159 | roll_pitch_rate=tuning_values[2], 160 | yaw_rate=tuning_values[3], 161 | dynamic_throttle_pid=tuning_values[4], 162 | throttle_mid=tuning_values[5], 163 | throttle_expo=tuning_values[6] 164 | ) 165 | 166 | serializable_data = rc_tuning.as_serializable() 167 | 168 | assert serializable_data == expected_serializable 169 | -------------------------------------------------------------------------------- /tests/unit/test_navigation.py: -------------------------------------------------------------------------------- 1 | from multiwii.data import ( 2 | Coordinates, 3 | MspCompGps, 4 | MspRawGps, 5 | MspWaypoint 6 | ) 7 | 8 | import pytest 9 | 10 | @pytest.mark.parametrize( 11 | "data, expected_values", 12 | [ 13 | ( 14 | (1000, 45, 1), 15 | (1000, 45, 1) 16 | ), 17 | ] 18 | ) 19 | def test_msp_comp_gps_parse(data, expected_values): 20 | result = MspCompGps.parse(data) 21 | 22 | assert isinstance(result, MspCompGps) 23 | assert result.distance_to_home == expected_values[0] 24 | assert result.direction_to_home == expected_values[1] 25 | assert result.update_status == expected_values[2] 26 | 27 | @pytest.mark.parametrize( 28 | "data", 29 | [ 30 | (1000, 45), 31 | ] 32 | ) 33 | def test_msp_comp_gps_parse_invalid_data(data): 34 | with pytest.raises(TypeError): 35 | MspCompGps.parse(data) 36 | 37 | @pytest.mark.parametrize( 38 | "data, expected_values", 39 | [ 40 | ( 41 | (1, 6, 123456789, 987654321, 100, 150, 270), 42 | (1, 6, 12.3456789, 98.7654321, 100, [5], 27.0) 43 | ), 44 | ] 45 | ) 46 | def test_msp_raw_gps_parse(data, expected_values): 47 | result = MspRawGps.parse(data) 48 | 49 | assert isinstance(result, MspRawGps) 50 | assert result.fix == expected_values[0] 51 | assert result.satellites == expected_values[1] 52 | assert result.coordinates.latitude == expected_values[2] 53 | assert result.coordinates.longitude == expected_values[3] 54 | assert result.altitude == expected_values[4] 55 | assert result.speed == expected_values[5] 56 | assert result.ground_course == expected_values[6] 57 | 58 | @pytest.mark.parametrize( 59 | "gps_values, expected_serializable", 60 | [ 61 | ( 62 | MspRawGps(1, 6, Coordinates(123456789, 987654321), 100, 150, 270), 63 | (1, 6, 123456789, 987654321, 100, 150, 2700) 64 | ), 65 | ] 66 | ) 67 | def test_msp_raw_gps_as_serializable(gps_values, expected_serializable): 68 | serializable_data = gps_values.as_serializable() 69 | 70 | assert isinstance(serializable_data, tuple) 71 | assert serializable_data == expected_serializable 72 | 73 | @pytest.mark.parametrize( 74 | "data", 75 | [ 76 | (1, 6, 123456789, 987654321, 100, 150), 77 | ] 78 | ) 79 | def test_msp_raw_gps_parse_invalid_data(data): 80 | with pytest.raises(TypeError): 81 | MspRawGps.parse(data) 82 | 83 | @pytest.mark.parametrize( 84 | "data, expected_values", 85 | [ 86 | ( 87 | (1, 123456789, 987654321, 100, 90, 60, 1), 88 | (1, 12.3456789, 98.7654321, 100, 90, 60, 1) 89 | ), 90 | ] 91 | ) 92 | def test_msp_waypoint_parse(data, expected_values): 93 | result = MspWaypoint.parse(data) 94 | 95 | assert isinstance(result, MspWaypoint) 96 | assert result.number == expected_values[0] 97 | assert result.coordinates.latitude == expected_values[1] 98 | assert result.coordinates.longitude == expected_values[2] 99 | assert result.altitude_hold == expected_values[3] 100 | assert result.heading == expected_values[4] 101 | assert result.time_to_stay == expected_values[5] 102 | assert result.status_flag == expected_values[6] 103 | 104 | @pytest.mark.parametrize( 105 | "waypoint_values, expected_serializable", 106 | [ 107 | ( 108 | MspWaypoint(1, Coordinates(123456789, 987654321), 100, 90, 60, 1), 109 | (1, 123456789, 987654321, 100, 90, 60, 1) 110 | ), 111 | ] 112 | ) 113 | def test_msp_waypoint_as_serializable(waypoint_values, expected_serializable): 114 | serializable_data = waypoint_values.as_serializable() 115 | 116 | assert isinstance(serializable_data, tuple) 117 | assert serializable_data == expected_serializable 118 | 119 | @pytest.mark.parametrize( 120 | "data", 121 | [ 122 | (1, 123456789, 987654321, 100, 90, 60), 123 | ] 124 | ) 125 | def test_msp_waypoint_parse_invalid_data(data): 126 | with pytest.raises(TypeError): 127 | MspWaypoint.parse(data) 128 | 129 | @pytest.mark.parametrize( 130 | "gps_values, expected_serializable", 131 | [ 132 | ( 133 | MspRawGps(0, 0, Coordinates(0, 0), 0, 0, 0), 134 | (0, 0, 0, 0, 0, 0, 0) 135 | ), 136 | ] 137 | ) 138 | def test_msp_raw_gps_as_serializable_empty(gps_values, expected_serializable): 139 | serializable_data = gps_values.as_serializable() 140 | 141 | assert serializable_data == expected_serializable 142 | 143 | @pytest.mark.parametrize( 144 | "comp_gps_values, expected_serializable", 145 | [ 146 | ( 147 | MspCompGps(0, 0, 0), 148 | (0, 0, 0) 149 | ), 150 | ] 151 | ) 152 | def test_msp_comp_gps_as_serializable_empty(comp_gps_values, expected_serializable): 153 | serializable_data = comp_gps_values.as_serializable() 154 | 155 | assert serializable_data == expected_serializable 156 | 157 | @pytest.mark.parametrize( 158 | "waypoint_values, expected_serializable", 159 | [ 160 | ( 161 | MspWaypoint(0, Coordinates(0, 0), 0, 0, 0, 0), 162 | (0, 0, 0, 0, 0, 0, 0) 163 | ), 164 | ] 165 | ) 166 | def test_msp_waypoint_as_serializable_empty(waypoint_values, expected_serializable): 167 | serializable_data = waypoint_values.as_serializable() 168 | 169 | assert serializable_data == expected_serializable 170 | -------------------------------------------------------------------------------- /src/multiwii/commands.py: -------------------------------------------------------------------------------- 1 | from ._command import _MspCommand 2 | 3 | from typing import Final 4 | 5 | MSP_IDENT: Final[_MspCommand] = _MspCommand(100, '3BI:4:!') 6 | """_MspCommand: An instance representing the MSP_IDENT (100) command.""" 7 | 8 | MSP_STATUS: Final[_MspCommand] = _MspCommand(101, '3HIB:5:!') 9 | """_MspCommand: An instance representing the MSP_STATUS (101) command.""" 10 | 11 | MSP_RAW_IMU: Final[_MspCommand] = _MspCommand(102, '8h:8:!') 12 | """_MspCommand: An instance representing the MSP_RAW_IMU (102) command.""" 13 | 14 | MSP_SERVO: Final[_MspCommand] = _MspCommand(103, '16H:16:!') 15 | """_MspCommand: An instance representing the MSP_SERVO (103) command.""" 16 | 17 | MSP_MOTOR: Final[_MspCommand] = _MspCommand(104, '16H:16:!') 18 | """_MspCommand: An instance representing the MSP_MOTOR (104) command.""" 19 | 20 | MSP_RC: Final[_MspCommand] = _MspCommand(105, '16H:16:!') 21 | """_MspCommand: An instance representing the MSP_RC (105) command.""" 22 | 23 | MSP_RAW_GPS: Final[_MspCommand] = _MspCommand(106, '2B2I3H:7:!') 24 | """_MspCommand: An instance representing the MSP_RAW_GPS (106) command.""" 25 | 26 | MSP_COMP_GPS: Final[_MspCommand] = _MspCommand(107, '2HB:3:!') 27 | """_MspCommand: An instance representing the MSP_COMP_GPS (107) command.""" 28 | 29 | MSP_ATTITUDE: Final[_MspCommand] = _MspCommand(108, '3h:3:!') 30 | """_MspCommand: An instance representing the MSP_ATTITUDE (108) command.""" 31 | 32 | MSP_ALTITUDE: Final[_MspCommand] = _MspCommand(109, 'ih:2:!') 33 | """_MspCommand: An instance representing the MSP_ALTITUDE (109) command.""" 34 | 35 | MSP_ANALOG: Final[_MspCommand] = _MspCommand(110, 'B3H:4:!') 36 | """_MspCommand: An instance representing the MSP_ANALOG (110) command.""" 37 | 38 | MSP_RC_TUNING: Final[_MspCommand] = _MspCommand(111, '7B:7:!') 39 | """_MspCommand: An instance representing the MSP_RC_TUNING (111) command.""" 40 | 41 | MSP_PID: Final[_MspCommand] = _MspCommand(112, '30B:30:!') 42 | """_MspCommand: An instance representing the MSP_PID (112) command.""" 43 | 44 | MSP_BOX: Final[_MspCommand] = _MspCommand(113, 'H:1:?') 45 | """_MspCommand: An instance representing the MSP_BOX (113) command.""" 46 | 47 | MSP_MISC: Final[_MspCommand] = _MspCommand(114, '6HIH4B:12:!') 48 | """_MspCommand: An instance representing the MSP_MISC (114) command.""" 49 | 50 | MSP_MOTOR_PINS: Final[_MspCommand] = _MspCommand(115, '8B:8:!') 51 | """_MspCommand: An instance representing the MSP_MOTOR_PINS (115) command.""" 52 | 53 | MSP_BOXNAMES: Final[_MspCommand] = _MspCommand(116, 's:1:?') 54 | """_MspCommand: An instance representing the MSP_BOXNAMES (116) command.""" 55 | 56 | MSP_PIDNAMES: Final[_MspCommand] = _MspCommand(117, 's:1:?') 57 | """_MspCommand: An instance representing the MSP_PIDNAMES (117) command.""" 58 | 59 | MSP_WP: Final[_MspCommand] = _MspCommand(118, 'B3I2HB:7:!') 60 | """_MspCommand: An instance representing the MSP_WP (118) command.""" 61 | 62 | MSP_BOXIDS: Final[_MspCommand] = _MspCommand(119, '3HB:4:?') 63 | """_MspCommand: An instance representing the MSP_BOXIDS (119) command.""" 64 | 65 | MSP_SERVO_CONF: Final[_MspCommand] = _MspCommand(120, '3HB'*8 + ':32:!') 66 | """_MspCommand: An instance representing the MSP_SERVO_CONF (120) command.""" 67 | 68 | MSP_SET_RAW_RC: Final[_MspCommand] = _MspCommand(200, '16H:16:!') 69 | """_MspCommand: An instance representing the MSP_SET_RAW_RC (200) command.""" 70 | 71 | MSP_SET_RAW_GPS: Final[_MspCommand] = _MspCommand(201, '2B2I2H:6:!') 72 | """_MspCommand: An instance representing the MSP_SET_RAW_GPS (201) command.""" 73 | 74 | MSP_SET_PID: Final[_MspCommand] = _MspCommand(202, '30B:30:!') 75 | """_MspCommand: An instance representing the MSP_SET_PID (202) command.""" 76 | 77 | MSP_SET_BOX: Final[_MspCommand] = _MspCommand(203, 'H:1:?') 78 | """_MspCommand: An instance representing the MSP_SET_BOX (203) command.""" 79 | 80 | MSP_SET_RC_TUNING: Final[_MspCommand] = _MspCommand(204, '7B:7:!') 81 | """_MspCommand: An instance representing the MSP_SET_RC_TUNING (204) command.""" 82 | 83 | MSP_ACC_CALIBRATION: Final[_MspCommand] = _MspCommand(205) 84 | """_MspCommand: An instance representing the MSP_ACC_CALIBRATION (205) command.""" 85 | 86 | MSP_MAG_CALIBRATION: Final[_MspCommand] = _MspCommand(206) 87 | """_MspCommand: An instance representing the MSP_MAG_CALIBRATION (206) command.""" 88 | 89 | MSP_SET_MISC: Final[_MspCommand] = _MspCommand(207, '6HIH4B:12:!') 90 | """_MspCommand: An instance representing the MSP_SET_MISC (207) command.""" 91 | 92 | MSP_RESET_CONF: Final[_MspCommand] = _MspCommand(208) 93 | """_MspCommand: An instance representing the MSP_RESET_CONF (208) command.""" 94 | 95 | MSP_SET_WP: Final[_MspCommand] = _MspCommand(209, 'B3I2HB:7:!') 96 | """_MspCommand: An instance representing the MSP_SET_WP (209) command.""" 97 | 98 | MSP_SELECT_SETTING: Final[_MspCommand] = _MspCommand(210, 'B:1:!') 99 | """_MspCommand: An instance representing the MSP_SELECT_SETTING (210) command.""" 100 | 101 | MSP_SET_HEAD: Final[_MspCommand] = _MspCommand(211, 'h:1:!') 102 | """_MspCommand: An instance representing the MSP_SET_HEAD (211) command.""" 103 | 104 | MSP_SET_SERVO_CONF: Final[_MspCommand] = _MspCommand(212, '3HB'*8 + ':32:!') 105 | """_MspCommand: An instance representing the MSP_SET_SERVO_CONF (212) command.""" 106 | 107 | MSP_SET_MOTOR: Final[_MspCommand] = _MspCommand(214, '16H:16:!') 108 | """_MspCommand: An instance representing the MSP_SET_MOTOR (214) command.""" 109 | 110 | MSP_BIND: Final[_MspCommand] = _MspCommand(240) 111 | """_MspCommand: An instance representing the MSP_BIND (240) command.""" 112 | 113 | MSP_EEPROM_WRITE: Final[_MspCommand] = _MspCommand(250) 114 | """_MspCommand: An instance representing the MSP_EEPROM_WRITE (250) command.""" -------------------------------------------------------------------------------- /src/multiwii/data/box.py: -------------------------------------------------------------------------------- 1 | from ..config import MultiWiiBox, MultiWiiBoxState 2 | from ..messaging import _decode_names 3 | 4 | from dataclasses import dataclass 5 | from typing import Self 6 | 7 | @dataclass 8 | class MspBoxItem: 9 | """ 10 | Represents data values for the MSP_SET_BOX command. 11 | 12 | This class encapsulates the configuration of auxiliary (aux) control boxes in a MultiWii 13 | flight controller. Each box can be assigned a specific function or mode, and its state can 14 | be set to the following values: 15 | 16 | * Empty (0b000) (Unselected) 17 | * Low (0b001) (LOW) 18 | * Mid (0b010) (MID) 19 | * High (0b100) (HIGH) 20 | """ 21 | aux1: MultiWiiBoxState 22 | """MultiWiiStateBox: The state value for the first auxiliary function.""" 23 | 24 | aux2: MultiWiiBoxState 25 | """MultiWiiStateBox: The state value for the second auxiliary function.""" 26 | 27 | aux3: MultiWiiBoxState 28 | """MultiWiiStateBox: The state value for the third auxiliary function.""" 29 | 30 | aux4: MultiWiiBoxState 31 | """MultiWiiStateBox: The state value for the fourth auxiliary function.""" 32 | 33 | @classmethod 34 | def parse(cls, value: int) -> Self: 35 | """ 36 | Parses all auxiliary state values from the given parameter and instantiates an instance 37 | with the parsed state values. 38 | 39 | Parameters 40 | ---------- 41 | value : int 42 | A single integer value consisting of all of the auxiliary states. 43 | 44 | Returns 45 | ------- 46 | MspBoxItem 47 | An instance of the `MspBoxItem` class with parsed box item state values. 48 | """ 49 | return cls( 50 | aux1=MultiWiiBoxState.parse(value), 51 | aux2=MultiWiiBoxState.parse(value >> 3), 52 | aux3=MultiWiiBoxState.parse(value >> 6), 53 | aux4=MultiWiiBoxState.parse(value >> 9) 54 | ) 55 | 56 | def compile(self) -> int: 57 | """ 58 | Compiles all of the box state values into a single unsigned integer value. 59 | 60 | Returns 61 | ------- 62 | int 63 | The compiled integer value representing the combined state of all auxiliary control 64 | boxes. 65 | """ 66 | return self.aux1 | self.aux2 << 3 | self.aux3 << 6 | self.aux4 << 9 67 | 68 | @dataclass 69 | class MspBox: 70 | """ 71 | Represents data values for the MSP_BOX command. 72 | 73 | This class is used to store the state values of various control boxes in a MultiWii flight 74 | controller. Control boxes can be used to enable or disable specific functions or modes 75 | during flight. 76 | """ 77 | items: tuple[MspBoxItem] 78 | """tuple[MspBoxItem]: A tuple with the box items.""" 79 | 80 | @classmethod 81 | def parse(cls, data: tuple) -> Self: 82 | """ 83 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance 84 | of the `MspBox` class. 85 | 86 | Parameters 87 | ---------- 88 | data : tuple 89 | A tuple containing unpacked data values. 90 | 91 | Returns 92 | ------- 93 | MspBox 94 | An instance of the `MspBox` class populated with the parsed data. 95 | """ 96 | return cls(tuple(MspBoxItem.parse(state) for state in data)) 97 | 98 | def as_serializable(self) -> tuple[int]: 99 | """ 100 | Returns a tuple with integer values to be used for serialization. 101 | 102 | Returns 103 | ------- 104 | tuple[int] 105 | A tuple with serializable integer values. 106 | """ 107 | return (box_item.compile() for box_item in self.values) 108 | 109 | @dataclass 110 | class MspBoxIds: 111 | """ 112 | Represents data values for the MSP_BOXIDS command. 113 | """ 114 | values: tuple[MultiWiiBox] 115 | """tuple[MultiWiiBox]: A tuple with `MultiWiiBox` values.""" 116 | 117 | @classmethod 118 | def parse(cls, data: tuple) -> Self: 119 | """ 120 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 121 | the `MspBoxIds` class. 122 | 123 | Parameters 124 | ---------- 125 | data : tuple 126 | A tuple containing unpacked data values. 127 | 128 | Returns 129 | ------- 130 | MspBoxIds 131 | An instance of the `MspBoxIds` class populated with the parsed data. 132 | """ 133 | return cls(tuple(MultiWiiBox(value) for value in data)) 134 | 135 | @dataclass 136 | class MspBoxNames: 137 | """ 138 | Represents data values for the MSP_BOXNAMES command. 139 | 140 | This class is used to store the names of various boxes that can be checked in a MultiWii 141 | flight controller. Each box corresponds to a specific function or mode that can be 142 | activated in the flight controller's configuration. 143 | """ 144 | names: tuple[str] 145 | """tuple[str]: The name of the boxes as strings.""" 146 | 147 | @classmethod 148 | def parse(cls, data: tuple) -> Self: 149 | """ 150 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 151 | the `MspBoxNames` class. 152 | 153 | Parameters 154 | ---------- 155 | data : tuple 156 | A tuple containing unpacked data values. 157 | 158 | Returns 159 | ------- 160 | MspBoxNames 161 | An instance of the `MspBoxNames` class populated with the parsed data. 162 | """ 163 | return cls(_decode_names(data)) -------------------------------------------------------------------------------- /tests/unit/test_info.py: -------------------------------------------------------------------------------- 1 | from multiwii.config import ( 2 | MultiWiiCapability, 3 | MultiWiiMultitype, 4 | MultiWiiSensor 5 | ) 6 | 7 | from multiwii.data import ( 8 | MspAnalog, 9 | MspIdent, 10 | MspMisc, 11 | MspSetMisc, 12 | MspStatus 13 | ) 14 | 15 | import pytest 16 | 17 | @pytest.mark.parametrize("data, expected_values", [ 18 | ( 19 | (120.0, 500, 80, 1500), 20 | (12.0, 500, 80, 1500) 21 | ), 22 | ( 23 | (100.0, 1000, 50, 2000), 24 | (10.0, 1000, 50, 2000) 25 | ), 26 | ( 27 | (50.0, 250, 90, 1000), 28 | (5.0, 250, 90, 1000) 29 | ) 30 | ]) 31 | def test_msp_analog_parse(data, expected_values): 32 | result = MspAnalog.parse(data) 33 | 34 | assert isinstance(result, MspAnalog) 35 | assert result.voltage == expected_values[0] 36 | assert result.power_meter_sum == expected_values[1] 37 | assert result.rssi == expected_values[2] 38 | assert result.amperage == expected_values[3] 39 | 40 | @pytest.mark.parametrize("data, expected_values", [ 41 | ( 42 | (1, 2, 0b101, 3), 43 | (1, MultiWiiMultitype.Gimbal, 3) 44 | ), 45 | ( 46 | (2, 1, 0b110, 4), 47 | (2, MultiWiiMultitype.Y6, 4) 48 | ), 49 | ( 50 | (3, 4, 0b111, 5), 51 | (3, MultiWiiMultitype.Hex6, 5) 52 | ) 53 | ]) 54 | def test_msp_ident_parse(data, expected_values): 55 | result = MspIdent.parse(data) 56 | 57 | assert isinstance(result, MspIdent) 58 | assert result.version == expected_values[0] 59 | assert result.multitype == expected_values[1] 60 | assert len(result.capabilities) == 3 61 | assert result.navigation_version == expected_values[2] 62 | 63 | @pytest.mark.parametrize("data, expected_values", [ 64 | ( 65 | (1, 2, 3, 4, 5, 6, 7, 80, 9, 10, 11, 12), 66 | (1, 2, 3, 4, 5, 6, 7, 8.0, 9, 1.0, 1.1, 1.2) 67 | ), 68 | ( 69 | (10, 20, 30, 40, 50, 60, 70, 160, 90, 100, 110, 120), 70 | (10, 20, 30, 40, 50, 60, 70, 8.0, 90, 1.0, 1.1, 1.2) 71 | ), 72 | ( 73 | (100, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100, 1200), 74 | (100, 200, 300, 400, 500, 600, 700, 8.0, 900, 1.0, 1.1, 1.2) 75 | ) 76 | ]) 77 | def test_msp_misc_parse(data, expected_values): 78 | result = MspMisc.parse(data) 79 | 80 | assert isinstance(result, MspMisc) 81 | assert result.power_trigger == expected_values[0] 82 | assert result.throttle_failsafe == expected_values[1] 83 | assert result.throttle_idle == expected_values[2] 84 | assert result.throttle_min == expected_values[3] 85 | assert result.throttle_max == expected_values[4] 86 | assert result.power_logger_arm == expected_values[5] 87 | assert result.power_logger_lifetime == expected_values[6] 88 | assert result.magnetometer_declination == expected_values[7] 89 | assert result.battery_scale == expected_values[8] 90 | assert result.battery_warning_1 == expected_values[9] 91 | assert result.battery_warning_2 == expected_values[10] 92 | assert result.battery_critical == expected_values[11] 93 | 94 | @pytest.mark.parametrize("misc_data, expected_serializable", [ 95 | ( 96 | (1, 2, 3, 4, 5, 6, 7, 80, 9, 100, 110, 120), 97 | (1, 2, 3, 4, 5, 6, 7, 80, 9, 100, 110, 120) 98 | ), 99 | ( 100 | (10, 20, 30, 40, 50, 60, 70, 160, 90, 100, 110, 120), 101 | (10, 20, 30, 40, 50, 60, 70, 160, 90, 100, 110, 120) 102 | ), 103 | ( 104 | (100, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100, 1200), 105 | (100, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100, 1200) 106 | ) 107 | ]) 108 | def test_msp_set_misc_as_serializable(misc_data, expected_serializable): 109 | misc = MspSetMisc(*misc_data) 110 | 111 | serializable_data = misc.as_serializable() 112 | 113 | assert isinstance(serializable_data, tuple) 114 | assert len(serializable_data) == 12 115 | assert serializable_data == expected_serializable 116 | 117 | @pytest.mark.parametrize("data, expected_values", [ 118 | ( 119 | (1000, 5, 0b1010, 3, 7), 120 | (1000, 5, 2, 3, 7) 121 | ), 122 | ( 123 | (2000, 10, 0b1100, 4, 8), 124 | (2000, 10, 2, 4, 8) 125 | ), 126 | ( 127 | (1500, 8, 0b1110, 5, 6), 128 | (1500, 8, 3, 5, 6) 129 | ) 130 | ]) 131 | def test_msp_status_parse(data, expected_values): 132 | result = MspStatus.parse(data) 133 | 134 | assert isinstance(result, MspStatus) 135 | assert result.cycle_time == expected_values[0] 136 | assert result.i2c_errors == expected_values[1] 137 | assert len(result.sensors) == expected_values[2] 138 | assert result.status_flag == expected_values[3] 139 | assert result.global_config == expected_values[4] 140 | 141 | @pytest.mark.parametrize("invalid_data", [ 142 | "invalid_data", 143 | ("invalid", "data", "tuple"), 144 | (1, 2, 3) 145 | ]) 146 | def test_msp_analog_parse_invalid_data(invalid_data): 147 | with pytest.raises(TypeError): 148 | MspAnalog.parse(invalid_data) 149 | 150 | @pytest.mark.parametrize("invalid_data", [ 151 | ("invalid", "data", "tuple"), 152 | ("wrong", "format"), 153 | (None,) 154 | ]) 155 | def test_msp_ident_parse_invalid_data(invalid_data): 156 | with pytest.raises(ValueError): 157 | MspIdent.parse(invalid_data) 158 | 159 | @pytest.mark.parametrize("invalid_data", [ 160 | (1, 2, 3), 161 | (1, 2), 162 | (None,) 163 | ]) 164 | def test_msp_misc_parse_invalid_data(invalid_data): 165 | with pytest.raises(ValueError): 166 | MspMisc.parse(invalid_data) 167 | 168 | @pytest.mark.parametrize("misc_data", [ 169 | (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) 170 | ]) 171 | def test_msp_set_misc_as_serializable_empty(misc_data): 172 | misc = MspSetMisc(*misc_data) 173 | 174 | serializable_data = misc.as_serializable() 175 | 176 | assert serializable_data == misc_data 177 | 178 | @pytest.mark.parametrize("invalid_data", [ 179 | ("invalid", "data", "tuple") 180 | ]) 181 | def test_msp_status_parse_invalid_data(invalid_data): 182 | with pytest.raises(ValueError): 183 | MspStatus.parse(invalid_data) 184 | -------------------------------------------------------------------------------- /src/multiwii/data/navigation.py: -------------------------------------------------------------------------------- 1 | from . import Coordinates 2 | 3 | from dataclasses import dataclass 4 | from typing import Self 5 | 6 | @dataclass 7 | class MspCompGps: 8 | """ 9 | Represents data values for the MSP_COMP_GPS command. 10 | 11 | This class encapsulates the GPS compass data from the MultiWii flight controller. 12 | It provides information about the distance and direction to the hime position, as 13 | well as the update status of the GPS data. 14 | """ 15 | distance_to_home: int 16 | """int: The distance to the home position in meters.""" 17 | 18 | direction_to_home: int 19 | """int: The direction to the home position in degrees.""" 20 | 21 | update_status: int 22 | """int: The update status of the GPS data.""" 23 | 24 | @classmethod 25 | def parse(cls, data: tuple) -> Self: 26 | """ 27 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 28 | the `MspCompGps` class. 29 | 30 | Parameters 31 | ---------- 32 | data : tuple 33 | A tuple containing unpacked data values. 34 | 35 | Returns 36 | ------- 37 | MspCompGps 38 | An instance of the `MspCompGps` class populated with the parsed data. 39 | """ 40 | return cls(*data) 41 | 42 | @dataclass 43 | class MspRawGps: 44 | """ 45 | Represents data values for the MSP_RAW_GPS command. 46 | 47 | This class encapsulates the GPS compass data from the MultiWii flight controller. 48 | It provides information about the GPS fix status, number of satellites in view, 49 | coordinates (latitude and longitude), altitude, speed, and ground course. 50 | """ 51 | 52 | fix: int 53 | """int: The GPS fix status.""" 54 | 55 | satellites: int 56 | """int: The number of satellites in view.""" 57 | 58 | coordinates: Coordinates[float] 59 | """Coordinates[float]: The GPS coordinates (latitude and longitude).""" 60 | 61 | altitude: int 62 | """int: The altitude in meters.""" 63 | 64 | speed: int 65 | """int: The speed in cm/s.""" 66 | 67 | ground_course: float 68 | """float: The ground course in degrees.""" 69 | 70 | @classmethod 71 | def parse(cls, data: tuple) -> Self: 72 | """ 73 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 74 | the `MspRawGps` class. 75 | 76 | Parameters 77 | ---------- 78 | data : tuple 79 | A tuple containing unpacked data values. 80 | 81 | Returns 82 | ------- 83 | MspRawGps 84 | An instance of the `MspRawGps` class populated with the parsed data. 85 | """ 86 | return cls( 87 | fix=data[0], 88 | satellites=data[1], 89 | coordinates=Coordinates( 90 | latitude=data[2] / 10000000.0, 91 | longitude=data[3] / 10000000.0 92 | ), 93 | altitude=data[4], 94 | speed=[5], 95 | ground_course=data[6] / 10.0 96 | ) 97 | 98 | def as_serializable(self) -> tuple[int]: 99 | """ 100 | Returns a tuple with integer values to be used for serialization. 101 | 102 | Returns 103 | ------- 104 | tuple[int] 105 | A tuple with serializable integer values. 106 | """ 107 | return ( 108 | fix, 109 | satellites, 110 | int(coordinates.latitude * 10000000), 111 | int(coordinates.longitude * 10000000), 112 | altitude, 113 | speed, 114 | int(ground_course * 10) 115 | ) 116 | 117 | @dataclass 118 | class MspWaypoint: 119 | """ 120 | Represents data values for the MSP_WP command. 121 | 122 | This class encapsulates the waypoint data from the MultiWii flight controller. 123 | It provides information about the waypoint number, coordinates, altitude hold, 124 | heading, time to stay at the waypoint, and the waypoint flag. 125 | """ 126 | number: int 127 | """int: The waypoint number.""" 128 | 129 | coordinates: Coordinates[int] 130 | """Coordinates[int]: The GPS coordinates (latitude and longitude) of the waypoint.""" 131 | 132 | altitude_hold: int 133 | """int: The altitude hold value in meters.""" 134 | 135 | heading: int 136 | """int: The heading in degrees.""" 137 | 138 | time_to_stay: int 139 | """int: The time to stay at the waypoint in seconds.""" 140 | 141 | status_flag: int 142 | """int: The waypoint flag indicating the waypoint's status or type.""" 143 | 144 | @classmethod 145 | def parse(cls, data: tuple) -> Self: 146 | """ 147 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 148 | the `MspWaypoint` class. 149 | 150 | Parameters 151 | ---------- 152 | data : tuple 153 | A tuple containing unpacked data values. 154 | 155 | Returns 156 | ------- 157 | MspWaypoint 158 | An instance of the `MspWaypoint` class populated with the parsed data. 159 | """ 160 | return cls( 161 | number=data[0], 162 | coordinates=Coordinates( 163 | latitude=data[1] / 10000000.0, 164 | longitude=data[2] / 10000000.0 165 | ), 166 | altitude_hold=data[3], 167 | heading=data[4], 168 | time_to_stay=data[5], 169 | status_flag=data[6] 170 | ) 171 | 172 | def as_serializable(self) -> tuple[int]: 173 | """ 174 | Returns a tuple with integer values to be used for serialization. 175 | 176 | Returns 177 | ------- 178 | tuple[int] 179 | A tuple with serializable integer values. 180 | """ 181 | return ( 182 | number, 183 | int(coordinates.latitude * 100000000), 184 | int(coordinates.longitude * 100000000), 185 | altitude_hold, 186 | heading, 187 | time_to_stay, 188 | status_flag 189 | ) -------------------------------------------------------------------------------- /tests/unit/test_pid.py: -------------------------------------------------------------------------------- 1 | from multiwii.data import MspPid, MspPidNames, Pid 2 | 3 | import pytest 4 | 5 | @pytest.mark.parametrize( 6 | "data, expected_values", 7 | [ 8 | ( 9 | ( 10 | 100, 200, 300, 11 | 400, 500, 600, 12 | 700, 800, 900, 13 | 1000, 1100, 1200, 14 | 1300, 1400, 1500, 15 | 1600, 1700, 1800, 16 | 1900, 2000, 2100, 17 | 2200, 2300, 2400, 18 | 2500, 2600, 2700, 19 | 2800, 2900, 3000 20 | ), 21 | ( 22 | 100, 200, 300, 23 | 400, 500, 600, 24 | 700, 800, 900, 25 | 1000, 1100, 1200, 26 | 1300, 1400, 1500, 27 | 1600, 1700, 1800, 28 | 1900, 2000, 2100, 29 | 2200, 2300, 2400, 30 | 2500, 2600, 2700, 31 | 2800, 2900, 3000 32 | ) 33 | ), 34 | ] 35 | ) 36 | def test_msp_pid_parse(data, expected_values): 37 | result = MspPid.parse(data) 38 | 39 | assert isinstance(result, MspPid) 40 | 41 | assert result.roll.p == expected_values[0] 42 | assert result.roll.i == expected_values[1] 43 | assert result.roll.d == expected_values[2] 44 | 45 | assert result.pitch.p == expected_values[3] 46 | assert result.pitch.i == expected_values[4] 47 | assert result.pitch.d == expected_values[5] 48 | 49 | assert result.yaw.p == expected_values[6] 50 | assert result.yaw.i == expected_values[7] 51 | assert result.yaw.d == expected_values[8] 52 | 53 | @pytest.mark.parametrize( 54 | "data", 55 | [ 56 | (100, 200, 300, 400, 500), 57 | ] 58 | ) 59 | def test_msp_pid_parse_invalid_data(data): 60 | with pytest.raises(TypeError): 61 | MspPid.parse(data) 62 | 63 | @pytest.mark.parametrize( 64 | "pid_values, expected_serializable", 65 | [ 66 | ( 67 | ( 68 | 100, 200, 300, 69 | 400, 500, 600, 70 | 700, 800, 900, 71 | 1000, 1100, 1200, 72 | 1300, 1400, 1500, 73 | 1600, 1700, 1800, 74 | 1900, 2000, 2100, 75 | 2200, 2300, 2400, 76 | 2500, 2600, 2700, 77 | 2800, 2900, 3000 78 | ), 79 | ( 80 | 100, 200, 300, 81 | 400, 500, 600, 82 | 700, 800, 900, 83 | 1000, 1100, 1200, 84 | 1300, 1400, 1500, 85 | 1600, 1700, 1800, 86 | 1900, 2000, 2100, 87 | 2200, 2300, 2400, 88 | 2500, 2600, 2700, 89 | 2800, 2900, 3000 90 | ) 91 | ), 92 | ] 93 | ) 94 | def test_msp_pid_as_serializable(pid_values, expected_serializable): 95 | pid = MspPid( 96 | roll=Pid(pid_values[0], pid_values[1], pid_values[2]), 97 | pitch=Pid(pid_values[3], pid_values[4], pid_values[5]), 98 | yaw=Pid(pid_values[6], pid_values[7], pid_values[8]), 99 | altitude_hold=Pid(pid_values[9], pid_values[10], pid_values[11]), 100 | position_hold=Pid(pid_values[12], pid_values[13], pid_values[14]), 101 | position_rate=Pid(pid_values[15], pid_values[16], pid_values[17]), 102 | navigation_rate=Pid(pid_values[18], pid_values[19], pid_values[20]), 103 | level_mode=Pid(pid_values[21], pid_values[22], pid_values[23]), 104 | magnetometer=Pid(pid_values[24], pid_values[25], pid_values[26]), 105 | velocity=Pid(pid_values[27], pid_values[28], pid_values[29]) 106 | ) 107 | 108 | serializable_data = pid.as_serializable() 109 | 110 | assert isinstance(serializable_data, tuple) 111 | assert serializable_data == expected_serializable 112 | 113 | @pytest.mark.parametrize( 114 | "pid_values, expected_serializable", 115 | [ 116 | ( 117 | ( 118 | 0, 0, 0, 119 | 0, 0, 0, 120 | 0, 0, 0, 121 | 0, 0, 0, 122 | 0, 0, 0, 123 | 0, 0, 0, 124 | 0, 0, 0, 125 | 0, 0, 0, 126 | 0, 0, 0, 127 | 0, 0, 0, 128 | 0, 0, 0 129 | ), 130 | (0,) * 30 131 | ), 132 | ] 133 | ) 134 | def test_msp_pid_as_serializable_empty(pid_values, expected_serializable): 135 | pid = MspPid( 136 | roll=Pid(pid_values[0], pid_values[1], pid_values[2]), 137 | pitch=Pid(pid_values[3], pid_values[4], pid_values[5]), 138 | yaw=Pid(pid_values[6], pid_values[7], pid_values[8]), 139 | altitude_hold=Pid(pid_values[9], pid_values[10], pid_values[11]), 140 | position_hold=Pid(pid_values[12], pid_values[13], pid_values[14]), 141 | position_rate=Pid(pid_values[15], pid_values[16], pid_values[17]), 142 | navigation_rate=Pid(pid_values[18], pid_values[19], pid_values[20]), 143 | level_mode=Pid(pid_values[21], pid_values[22], pid_values[23]), 144 | magnetometer=Pid(pid_values[24], pid_values[25], pid_values[26]), 145 | velocity=Pid(pid_values[27], pid_values[28], pid_values[29]) 146 | ) 147 | 148 | serializable_data = pid.as_serializable() 149 | 150 | assert serializable_data == expected_serializable 151 | 152 | @pytest.mark.parametrize( 153 | "data, expected_values", 154 | [ 155 | ( 156 | (b'Roll;Pitch;Yaw;AltitudeHold;PositionHold',), 157 | ("Roll", "Pitch", "Yaw", "AltitudeHold", "PositionHold") 158 | ), 159 | ] 160 | ) 161 | def test_msp_pidnames_parse(data, expected_values): 162 | result = MspPidNames.parse(data) 163 | 164 | assert isinstance(result, MspPidNames) 165 | assert result.names == expected_values 166 | 167 | @pytest.mark.parametrize( 168 | "data", 169 | [ 170 | (1, 2, 3), 171 | ] 172 | ) 173 | def test_msp_pidnames_parse_invalid_data(data): 174 | with pytest.raises(TypeError): 175 | MspPidNames.parse(data) 176 | 177 | @pytest.mark.parametrize( 178 | "data, expected_values", 179 | [ 180 | ( 181 | (), 182 | () 183 | ), 184 | ] 185 | ) 186 | def test_msp_pidnames_empty(data, expected_values): 187 | result = MspPidNames.parse(data) 188 | 189 | assert isinstance(result, MspPidNames) 190 | assert result.names == expected_values 191 | -------------------------------------------------------------------------------- /src/multiwii/_command.py: -------------------------------------------------------------------------------- 1 | from struct import calcsize 2 | from typing import Final, NoReturn 3 | 4 | class _MspCommand(object): 5 | """ 6 | Represents a MultiWii Serial Protocol command. 7 | 8 | This class encapsulates the details of a command for the MultiWii Serial Protocol (MSP). 9 | It includes information about the command code, whether the command size is variable, 10 | if it is a set-command, and details about the structure format used for serializing and 11 | deserializing corresponding data values. 12 | """ 13 | _code: Final[int] 14 | 15 | _data_field_count: Final[int] 16 | 17 | _data_size: Final[int] 18 | 19 | _has_variable_size: Final[bool] 20 | 21 | _is_set_command: Final[bool] 22 | 23 | _payload_struct_format: Final[str] 24 | 25 | def __init__(self, code: int, data_format: str = None) -> NoReturn: 26 | """ 27 | Initializes an instance using the provided code and struct format. 28 | 29 | Note 30 | ---- 31 | This constructor will use `struct.calcsize` to calculate the data size of the data 32 | structure format, and to validate the format string itself. Invalid format strings 33 | will cause `struct.calcsize` to raise an exception of type `struct.error`. 34 | 35 | Parameters 36 | ---------- 37 | code : int 38 | The unique code representing the specific MSP command. Must be between 100 and 240. 39 | data_format : str 40 | The `struct` format string used for packing and unpacking a corresponding payload. 41 | 42 | Raises 43 | ------ 44 | ValueError 45 | If the provided code is not between 100 or 250. 46 | """ 47 | if not 100 <= code <= 250: 48 | raise ValueError('Command code must be between 100 and 250.') 49 | 50 | data_struct_format = None 51 | 52 | data_field_count = 0 53 | 54 | has_variable_size = False 55 | 56 | if data_format: 57 | format_properties = data_format.split(':') 58 | 59 | data_struct_format = format_properties[0] 60 | 61 | data_field_count = int(format_properties[1]) 62 | 63 | has_variable_size = format_properties[2] == '?' 64 | 65 | self._code = code 66 | 67 | self._has_variable_size = has_variable_size 68 | 69 | self._is_set_command = code >= 200 70 | 71 | if not data_struct_format: 72 | self._data_field_count = 0 73 | 74 | self._data_size = 0 75 | 76 | self._payload_struct_format = None 77 | 78 | return 79 | 80 | self._data_size = calcsize(f'<{data_struct_format}') 81 | 82 | self._data_field_count = data_field_count 83 | 84 | self._payload_struct_format = f'<2B{data_struct_format}' 85 | 86 | def __int__(self) -> int: 87 | """ 88 | Returns the integer representation of the object, as the MSP command code. 89 | 90 | Returns 91 | ------- 92 | int 93 | The MSP command code. 94 | """ 95 | self._code 96 | 97 | def __repr__(self) -> str: 98 | """ 99 | Returns a string representation of the object. 100 | 101 | This method perform formats a string using the following values: 102 | 103 | * Class name 104 | * Command code 105 | * Data structure format 106 | * Data size 107 | 108 | Returns 109 | ------- 110 | str 111 | A detailed string representation of the object. 112 | """ 113 | data_struct_format = self.data_struct_format 114 | 115 | if self._has_variable_size: 116 | data_struct_format = f'*{data_struct_format}' 117 | 118 | return '{}<{}, "{}", {}, {}, "{}">'.format( 119 | self.__class__.__name__, 120 | self._code, 121 | data_struct_format, 122 | self._data_size, 123 | self._data_field_count, 124 | '?' if self._has_variable_size else '!' 125 | ) 126 | 127 | def __str__(self) -> str: 128 | """ 129 | Returns the string representation of the object, as the data structure format. 130 | 131 | Returns 132 | ------- 133 | str 134 | The data structure format string. 135 | """ 136 | return self.data_struct_format 137 | 138 | @property 139 | def code(self) -> int: 140 | """ 141 | Gets the unique command code. 142 | 143 | Returns 144 | ------- 145 | int 146 | The unique MSP command code. 147 | """ 148 | return self._code 149 | 150 | @property 151 | def data_field_count(self) -> int: 152 | """ 153 | Gets the data field count. 154 | 155 | Returns 156 | ------- 157 | int 158 | The field count for the provided data structure format. 159 | """ 160 | return self._data_field_count 161 | 162 | @property 163 | def data_size(self) -> int: 164 | """ 165 | Gets the data structure size. 166 | 167 | Returns 168 | ------- 169 | int 170 | The data structure size. 171 | """ 172 | return self._data_size 173 | 174 | @property 175 | def data_struct_format(self) -> str: 176 | """ 177 | Gets the data structure format string. 178 | 179 | Returns 180 | ------- 181 | str 182 | The data structure format that was provided initially. 183 | """ 184 | format = self._payload_struct_format 185 | 186 | return format[3:] if format else None 187 | 188 | @property 189 | def has_variable_size(self) -> bool: 190 | """ 191 | Gets a value indicative whether the data size is variable. 192 | 193 | Returns 194 | ------- 195 | bool 196 | True if data size is variable, False otherwise. 197 | """ 198 | return self._has_variable_size 199 | 200 | @property 201 | def is_set_command(self) -> bool: 202 | """ 203 | Gets a value indicative whether the command is a set-command. 204 | 205 | Returns 206 | ------- 207 | bool 208 | True if the command is a set-command, False otherwise. 209 | """ 210 | return self._is_set_command 211 | 212 | @property 213 | def payload_struct_format(self) -> str: 214 | """ 215 | Gets the payload format string. 216 | 217 | Returns 218 | ------- 219 | str 220 | The payload struct format string. 221 | """ 222 | return self._payload_struct_format -------------------------------------------------------------------------------- /tests/unit/test_command.py: -------------------------------------------------------------------------------- 1 | from multiwii import _MspCommand 2 | from struct import error as StructError 3 | 4 | import pytest 5 | 6 | @pytest.mark.parametrize( 7 | "code, data_format, expected_code, expected_data_field_count, expected_data_size, expected_has_variable_size", 8 | [ 9 | (150, 'B:1:?', 150, 1, 1, True) 10 | ] 11 | ) 12 | def test_valid_code_and_format( 13 | code, 14 | data_format, 15 | expected_code, 16 | expected_data_field_count, 17 | expected_data_size, 18 | expected_has_variable_size 19 | ): 20 | command = _MspCommand(code, data_format) 21 | 22 | assert command.code == expected_code 23 | assert command.data_field_count == expected_data_field_count 24 | assert command.data_size == expected_data_size 25 | assert command.has_variable_size == expected_has_variable_size 26 | 27 | @pytest.mark.parametrize("invalid_code", [99, 251]) 28 | def test_invalid_code(invalid_code): 29 | with pytest.raises(ValueError): 30 | _MspCommand(invalid_code, 'B:1:?') 31 | 32 | def test_invalid_format_string(): 33 | with pytest.raises(ValueError): 34 | _MspCommand(150, 'B:1') 35 | 36 | with pytest.raises(StructError): 37 | _MspCommand(150, 'X:1:?') 38 | 39 | @pytest.mark.parametrize("code", [100, 250]) 40 | def test_edge_case_code(code): 41 | command = _MspCommand(code, 'B:1:?') 42 | 43 | assert command.code == code 44 | 45 | @pytest.mark.parametrize("data_format, expected_data_field_count, expected_data_size", [ 46 | (None, 0, 0) 47 | ]) 48 | def test_empty_data_format(data_format, expected_data_field_count, expected_data_size): 49 | command = _MspCommand(150, data_format) 50 | 51 | assert command.data_field_count == expected_data_field_count 52 | assert command.data_size == expected_data_size 53 | 54 | @pytest.mark.parametrize("code, expected_code", [(150, 150)]) 55 | def test_code_property(code, expected_code): 56 | command = _MspCommand(code, 'B:1:?') 57 | 58 | assert command.code == expected_code 59 | 60 | @pytest.mark.parametrize("data_format, expected_data_field_count", [ 61 | ('B:2:?', 2) 62 | ]) 63 | def test_data_field_count_property(data_format, expected_data_field_count): 64 | command = _MspCommand(150, data_format) 65 | 66 | assert command.data_field_count == expected_data_field_count 67 | 68 | @pytest.mark.parametrize("data_format, expected_data_size", [ 69 | ('B:1:?', 1) 70 | ]) 71 | def test_data_size_property(data_format, expected_data_size): 72 | command = _MspCommand(150, data_format) 73 | 74 | assert command.data_size == expected_data_size 75 | 76 | @pytest.mark.parametrize("data_format, expected_data_struct_format", [ 77 | ('B:1:?', 'B') 78 | ]) 79 | def test_data_struct_format_property(data_format, expected_data_struct_format): 80 | command = _MspCommand(150, data_format) 81 | 82 | assert command.data_struct_format == expected_data_struct_format 83 | 84 | @pytest.mark.parametrize("data_format, expected_has_variable_size", [ 85 | ('B:1:', False), 86 | ('B:1:?', True) 87 | ]) 88 | def test_has_variable_size_property(data_format, expected_has_variable_size): 89 | command = _MspCommand(150, data_format) 90 | 91 | assert command.has_variable_size == expected_has_variable_size 92 | 93 | @pytest.mark.parametrize("code, data_format, expected_is_set_command", [ 94 | (200, 'B:1:?', True), 95 | (150, 'B:1:?', False) 96 | ]) 97 | def test_is_set_command_property(code, data_format, expected_is_set_command): 98 | command = _MspCommand(code, data_format) 99 | 100 | assert command.is_set_command == expected_is_set_command 101 | 102 | @pytest.mark.parametrize("data_format, expected_payload_struct_format", [ 103 | ('B:1:?', '<2BB') 104 | ]) 105 | def test_payload_struct_format_property(data_format, expected_payload_struct_format): 106 | command = _MspCommand(150, data_format) 107 | 108 | assert command.payload_struct_format == expected_payload_struct_format 109 | 110 | @pytest.mark.parametrize("code, data_format, expected_int", [ 111 | (150, 'B:1:?', 150) 112 | ]) 113 | def test_int_method(code, data_format, expected_int): 114 | command = _MspCommand(code, data_format) 115 | 116 | assert int(command) == expected_int 117 | 118 | @pytest.mark.parametrize("code, data_format", [(150, 'B:1:?')]) 119 | def test_repr_method(code, data_format): 120 | command = _MspCommand(code, data_format) 121 | 122 | repr_str = repr(command) 123 | 124 | assert '<150' in repr_str 125 | assert '1' in repr_str 126 | assert '!' in repr_str 127 | 128 | @pytest.mark.parametrize("code, data_format", [ 129 | (150, 'B:1:?') 130 | ]) 131 | def test_str_method(code, data_format): 132 | command = _MspCommand(code, data_format) 133 | 134 | assert str(command) == 'B' 135 | 136 | @pytest.mark.parametrize("code", [100, 250]) 137 | def test_code_boundaries(code): 138 | command = _MspCommand(code, 'B:1:?') 139 | 140 | assert command.code == code 141 | 142 | @pytest.mark.parametrize("invalid_code", [None, "string"]) 143 | def test_invalid_code_type(invalid_code): 144 | with pytest.raises(ValueError): 145 | _MspCommand(invalid_code, 'B:1:?') 146 | 147 | @pytest.mark.parametrize("invalid_format", [None, 123]) 148 | def test_invalid_data_format_type(invalid_format): 149 | with pytest.raises(ValueError): 150 | _MspCommand(150, invalid_format) 151 | 152 | @pytest.mark.parametrize("data_format, expected_data_size", [ 153 | ('B:1:?', 1), 154 | ('H:2:?', 4) 155 | ]) 156 | def test_non_standard_formats(data_format, expected_data_size): 157 | command = _MspCommand(150, data_format) 158 | 159 | assert command.data_size == expected_data_size 160 | 161 | @pytest.mark.parametrize("data_format", [ 162 | 'ZZ:1:?' 163 | ]) 164 | def test_invalid_struct_format(data_format): 165 | with pytest.raises(StructError): 166 | _MspCommand(150, data_format) 167 | 168 | @pytest.mark.parametrize("data_format", [ 169 | ('B:1:?') 170 | ]) 171 | def test_repr_with_variable_size(data_format): 172 | command = _MspCommand(150, data_format) 173 | 174 | assert "?" in repr(command) 175 | 176 | @pytest.mark.parametrize("code, data_format, expected_is_set_command", [ 177 | (200, 'B:1:?', True), 178 | (100, 'H:1:?', False) 179 | ]) 180 | def test_command_initialization(code, data_format, expected_is_set_command): 181 | command = _MspCommand(code, data_format) 182 | 183 | assert command.is_set_command == expected_is_set_command 184 | 185 | @pytest.mark.parametrize("data_format", [ 186 | None 187 | ]) 188 | def test_constructor_no_format(data_format): 189 | command = _MspCommand(150, data_format) 190 | 191 | assert command.data_field_count == 0 192 | assert command.data_size == 0 193 | -------------------------------------------------------------------------------- /tests/unit/test_multiwii.py: -------------------------------------------------------------------------------- 1 | from multiwii import MultiWii 2 | 3 | from multiwii.messaging import _MspResponseMessage, MspMessageError 4 | 5 | from multiwii.commands import ( 6 | MSP_ACC_CALIBRATION, 7 | MSP_ALTITUDE, 8 | MSP_BIND, 9 | MSP_EEPROM_WRITE, 10 | MSP_SELECT_SETTING, 11 | MSP_SET_BOX, 12 | MSP_SET_HEAD, 13 | MSP_SET_PID 14 | ) 15 | 16 | from multiwii.data import MspAltitude, MspRc 17 | 18 | from serial import Serial 19 | from time import sleep 20 | from unittest.mock import MagicMock 21 | 22 | import pytest 23 | 24 | @pytest.fixture 25 | def mock_serial(): 26 | return MagicMock(spec=Serial) 27 | 28 | @pytest.fixture 29 | def multiwii(mock_serial): 30 | return MultiWii(mock_serial) 31 | 32 | @pytest.mark.parametrize("serial_port", ['invalid_serial_port']) 33 | def test_init_with_invalid_serial_port(serial_port): 34 | with pytest.raises(TypeError): 35 | MultiWii(serial_port) 36 | 37 | @pytest.mark.parametrize("mock_serial", [MagicMock(spec=Serial)]) 38 | def test_init_with_valid_serial_port(mock_serial): 39 | multiwii = MultiWii(mock_serial) 40 | 41 | assert multiwii.serial_port == mock_serial 42 | assert multiwii.message_write_read_delay == MultiWii.DEFAULT_MESSAGE_WRITE_READ_DELAY 43 | 44 | def test_command_to_data_structure_type_map(multiwii): 45 | map_ = multiwii.command_to_data_structure_type_map 46 | 47 | assert isinstance(map_, dict) 48 | assert MSP_ALTITUDE in map_ 49 | assert map_[MSP_ALTITUDE] == MspAltitude 50 | 51 | @pytest.mark.parametrize("valid_delay", [0.01]) 52 | def test_set_message_write_read_delay_valid(multiwii, valid_delay): 53 | multiwii.message_write_read_delay = valid_delay 54 | 55 | assert multiwii.message_write_read_delay == valid_delay 56 | 57 | @pytest.mark.parametrize("invalid_delay", ['string']) 58 | def test_set_message_write_read_delay_invalid_type(multiwii, invalid_delay): 59 | with pytest.raises(TypeError): 60 | multiwii.message_write_read_delay = invalid_delay 61 | 62 | @pytest.mark.parametrize("negative_delay", [-0.01]) 63 | def test_set_message_write_read_delay_negative_value(multiwii, negative_delay): 64 | with pytest.raises(ValueError): 65 | multiwii.message_write_read_delay = negative_delay 66 | 67 | def test_read_response_message_invalid_header(multiwii, mock_serial): 68 | mock_serial.read.return_value = b'123' 69 | 70 | with pytest.raises(MspMessageError): 71 | multiwii._read_response_message(MSP_ALTITUDE) 72 | 73 | def test_read_response_message_invalid_command_code(multiwii, mock_serial): 74 | mock_serial.read.return_value = b'\x01\x01\x01' 75 | 76 | with pytest.raises(MspMessageError): 77 | multiwii._read_response_message(MSP_ALTITUDE) 78 | 79 | def test_read_response_message_invalid_checksum(multiwii, mock_serial): 80 | mock_serial.read.return_value = b'\x01\x01\x01\x02' 81 | 82 | with pytest.raises(MspMessageError): 83 | multiwii._read_response_message(MSP_ALTITUDE) 84 | 85 | def test_get_data_valid(multiwii, mock_serial): 86 | response_message = _MspResponseMessage( 87 | MSP_ALTITUDE, 88 | b'\x01\x01\x01\x03\x00\x05', 89 | 6 90 | ) 91 | 92 | multiwii._read_response_message = MagicMock(return_value=response_message) 93 | 94 | data = multiwii.get_data(MSP_ALTITUDE) 95 | 96 | assert data == response_message.data 97 | 98 | @pytest.mark.parametrize("error_message", ['Invalid command']) 99 | def test_get_data_invalid_command(multiwii, mock_serial, error_message): 100 | multiwii._read_response_message = MagicMock( 101 | side_effect=MspMessageError(error_message) 102 | ) 103 | 104 | with pytest.raises(MspMessageError): 105 | multiwii.get_data(MSP_ALTITUDE) 106 | 107 | def test_arm_function(multiwii, mock_serial): 108 | multiwii.set_raw_rc = MagicMock() 109 | 110 | multiwii.arm() 111 | 112 | assert multiwii.set_raw_rc.call_count > 0 113 | 114 | def test_disarm_function(multiwii, mock_serial): 115 | multiwii.set_raw_rc = MagicMock() 116 | 117 | multiwii.disarm() 118 | 119 | assert multiwii.set_raw_rc.call_count > 0 120 | 121 | def test_bind_transmitter_and_receiver(multiwii, mock_serial): 122 | multiwii._send_request_message = MagicMock() 123 | 124 | multiwii.bind_transmitter_and_receiver() 125 | 126 | multiwii._send_request_message.assert_called_once_with(MSP_BIND) 127 | 128 | def test_calibrate_accelerometer(multiwii, mock_serial): 129 | multiwii._send_request_message = MagicMock() 130 | 131 | multiwii.calibrate_accelerometer() 132 | 133 | multiwii._send_request_message.assert_called_once_with(MSP_ACC_CALIBRATION) 134 | 135 | def test_calibrate_magnetometer(multiwii, mock_serial): 136 | multiwii._send_request_message = MagicMock() 137 | 138 | multiwii.calibrate_magnetometer() 139 | 140 | multiwii._send_request_message.assert_called_once_with(MSP_ACC_CALIBRATION) 141 | 142 | def test_save_config_to_eeprom(multiwii, mock_serial): 143 | multiwii._send_request_message = MagicMock() 144 | 145 | multiwii.save_config_to_eeprom() 146 | 147 | multiwii._send_request_message.assert_called_once_with(MSP_EEPROM_WRITE) 148 | 149 | @pytest.mark.parametrize("setting_value", [1]) 150 | def test_select_setting_valid(multiwii, mock_serial, setting_value): 151 | multiwii._send_request_message = MagicMock() 152 | 153 | multiwii.select_setting(setting_value) 154 | 155 | multiwii._send_request_message.assert_called_once_with( 156 | MSP_SELECT_SETTING, 157 | data=(setting_value,) 158 | ) 159 | 160 | @pytest.mark.parametrize("invalid_setting", [5]) 161 | def test_select_setting_invalid_value(multiwii, invalid_setting): 162 | with pytest.raises(ValueError): 163 | multiwii.select_setting(invalid_setting) 164 | 165 | def test_set_boxes(multiwii, mock_serial): 166 | data = MagicMock() 167 | 168 | data.as_serializable = MagicMock(return_value=('serializable_data',)) 169 | 170 | multiwii._send_request_message = MagicMock() 171 | 172 | multiwii.set_boxes(data) 173 | 174 | multiwii._send_request_message.assert_called_once_with( 175 | MSP_SET_BOX, 176 | ('serializable_data',) 177 | ) 178 | 179 | @pytest.mark.parametrize("heading_value", [90]) 180 | def test_set_head_valid(multiwii, mock_serial, heading_value): 181 | multiwii._send_request_message = MagicMock() 182 | 183 | multiwii.set_head(heading_value) 184 | 185 | multiwii._send_request_message.assert_called_once_with( 186 | MSP_SET_HEAD, 187 | data=(heading_value,) 188 | ) 189 | 190 | @pytest.mark.parametrize("invalid_heading", [200]) 191 | def test_set_head_invalid_value(multiwii, invalid_heading): 192 | with pytest.raises(ValueError): 193 | multiwii.set_head(invalid_heading) 194 | 195 | def test_set_pid_values(multiwii, mock_serial): 196 | data = MagicMock() 197 | 198 | data.as_serializable = MagicMock(return_value=('serializable_data',)) 199 | 200 | multiwii._send_request_message = MagicMock() 201 | 202 | multiwii.set_pid_values(data) 203 | 204 | multiwii._send_request_message.assert_called_once_with( 205 | MSP_SET_PID, 206 | ('serializable_data',) 207 | ) 208 | -------------------------------------------------------------------------------- /tests/unit/test_messaging.py: -------------------------------------------------------------------------------- 1 | """ 2 | Test suite generated using ChatGPT-4o mini. 3 | 4 | This suite contains various tests for the functions and classes in the 5 | `messaging.py` module. It covers the checksum calculation, message 6 | serialization, response message parsing, and error handling, ensuring 7 | correct functionality for each component. 8 | """ 9 | 10 | from multiwii.messaging import ( 11 | _MspResponseMessage, 12 | _crc8_xor, 13 | _create_request_message, 14 | _decode_names, 15 | _parse_response_message, 16 | MESSAGE_ERROR_HEADER, 17 | MESSAGE_INCOMING_HEADER, 18 | MESSAGE_OUTGOING_HEADER, 19 | MspMessageError 20 | ) 21 | 22 | from multiwii import _MspCommand 23 | from struct import error as StructError 24 | 25 | import pytest 26 | 27 | @pytest.mark.parametrize("payload,expected_checksum", [ 28 | (b'\x01\x02\x03\x04', 0x00), 29 | (b'\xFF\xFF\xFF\xFF', 0x00), 30 | (b'\x00\x00\x00\x00', 0x00), 31 | ]) 32 | def test_crc8_xor(payload, expected_checksum): 33 | """ 34 | Test the `_crc8_xor` function. 35 | 36 | This test verifies that the `_crc8_xor` function correctly calculates 37 | the checksum for the provided payload using XOR CRC. 38 | """ 39 | assert _crc8_xor(payload) == expected_checksum 40 | 41 | @pytest.mark.parametrize("command,data", [ 42 | (_MspCommand(150, 'B:1:?'), (42,)), 43 | (_MspCommand(100, 'B:2:?'), (10, 20)), 44 | ]) 45 | def test_create_request_message_with_variable_size(command, data): 46 | """ 47 | Test `_create_request_message` with variable size data. 48 | 49 | This test checks the creation of a request message where the command has 50 | variable size data. It ensures that the message is serialized correctly. 51 | """ 52 | message = _create_request_message(command, data) 53 | 54 | assert message.startswith(MESSAGE_OUTGOING_HEADER) 55 | assert len(message) > len(MESSAGE_OUTGOING_HEADER) 56 | 57 | @pytest.mark.parametrize("command,data", [ 58 | (_MspCommand(150, 'B:1:?'), (42,)), 59 | (_MspCommand(100, 'B:2:?'), (10, 20)), 60 | ]) 61 | def test_create_request_message_with_fixed_size(command, data): 62 | """ 63 | Test `_create_request_message` with fixed size data. 64 | 65 | This test checks the creation of a request message where the command has 66 | fixed size data. It ensures that the message is serialized correctly. 67 | """ 68 | message = _create_request_message(command, data) 69 | 70 | assert message.startswith(MESSAGE_OUTGOING_HEADER) 71 | assert len(message) > len(MESSAGE_OUTGOING_HEADER) 72 | 73 | @pytest.mark.parametrize("command,data", [ 74 | (_MspCommand(150), ()), 75 | (_MspCommand(100), ()), 76 | ]) 77 | def test_create_request_message_with_no_data(command, data): 78 | """ 79 | Test `_create_request_message` with no data. 80 | 81 | This test verifies that the request message is created correctly when 82 | no data is provided. 83 | """ 84 | message = _create_request_message(command, data) 85 | 86 | assert message.startswith(MESSAGE_OUTGOING_HEADER) 87 | assert len(message) == len(MESSAGE_OUTGOING_HEADER) + 3 88 | 89 | @pytest.mark.parametrize("data,expected_names", [ 90 | (b'John;Doe', ('John', 'Doe')), 91 | (b'Jane;Doe', ('Jane', 'Doe')), 92 | ]) 93 | def test_decode_names(data, expected_names): 94 | """ 95 | Test the `_decode_names` function. 96 | 97 | This test checks that the `_decode_names` function correctly decodes 98 | a tuple of names from a serialized payload. 99 | """ 100 | assert _decode_names(data) == expected_names 101 | 102 | @pytest.mark.parametrize("command,payload,expected_command,expected_data,expected_data_size", [ 103 | (_MspCommand(150, 'B:1:?'), b'$M>1\x96\x00', 150, (150,), 1), 104 | (_MspCommand(100, 'B:2:?'), b'$M>2\x96\x00', 100, (100,), 2), 105 | ]) 106 | def test_parse_response_message_success(command, payload, expected_command, expected_data, expected_data_size): 107 | """ 108 | Test `_parse_response_message` with valid response. 109 | 110 | This test ensures that the `_parse_response_message` function correctly 111 | parses a valid response message and returns the expected result. 112 | """ 113 | response = _parse_response_message(command, payload) 114 | 115 | assert response.command.code == expected_command 116 | assert response.data == expected_data 117 | assert response.data_size == expected_data_size 118 | 119 | @pytest.mark.parametrize("command,payload", [ 120 | (_MspCommand(150, 'B:1:?'), b'$M>2\x96\x00'), 121 | (_MspCommand(100, 'B:2:?'), b'$M>1\x96\x00'), 122 | ]) 123 | def test_parse_response_message_invalid_code(command, payload): 124 | """ 125 | Test `_parse_response_message` with invalid response code. 126 | 127 | This test ensures that a `ValueError` is raised when the command code 128 | in the payload does not match the expected command code. 129 | """ 130 | with pytest.raises(ValueError): 131 | _parse_response_message(command, payload) 132 | 133 | @pytest.mark.parametrize("command,payload", [ 134 | (_MspCommand(150, 'B:1:?'), b'$M>1xyz'), 135 | (_MspCommand(100, 'B:2:?'), b'$M>2xyz'), 136 | ]) 137 | def test_parse_response_message_invalid_format(command, payload): 138 | """ 139 | Test `_parse_response_message` with an invalid format. 140 | 141 | This test ensures that a `StructError` is raised when the payload data 142 | does not match the expected format. 143 | """ 144 | with pytest.raises(StructError): 145 | _parse_response_message(command, payload) 146 | 147 | def test_msp_message_error(): 148 | """ 149 | Test the `MspMessageError` exception. 150 | 151 | This test verifies that the `MspMessageError` exception can be raised 152 | and caught correctly. 153 | """ 154 | with pytest.raises(MspMessageError): 155 | raise MspMessageError("Test error") 156 | 157 | @pytest.mark.parametrize("header,expected_header", [ 158 | (MESSAGE_ERROR_HEADER, b'$M!'), 159 | (MESSAGE_INCOMING_HEADER, b'$M<'), 160 | (MESSAGE_OUTGOING_HEADER, b'$M>'), 161 | ]) 162 | def test_message_headers(header, expected_header): 163 | """ 164 | Test that the defined message headers are correct. 165 | 166 | This test checks that the error, incoming, and outgoing message headers 167 | are correctly defined and match the expected values. 168 | """ 169 | assert header == expected_header 170 | 171 | @pytest.mark.parametrize("data, expected_size", [ 172 | ((42,), 1), 173 | ((1, 2, 3), 3), 174 | ((4, 5, 6, 7), 4), 175 | ]) 176 | def test_create_request_message_size(data, expected_size): 177 | """ 178 | Test the size of the serialized message based on data. 179 | 180 | This test checks the correct size of the message payload when different 181 | amounts of data are provided. 182 | """ 183 | command = _MspCommand(150, 'B:1:?') 184 | 185 | message = _create_request_message(command, data) 186 | 187 | assert len(message) == len(MESSAGE_OUTGOING_HEADER) + expected_size + 3 188 | 189 | @pytest.mark.parametrize("data, expected_checksum", [ 190 | (b'\x01\x02\x03\x04', 0x00), 191 | (b'\xFF\xFF\xFF\xFF', 0x00), 192 | (b'\x00\x00\x00\x00', 0x00), 193 | ]) 194 | def test_crc8_xor_checksum(data, expected_checksum): 195 | """ 196 | Test the `_crc8_xor` function with different payloads. 197 | 198 | This test checks the checksum calculation using different data payloads. 199 | """ 200 | assert _crc8_xor(data) == expected_checksum 201 | -------------------------------------------------------------------------------- /src/multiwii/data/info.py: -------------------------------------------------------------------------------- 1 | from ..config import MultiWiiCapability, MultiWiiMultitype, MultiWiiSensor 2 | 3 | from dataclasses import dataclass 4 | from typing import Self 5 | 6 | @dataclass 7 | class MspAnalog: 8 | """ 9 | Represents data values for the MSP_ANALOG command. 10 | 11 | This class encapsulates the analog telemetry data from the MultiWii flight controller. It 12 | provides information about the system's voltage, power meter, RSSI, and amperage. 13 | """ 14 | voltage: float 15 | """float: The voltage of the system, measured in volts.""" 16 | 17 | power_meter_sum: int 18 | """int: The accumulated power consumption, measured in arbitrary units.""" 19 | 20 | rssi: int 21 | """int: The Received Signal Strength Indicator (RSSI), representing the signal strength.""" 22 | 23 | amperage: int 24 | """int: The current amperage drawn, measured in milliamps.""" 25 | 26 | @classmethod 27 | def parse(cls, data: tuple) -> Self: 28 | """ 29 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance 30 | of the `MspAnalog` class. 31 | 32 | Parameters 33 | ---------- 34 | data : tuple 35 | A tuple containing unpacked data values. 36 | 37 | Returns 38 | ------- 39 | MspAnalog 40 | An instance of the `MspAnalog` class populated with the parsed data. 41 | """ 42 | return cls( 43 | voltage=data[0] / 10.0, 44 | power_meter_sum=data[1], 45 | rssi=data[2], 46 | amperage=data[3] 47 | ) 48 | 49 | @dataclass 50 | class MspIdent: 51 | """ 52 | Represents data values for the MSP_IDENT command. 53 | 54 | This class encapsulates the identification data from the MultiWii flight controller. It 55 | provides information about the firmware version, multitype, capabilities, and navigation 56 | version. 57 | """ 58 | version: int 59 | """int: The firmware version of the flight controller.""" 60 | 61 | multitype: MultiWiiMultitype 62 | """MultiWiiMultitype: The vehicle configuration type.""" 63 | 64 | capabilities: tuple[MultiWiiCapability] 65 | """tuple[MultiWiiCapability]: A tuple with available flight capabilities.""" 66 | 67 | navigation_version: int 68 | """The navigation version of the firmware.""" 69 | 70 | @classmethod 71 | def parse(cls, data: tuple) -> Self: 72 | """ 73 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 74 | the `MspIdent` class. 75 | 76 | Parameters 77 | ---------- 78 | data : tuple 79 | A tuple containing unpacked data values. 80 | 81 | Returns 82 | ------- 83 | MspIdent 84 | An instance of the `MspIdent` class populated with the parsed data. 85 | """ 86 | capabilitites = () 87 | 88 | available_capabilities = data[2] 89 | 90 | for capability in MultiWiiCapability: 91 | if capability & available_capabilities == capability: 92 | capabilities += (capability,) 93 | 94 | return cls( 95 | version=data[0], 96 | multitype=MultiWiiMultitype(data[1]), 97 | capabilities=capabilities, 98 | navigation_version=data[3] 99 | ) 100 | 101 | @dataclass 102 | class MspMisc: 103 | """ 104 | Represents data values for the MSP_MISC command. 105 | 106 | This class encapsulates miscellaneous configuration and status data from the MultiWii 107 | flight controller. It includes information about power triggers, throttle settings, 108 | battery warnings, and other miscellaneous parameters. 109 | """ 110 | power_trigger: int 111 | """int: The power trigger value.""" 112 | 113 | throttle_failsafe: int 114 | """int: The throttle failsafe value.""" 115 | 116 | throttle_idle: int 117 | """int: The idle throttle value.""" 118 | 119 | throttle_min: int 120 | """int: The minimum throttle value.""" 121 | 122 | throttle_max: int 123 | """int: The maximum throttle value.""" 124 | 125 | power_logger_arm: int 126 | """int: The power logger arm value.""" 127 | 128 | power_logger_lifetime: int 129 | """int: The power logger lifetime value.""" 130 | 131 | magnetometer_declination: float 132 | """float: The magnetic declination value, measured in degrees.""" 133 | 134 | battery_scale: int 135 | """int: The battery scale value.""" 136 | 137 | battery_warning_1: float 138 | """float: The first battery warning level, measured in volts.""" 139 | 140 | battery_warning_2: float 141 | """float: The second battery warning level, measured in volts.""" 142 | 143 | battery_critical: float 144 | """float: The critical battery level, measured in volts.""" 145 | 146 | @classmethod 147 | def parse(cls, data: tuple) -> Self: 148 | """ 149 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 150 | the `MspMisc` class. 151 | 152 | Parameters 153 | ---------- 154 | data : tuple 155 | A tuple containing unpacked data values. 156 | 157 | Returns 158 | ------- 159 | MspMisc 160 | An instance of the `MspMisc` class populated with the parsed data. 161 | """ 162 | return cls( 163 | power_trigger=data[0], 164 | throttle_failsafe=data[1], 165 | throttle_idle=data[2], 166 | throttle_min=data[3], 167 | throttle_max=data[4], 168 | power_logger_arm=data[5], 169 | power_logger_lifetime=data[6], 170 | magnetometer_declination=data[7] / 10.0, 171 | battery_scale=data[8], 172 | battery_warning_1=data[9] / 10.0, 173 | battery_warning_2=data[10] / 10.0, 174 | battery_critical=data[11] / 10.0 175 | ) 176 | 177 | @dataclass 178 | class MspSetMisc: 179 | """ 180 | Represents data values for the MSP_SET_MISC command. 181 | 182 | This class encapsulates miscellaneous configuration data to be set on the MultiWii flight 183 | controller. It includes information about power triggers, throttle settings, battery 184 | warnings, and other miscellaneous parameters. 185 | """ 186 | power_trigger: int 187 | """int: The power trigger value.""" 188 | 189 | throttle_min: int 190 | """int: The minimum throttle value.""" 191 | 192 | throttle_max: int 193 | """int: The maximum throttle value.""" 194 | 195 | min_command: int 196 | """int: The minimum command value.""" 197 | 198 | throttle_failsafe: int 199 | """int: The throttle value for failsafe.""" 200 | 201 | power_logger_arm: int 202 | """int: The power logger arm value.""" 203 | 204 | power_logger_lifetime: int 205 | """int: The power logger lifetime value.""" 206 | 207 | magnetometer_declination: float 208 | """float: The magnetic declination value, measured in degrees.""" 209 | 210 | battery_scale: int 211 | """int: The battery scale value.""" 212 | 213 | battery_warning_1: float 214 | """float: The first battery warning level, measured in volts.""" 215 | 216 | battery_warning_2: float 217 | """float: The second battery warning level, measured in volts.""" 218 | 219 | battery_critical: float 220 | """float: The critical battery level, measured in volts.""" 221 | 222 | def as_serializable(self) -> tuple[int]: 223 | """ 224 | Returns a tuple with integer values to be used for serialization. 225 | 226 | Returns 227 | ------- 228 | tuple[int] 229 | A tuple with serializable integer values. 230 | """ 231 | return ( 232 | self.power_trigger, 233 | self.throttle_min, 234 | self.throttle_max, 235 | self.min_command, 236 | self.throttle_failsafe, 237 | self.power_logger_arm, 238 | self.power_logger_lifetime, 239 | int(self.magnetometer_declination * 10), 240 | self.battery_scale, 241 | int(self.battery_warning_1 * 10), 242 | int(self.battery_warning_2 * 10), 243 | int(self.battery_critical * 10) 244 | ) 245 | 246 | @dataclass 247 | class MspStatus: 248 | """ 249 | Represents data values for the MSP_STATUS command. 250 | 251 | This class encapsulates the status data from the MultiWii flight controller. It provides 252 | information about cycle time, I2C errors, sensors, flags, and global configuration. 253 | """ 254 | cycle_time: int 255 | """int: The cycle time in microseconds.""" 256 | 257 | i2c_errors: int 258 | """int: The count of I2C errors.""" 259 | 260 | sensors: tuple[MultiWiiSensor] 261 | """tuple[MultiWiiSensor]: A tuple representing the sensors' status.""" 262 | 263 | status_flag: int 264 | """int: The status flag.""" 265 | 266 | global_config: int 267 | """int: The global configuration value.""" 268 | 269 | @classmethod 270 | def parse(cls, data: tuple) -> Self: 271 | """ 272 | Parses a tuple of data values obtained from `struct.unpack` and returns an instance of 273 | the `MspStatus` class. 274 | 275 | Parameters 276 | ---------- 277 | data : tuple 278 | A tuple containing unpacked data values. 279 | 280 | Returns 281 | ------- 282 | MspStatus 283 | An instance of the `MspStatus` class populated with the parsed data. 284 | """ 285 | sensors = () 286 | 287 | available_sensors = data[2] 288 | 289 | for sensor in MultiWiiSensor: 290 | if sensor & available_sensors == sensor: 291 | sensors += (sensor,) 292 | 293 | return cls( 294 | cycle_time=data[0], 295 | i2c_errors=data[1], 296 | sensors=sensors, 297 | status_flag=data[3], 298 | global_config=data[4] 299 | ) -------------------------------------------------------------------------------- /src/multiwii/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | multiwii-proxy-python: A simple and user-friendly Python 3 module for MultiWii-based drones. 3 | 4 | Formerly known as "WiiProxy". 5 | 6 | Copyright (C) 2024 BluDay 7 | 8 | This module only supports communication through MSP v1 and not for any of the newer 9 | versions. Consider using other libraries that uses MSP v2 and newer versions of the 10 | protocol. 11 | 12 | The re-write of this module was a great learning experience for me. 13 | """ 14 | 15 | __author__ = 'BluDay' 16 | 17 | __copyrights__ = '© 2024 BluDay' 18 | 19 | __credits__ = 'BluDay' 20 | 21 | __description__ = 'A simple and user-friendly Python 3 module for MultiWii-based drones.' 22 | 23 | __license__ = 'MIT' 24 | 25 | __maintainer__ = 'BluDay' 26 | 27 | __title__ = 'multiwii-proxy-python' 28 | 29 | __url__ = 'https://bluday.github.io/multiwii-proxy-python' 30 | 31 | __version__ = '3.0' 32 | 33 | from ._command import _MspCommand 34 | 35 | from .commands import ( 36 | MSP_ACC_CALIBRATION, 37 | MSP_ALTITUDE, 38 | MSP_ANALOG, 39 | MSP_ATTITUDE, 40 | MSP_BIND, 41 | MSP_BOX, 42 | MSP_BOXIDS, 43 | MSP_BOXNAMES, 44 | MSP_COMP_GPS, 45 | MSP_EEPROM_WRITE, 46 | MSP_IDENT, 47 | MSP_MAG_CALIBRATION, 48 | MSP_MISC, 49 | MSP_MOTOR, 50 | MSP_MOTOR_PINS, 51 | MSP_PID, 52 | MSP_PIDNAMES, 53 | MSP_RAW_GPS, 54 | MSP_RAW_IMU, 55 | MSP_RC, 56 | MSP_RC_TUNING, 57 | MSP_RESET_CONF, 58 | MSP_SELECT_SETTING, 59 | MSP_SERVO, 60 | MSP_SERVO_CONF, 61 | MSP_SET_BOX, 62 | MSP_SET_HEAD, 63 | MSP_SET_MISC, 64 | MSP_SET_MOTOR, 65 | MSP_SET_PID, 66 | MSP_SET_RAW_GPS, 67 | MSP_SET_RAW_RC, 68 | MSP_SET_RC_TUNING, 69 | MSP_SET_SERVO_CONF, 70 | MSP_SET_WP, 71 | MSP_STATUS, 72 | MSP_WP 73 | ) 74 | 75 | from .data import ( 76 | MspAltitude, 77 | MspAnalog, 78 | MspAttitude, 79 | MspBox, 80 | MspBoxIds, 81 | MspBoxItem, 82 | MspBoxNames, 83 | MspCompGps, 84 | MspIdent, 85 | MspMisc, 86 | MspMotor, 87 | MspMotorPins, 88 | MspPid, 89 | MspPidNames, 90 | MspRawGps, 91 | MspRawImu, 92 | MspRc, 93 | MspRcTuning, 94 | MspServo, 95 | MspServoConf, 96 | MspServoConfItem, 97 | MspSetMisc, 98 | MspStatus, 99 | MspWaypoint 100 | ) 101 | 102 | from .messaging import ( 103 | _crc8_xor, 104 | _create_request_message, 105 | _MspResponseMessage, 106 | _parse_response_message, 107 | MESSAGE_ERROR_HEADER, 108 | MESSAGE_INCOMING_HEADER, 109 | MspMessageError 110 | ) 111 | 112 | from serial import Serial 113 | 114 | from time import perf_counter, sleep 115 | 116 | from typing import Any, Final, NoReturn, Type 117 | 118 | class MultiWii(object): 119 | """ 120 | The main class for wiiproxy that handles communication with MultiWii flight controllers. 121 | 122 | This class requires an open serial port with a baudrate of 115200 to be passed at 123 | instantiation. 124 | 125 | Note 126 | ---- 127 | This class supports MSP v1 and does not support any newer versions. 128 | 129 | Note 130 | ---- 131 | This class can be imported directly through the main module. 132 | """ 133 | DEFAULT_MESSAGE_WRITE_READ_DELAY: Final[float] = 0.005 134 | """float: The default delay in seconds between writing and reading messages.""" 135 | 136 | MSP_VERSION: Final[int] = 1 137 | """int: The supported MultiWii Serial Protocol version.""" 138 | 139 | _message_write_read_delay: float 140 | 141 | _serial_port: Final[Serial] 142 | 143 | def __init__(self, serial_port: Serial) -> NoReturn: 144 | """ 145 | Initializes an instance using the provided serial port. 146 | 147 | This constructor initializes a new instance of the MultiWii class using the provided 148 | serial port for communication with the FC. It sets up the initial state of the object, 149 | including the activation status, command write-read delay and serial port configuration. 150 | Additionally, it ensures that the provided serial port is of the correct type (Serial). 151 | If the serial port is not of the expected type, a TypeError is raised. 152 | 153 | Parameters 154 | ---------- 155 | serial : Serial 156 | The serial port instance used for communication with the FC. This should be an 157 | instance of the `Serial` class from the `pyserial` library, which provides the 158 | interface for serial communication. 159 | 160 | Raises 161 | ------ 162 | TypeError 163 | If the provided serial port instance is not an instance of the `Serial` class. 164 | """ 165 | if not isinstance(serial_port, Serial): 166 | raise TypeError('The serial port must be an instance of "Serial".') 167 | 168 | self._command_to_data_structure_type_map = { 169 | MSP_ALTITUDE: MspAltitude, 170 | MSP_ANALOG: MspAnalog, 171 | MSP_ATTITUDE: MspAttitude, 172 | MSP_BOX: MspBox, 173 | MSP_BOXIDS: MspBoxIds, 174 | MSP_BOXNAMES: MspBoxNames, 175 | MSP_COMP_GPS: MspCompGps, 176 | MSP_IDENT: MspIdent, 177 | MSP_MISC: MspMisc, 178 | MSP_MOTOR: MspMotor, 179 | MSP_MOTOR_PINS: MspMotorPins, 180 | MSP_PID: MspPid, 181 | MSP_PIDNAMES: MspPidNames, 182 | MSP_RAW_GPS: MspRawGps, 183 | MSP_RAW_IMU: MspRawImu, 184 | MSP_RC: MspRc, 185 | MSP_RC_TUNING: MspRcTuning, 186 | MSP_SERVO: MspServo, 187 | MSP_SERVO_CONF: MspServoConf, 188 | MSP_STATUS: MspStatus, 189 | MSP_WP: MspWaypoint 190 | } 191 | 192 | self._message_write_read_delay = self.DEFAULT_MESSAGE_WRITE_READ_DELAY 193 | 194 | self._serial_port = serial_port 195 | 196 | @property 197 | def command_to_data_structure_type_map(self) -> dict[_MspCommand, Type]: 198 | """ 199 | Gets the command to data structure type dictionary. 200 | 201 | Returns 202 | ------- 203 | dict[_MspCommand, Type] 204 | A instance with a copy of the map. 205 | """ 206 | return dict(self._command_to_data_structure_type_map) 207 | 208 | @property 209 | def message_write_read_delay(self) -> float: 210 | """ 211 | Gets the delay (in seconds) between each write and read message. 212 | 213 | Returns 214 | ------- 215 | float 216 | The delay in seconds. 217 | """ 218 | return self._message_write_read_delay 219 | 220 | @property 221 | def serial_port(self) -> Serial: 222 | """ 223 | Gets the serial port instance. 224 | 225 | Returns 226 | ------- 227 | Serial 228 | The serial port instance. 229 | """ 230 | return self._serial_port 231 | 232 | @message_write_read_delay.setter 233 | def message_write_read_delay(self, value: float) -> NoReturn: 234 | """ 235 | Sets the delay (in seconds) between each write and read command. 236 | 237 | This property controls the delay between each write message followed by a read message 238 | sent to the FC. A message with empty data values is sent first, followed by a delay, 239 | and then a read message to retrieve information from the FC. 240 | 241 | Parameters 242 | ---------- 243 | value : float 244 | The delay in seconds. 245 | 246 | Raises 247 | ------ 248 | TypeError 249 | If the value is not a float. 250 | ValueError 251 | If the value is a negative number. 252 | """ 253 | if not isinstance(value, float): 254 | raise TypeError('Value must be a float.') 255 | 256 | if value < 0: 257 | raise ValueError('Value must be a non-negative number.') 258 | 259 | self._message_write_read_delay = value 260 | 261 | def _read_response_message(self, command: _MspCommand) -> _MspResponseMessage: 262 | """ 263 | Reads a response message from the FC using the MSP command. 264 | 265 | Note 266 | ---- 267 | This method sends a write message with empty values to the FC in order to retrieve a 268 | response message. Ensure that the FC is ready to to respond to the command code sent. 269 | 270 | Parameters 271 | ---------- 272 | command : _MspCommand 273 | An instance of `_MspCommand` representing the MSP command used to read the message. 274 | 275 | Raises 276 | ------ 277 | MspMessageError 278 | If an error message is returned from the FC. 279 | 280 | Returns 281 | ------- 282 | _MspResponseMessage 283 | A named tuple with the command, parsed data and additional information. 284 | """ 285 | try: 286 | self._send_request_message(command) 287 | 288 | sleep(self._message_write_read_delay) 289 | 290 | header = self._serial_port.read(3) 291 | 292 | if header == MESSAGE_ERROR_HEADER: 293 | raise MspMessageError('An error has occured.') 294 | 295 | if header != MESSAGE_INCOMING_HEADER: 296 | raise MspMessageError('Invalid incoming message preamble received.') 297 | 298 | command_code = self._serial_port.read(1) 299 | 300 | if command_code[0] != command.code: 301 | raise MspMessageError( 302 | 'Invalid command code detected. ({}, {})'.format( 303 | command.code, 304 | command_code 305 | ) 306 | ) 307 | 308 | payload = bytes() 309 | 310 | payload += command_code 311 | 312 | data_size = self._serial_port.read(1) 313 | data = self._serial_port.read(data_size[0]) 314 | 315 | payload += data_size 316 | payload += data 317 | 318 | checksum = self._serial_port.read(1) 319 | 320 | if checksum[0] != _crc8_xor(payload): 321 | raise MspMessageError(f'Invalid payload checksum detected for {command}.') 322 | 323 | return _parse_response_message(command, payload) 324 | finally: 325 | self._serial_port.reset_input_buffer() 326 | 327 | def _send_request_message(self, command: _MspCommand, data: tuple[int] = ()) -> NoReturn: 328 | """ 329 | Sends a request message to the FC using the provided MSP command and data values. 330 | 331 | Parameters 332 | ---------- 333 | command : _MspCommand 334 | An instance of `_MspCommand` representing the MSP command used to write the message. 335 | data : tuple[int] 336 | Data values to serialize and include in the message payload. 337 | """ 338 | try: 339 | self._serial_port.write(_create_request_message(command, data)) 340 | finally: 341 | self._serial_port.reset_output_buffer() 342 | 343 | def arm(self) -> NoReturn: 344 | """ 345 | Arms the vehicle. 346 | 347 | This method prepares the vehicle for operation by simulating the arming sequence 348 | performed by physical transmitters. It sets the throttle value to its minimum, and 349 | the yaw value to its maximum, for a few seconds simultaneously to initiate the arming 350 | process. This ensures that the vehicle is ready for further commands and a safe flight. 351 | 352 | Note 353 | ---- 354 | Ensure that the vehicle is in a safe enviornment and that conditions are suitable for 355 | arming before invoking this method. 356 | """ 357 | data = MspRc( 358 | roll=1500, 359 | pitch=1500, 360 | yaw=2000, 361 | throttle=1000, 362 | aux1=0, 363 | aux2=0, 364 | aux3=0, 365 | aux4=0 366 | ) 367 | 368 | start_time = perf_counter() 369 | 370 | elapsed_time = 0 371 | 372 | while elapsed_time < 0.5: 373 | self.set_raw_rc(data) 374 | 375 | sleep(0.05) 376 | 377 | elapsed_time = perf_counter() - start_time 378 | 379 | def bind_transmitter_and_receiver(self) -> NoReturn: 380 | """ 381 | Sends an MSP_BIND command to the FC using the provided data values. 382 | 383 | This command initiates the binding process between the transmitter (radio controller) 384 | and the receiver connected to the FC. Binding establishes a secure communication link 385 | between the transmitter (TX) and the receiver (RX). 386 | 387 | Note 388 | ---- 389 | Ensure that the FC is ready to receive the bind command and that the transmitter is in 390 | binding mode before calling this method. 391 | """ 392 | self._send_request_message(MSP_BIND) 393 | 394 | def calibrate_accelerometer(self) -> NoReturn: 395 | """ 396 | Sends an MSP_ACC_CALIBRATION command to the FC using the provided data values. 397 | 398 | This command initiates the accelerometer calibration process on the FC. Accelerometer 399 | calibration is essential for accurate attitude estimation and stabilization of the 400 | aircraft. 401 | 402 | Note 403 | ---- 404 | The FC should be placed on a level surface during the calibration to ensure accurate 405 | results. Avoid moving or disturbing the FC during the process. 406 | """ 407 | self._send_request_message(MSP_ACC_CALIBRATION) 408 | 409 | def calibrate_magnetometer(self) -> NoReturn: 410 | """ 411 | Sends an MSP_MAG_CALIBRATION command to the FC using the provided data values. 412 | 413 | This command initiates the magnetometer (compass) calibration process on the FC. 414 | Magnetometer calibration is crucial for accurate heading estimation and navigation, 415 | especially in GPS-assisted flight modes. 416 | 417 | Note 418 | ---- 419 | The FC should be rotated along all three axes (roll, pitch, yaw) in a smooth and 420 | consistent manner to ensure accurate results. Avoid any magnetic interference or 421 | disturbances during the process. 422 | """ 423 | self._send_request_message(MSP_ACC_CALIBRATION) 424 | 425 | def disarm(self) -> NoReturn: 426 | """ 427 | Disarms the vehicle. 428 | 429 | This method safely disarms the vehicle by resetting the yaw and throttle to their minimum 430 | values. This process simulates the disarming sequence used by physical transmitters, 431 | ensuring that the vehicle is no longer ready for flight, and that the vehicle is in a 432 | more safe state. 433 | 434 | Note 435 | ---- 436 | Always ensure that the vehicle is on the ground and stationary before invoking this 437 | method to avoid accidental movement or damage. 438 | """ 439 | data = MspRc( 440 | roll=1500, 441 | pitch=1500, 442 | yaw=1000, 443 | throttle=1000, 444 | aux1=0, 445 | aux2=0, 446 | aux3=0, 447 | aux4=0 448 | ) 449 | 450 | start_time = perf_counter() 451 | 452 | elapsed_time = 0 453 | 454 | while elapsed_time < 0.5: 455 | self.set_raw_rc(data) 456 | 457 | sleep(0.05) 458 | 459 | elapsed_time = perf_counter() - start_time 460 | 461 | def get_data(self, command: _MspCommand) -> Any: 462 | """ 463 | Sends a given command to the FC and parses the retrieved data values. 464 | 465 | Parameters 466 | ---------- 467 | command : _MspCommand 468 | An instance of `_MspCommand` representing the MSP command to get corresponding data 469 | values for. 470 | 471 | Returns 472 | ------- 473 | Any 474 | An instance of a corresponding data structure type for the given command. 475 | """ 476 | data = self._read_response_message(command).data 477 | 478 | return self._command_to_data_structure_type_map[command].parse(data) 479 | 480 | def reset_config(self) -> NoReturn: 481 | """ 482 | Sends an MSP_RESET_CONF command to the FC using the provided data values. 483 | 484 | This command resets the configuration settings on the FC to their default values. 485 | It effectively restores the FC to its initial configuration state, clearing any 486 | customized settings or adjustments made by the user. 487 | 488 | Note 489 | ---- 490 | Resetting the config should be done with caution, as it will revert all settings to 491 | their defaults. Make sure to reconfigure the FC according to your requirements after 492 | executing this command to the FC using the provided data values. 493 | """ 494 | self._send_request_message(MSP_RESET_CONF) 495 | 496 | def save_config_to_eeprom(self) -> NoReturn: 497 | """ 498 | Sends an MSP_EEPROM_WRITE command to the FC using the provided data values. 499 | 500 | This command writes the current configuration settings to the EEPROM of the FC. 501 | 502 | Note 503 | ---- 504 | Writing to EEPROM should be done with caution, as it modifies the stored config 505 | directly. Ensure that the values written are valid and intended, as incorrect 506 | values could lead to unexpected behavior or instability. 507 | """ 508 | self._send_request_message(MSP_EEPROM_WRITE) 509 | 510 | def select_setting(self, value: int) -> NoReturn: 511 | """ 512 | Sends an MSP_SELECT_SETTING command to the FC using the provided data values. 513 | 514 | Selects the "setting configuration" with different PID and rate values using the given 515 | range value. 516 | 517 | Parameters 518 | ---------- 519 | value : int 520 | A value of 0, 1 or 2. 521 | 522 | Raises 523 | ------ 524 | ValueError 525 | If the provided value is not 0, 1 or 2. 526 | """ 527 | if not value in (0, 1, 2): 528 | raise ValueError('Value must be 0, 1 or 2.') 529 | 530 | self._send_request_message(MSP_SELECT_SETTING, data=(value,)) 531 | 532 | def set_boxes(self, data: MspBox) -> NoReturn: 533 | """ 534 | Sends an MSP_SET_BOX command to the FC using the provided data values. 535 | 536 | Sets the flight modes (or "boxes") config on the FC. Flight modes define the behavior 537 | of the aircraft based on various inputs from the transmitter or other sources. 538 | 539 | Parameters 540 | ---------- 541 | data : tuple[MspBoxItem] 542 | A tuple of non-null `MspBoxItem` values. 543 | """ 544 | self._send_request_message(MSP_SET_BOX, data.as_serializable()) 545 | 546 | def set_head(self, value: int) -> NoReturn: 547 | """ 548 | Sends an MSP_SET_HEAD command to the FC using the provided data values. 549 | 550 | Sets the heading (yaw) direction reference on the FC with a value range of -180 to 180. 551 | It is used to define the forward direction of the aircraft relative to its orientation. 552 | 553 | Parameters 554 | ---------- 555 | range : int 556 | The heading direction value within a range of -180 and 180. 557 | 558 | Raises 559 | ------ 560 | ValueError 561 | If the provided range value is less than -180 or greater than 180. 562 | """ 563 | if not -180 <= value <= 180: 564 | raise ValueError('Value must be within the range of -180 and 180.') 565 | 566 | self._send_request_message(MSP_SET_HEAD, data=(value,)) 567 | 568 | def set_misc_config(self, data: MspSetMisc) -> NoReturn: 569 | """ 570 | Sends an MSP_SET_MISC command to the FC using the provided data values. 571 | 572 | Sets miscellaneous config parameters on the FC—such as battery voltage scaling, failsafe 573 | behavior, or other settings not covered by specific MSP commands. 574 | 575 | Parameters 576 | ---------- 577 | data : MspSetMisc 578 | An instance of the `MspSetMisc` class populated with values. 579 | """ 580 | self._send_request_message(MSP_SET_MISC, data.as_serializable()) 581 | 582 | def set_motors(self, data: MspMotor) -> NoReturn: 583 | """ 584 | Sends an MSP_SET_MOTOR command to the FC using the provided data values. 585 | 586 | Sets the motor output values on the FC. Motor output values determine the throttle level 587 | for each motor, controlling the rotation speed and thrust generated by the motors. 588 | 589 | Parameters 590 | ---------- 591 | data : MspMotor 592 | An instance of the `MspMotor` class populated with values. 593 | """ 594 | self._send_request_message(MSP_SET_MOTOR, data.as_serializable()) 595 | 596 | def set_pid_values(self, data: MspPid) -> NoReturn: 597 | """ 598 | Sends an MSP_SET_PID command to the FC using the provided data values. 599 | 600 | Sets the PID values on the FC. PID values are used to adjust the stability and response 601 | characteristics of the aircraft. 602 | 603 | Parameters 604 | ---------- 605 | data : MspPid 606 | An instance of the `MspPid` class populated with values. 607 | """ 608 | self._send_request_message(MSP_SET_PID, data.as_serializable()) 609 | 610 | def set_raw_gps(self, data: MspRawGps) -> NoReturn: 611 | """ 612 | Sends an MSP_SET_RAW_GPS command to the FC using the provided data values. 613 | 614 | Sets the raw GPS data on the FC—such as the latitude, longitude, altitude, and other 615 | GPS-related information. 616 | 617 | Parameters 618 | ---------- 619 | data : MspRawGps 620 | An instance of the `MspRawGps` class populated with values. 621 | """ 622 | self._send_request_message(MSP_SET_RAW_GPS, data.as_serializable()) 623 | 624 | def set_raw_rc(self, data: MspRc) -> NoReturn: 625 | """ 626 | Sends an MSP_SET_RAW_RC command to the FC using the provided data values. 627 | 628 | Sets the raw receiver (RC/RX) channel data on the FC. Raw FC data includes the pulse 629 | width values received from the transmitter for each channel, typically representing 630 | control inputs such as throttle, pitch, roll, and yaw. 631 | 632 | Parameters 633 | ---------- 634 | data : MspRc 635 | An instance of the `MspRc` class populated with values. 636 | """ 637 | self._send_request_message(MSP_SET_RAW_RC, data.as_serializable()) 638 | 639 | def set_rc_tuning(self, data: MspRcTuning) -> NoReturn: 640 | """ 641 | Sends an MSP_SET_RC_TUNING command to the FC using the provided data values. 642 | 643 | Sets RC tuning parameters on the FC—such as expo, rates, and other settings related to 644 | RC control response and behavior. 645 | 646 | Parameters 647 | ---------- 648 | data : MspRcTuning 649 | An instance of the `MspRcTuning` class populated with values. 650 | """ 651 | self._send_request_message(MSP_SET_RC_TUNING, data.as_serializable()) 652 | 653 | def set_servo_config(self, data: MspServoConf) -> NoReturn: 654 | """ 655 | Sends an MSP_SET_SERVO_CONF command to the FC using the provided data values. 656 | 657 | Sets servo config parameters on the FC—such as servo mapping, direction, endpoints, and 658 | other settings related to servo control. 659 | 660 | Parameters 661 | ---------- 662 | data : tuple[MspServoConfItem] 663 | A tuple with instances of the `MspServoConfItem` class populated with values. 664 | """ 665 | self._send_request_message(MSP_SET_SERVO_CONF, data.as_serializable()) 666 | 667 | def set_waypoint(self, data: MspWaypoint) -> NoReturn: 668 | """ 669 | Sends an MSP_SET_WP command to the FC using the provided data values. 670 | 671 | Dispatches a command to set a waypoint on the FC, providing specific latitude, longitude, 672 | altitude, heading, duration and navigation flags for precise navigation and waypoint 673 | management. 674 | 675 | Parameters 676 | ---------- 677 | data : MsWaypoint 678 | An instance of the `MspWaypoint` class populated with values. 679 | """ 680 | self._send_request_message(MSP_SET_WP, data.as_serializable()) --------------------------------------------------------------------------------