mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 01:29:33 +00:00
Add Python library (#20)
This commit is contained in:
parent
779fa13e80
commit
1f47aa6d62
15
.github/workflows/tools.yml
vendored
15
.github/workflows/tools.yml
vendored
@ -19,6 +19,21 @@ jobs:
|
|||||||
echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv
|
echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv
|
||||||
./csv_to_ulog log.csv
|
./csv_to_ulog log.csv
|
||||||
test $(stat -c %s log.ulg) -eq 196
|
test $(stat -c %s log.ulg) -eq 196
|
||||||
|
pyflix:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
- name: Install Python build tools
|
||||||
|
run: pip install build
|
||||||
|
- name: Build pyflix
|
||||||
|
run: python3 -m build tools
|
||||||
|
- name: Upload artifacts
|
||||||
|
uses: actions/upload-artifact@v4
|
||||||
|
with:
|
||||||
|
name: pyflix
|
||||||
|
path: |
|
||||||
|
tools/dist/pyflix-*.tar.gz
|
||||||
|
tools/dist/pyflix-*.whl
|
||||||
python_tools:
|
python_tools:
|
||||||
runs-on: ubuntu-latest
|
runs-on: ubuntu-latest
|
||||||
steps:
|
steps:
|
||||||
|
2
.gitignore
vendored
2
.gitignore
vendored
@ -2,6 +2,8 @@
|
|||||||
*.elf
|
*.elf
|
||||||
build/
|
build/
|
||||||
tools/log/
|
tools/log/
|
||||||
|
tools/dist/
|
||||||
|
*.egg-info/
|
||||||
.dependencies
|
.dependencies
|
||||||
.vscode/*
|
.vscode/*
|
||||||
!.vscode/settings.json
|
!.vscode/settings.json
|
||||||
|
@ -22,6 +22,7 @@
|
|||||||
* Precise simulation with Gazebo.
|
* Precise simulation with Gazebo.
|
||||||
* Wi-Fi and MAVLink support.
|
* Wi-Fi and MAVLink support.
|
||||||
* Wireless command line interface and analyzing.
|
* Wireless command line interface and analyzing.
|
||||||
|
* Python library.
|
||||||
* Textbook on flight control theory and practice ([in development](https://quadcopter.dev)).
|
* Textbook on flight control theory and practice ([in development](https://quadcopter.dev)).
|
||||||
* *Position control (using external camera) and autonomous flights¹*.
|
* *Position control (using external camera) and autonomous flights¹*.
|
||||||
|
|
||||||
@ -53,6 +54,7 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
|
|||||||
* [Building and running the code](docs/build.md).
|
* [Building and running the code](docs/build.md).
|
||||||
* [Troubleshooting](docs/troubleshooting.md).
|
* [Troubleshooting](docs/troubleshooting.md).
|
||||||
* [Firmware architecture overview](docs/firmware.md).
|
* [Firmware architecture overview](docs/firmware.md).
|
||||||
|
* [Python library tutorial](tools/pyflix/README.md).
|
||||||
* [Log analysis](docs/log.md).
|
* [Log analysis](docs/log.md).
|
||||||
* [User builds gallery](docs/user.md).
|
* [User builds gallery](docs/user.md).
|
||||||
|
|
||||||
|
@ -37,7 +37,7 @@ void sendMavlink() {
|
|||||||
|
|
||||||
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
|
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
|
||||||
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
|
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
|
||||||
0, MAV_STATE_STANDBY);
|
mode, MAV_STATE_STANDBY);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
|
|
||||||
mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
@ -208,7 +208,9 @@ void sendMavlinkPrint() {
|
|||||||
strlcpy(data, str + i, sizeof(data));
|
strlcpy(data, str + i, sizeof(data));
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
SERIAL_CONTROL_DEV_SHELL, 0, 0, 0, strlen(data), (uint8_t *)data, 0, 0);
|
SERIAL_CONTROL_DEV_SHELL,
|
||||||
|
i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go
|
||||||
|
0, 0, strlen(data), (uint8_t *)data, 0, 0);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
mavlinkPrintBuffer.clear();
|
mavlinkPrintBuffer.clear();
|
||||||
|
13
tools/cli.py
Executable file
13
tools/cli.py
Executable file
@ -0,0 +1,13 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
# Remote CLI for Flix
|
||||||
|
|
||||||
|
from pyflix import Flix
|
||||||
|
|
||||||
|
flix = Flix()
|
||||||
|
|
||||||
|
flix.on('print', lambda text: print(text, end=''))
|
||||||
|
|
||||||
|
while True:
|
||||||
|
command = input()
|
||||||
|
flix.cli(command, wait_response=False)
|
39
tools/example.py
Executable file
39
tools/example.py
Executable file
@ -0,0 +1,39 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
import math
|
||||||
|
from pyflix import Flix
|
||||||
|
|
||||||
|
print('=== Connect...')
|
||||||
|
flix = Flix()
|
||||||
|
|
||||||
|
print('Connected:', flix.connected)
|
||||||
|
print('Mode:', flix.mode)
|
||||||
|
print('Armed:', flix.armed)
|
||||||
|
print('Landed:', flix.landed)
|
||||||
|
print('Rates:', *[f'{math.degrees(r):.0f}°/s' for r in flix.rates])
|
||||||
|
print('Attitude:', *[f'{math.degrees(a):.0f}°' for a in flix.attitude_euler])
|
||||||
|
print('Motors:', flix.motors)
|
||||||
|
print('Acc', flix.acc)
|
||||||
|
print('Gyro', flix.gyro)
|
||||||
|
|
||||||
|
print('=== Execute commands...')
|
||||||
|
print('> time')
|
||||||
|
print(flix.cli('time'))
|
||||||
|
print('> imu')
|
||||||
|
print(flix.cli('imu'))
|
||||||
|
|
||||||
|
print('=== Get parameter...')
|
||||||
|
pitch_p = flix.get_param('PITCH_P')
|
||||||
|
print('PITCH_P = ', pitch_p)
|
||||||
|
|
||||||
|
print('=== Set parameter...')
|
||||||
|
flix.set_param('PITCH_P', pitch_p)
|
||||||
|
|
||||||
|
print('=== Wait for gyro update...')
|
||||||
|
print('Gyro: ', flix.wait('gyro'))
|
||||||
|
|
||||||
|
print('=== Wait for HEARTBEAT message...')
|
||||||
|
print(flix.wait('mavlink.HEARTBEAT'))
|
||||||
|
|
||||||
|
print('=== When until landed = False (remove drone from the surface)')
|
||||||
|
flix.wait('landed', value=False)
|
23
tools/log.py
Executable file
23
tools/log.py
Executable file
@ -0,0 +1,23 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
# Download flight log remotely and save to file
|
||||||
|
|
||||||
|
import os
|
||||||
|
import datetime
|
||||||
|
from pyflix import Flix
|
||||||
|
|
||||||
|
DIR = os.path.dirname(os.path.realpath(__file__))
|
||||||
|
|
||||||
|
flix = Flix()
|
||||||
|
|
||||||
|
print('Downloading log...')
|
||||||
|
lines = flix.cli('log').splitlines()
|
||||||
|
|
||||||
|
# sort by timestamp
|
||||||
|
header = lines.pop(0)
|
||||||
|
lines.sort(key=lambda line: float(line.split(',')[0]))
|
||||||
|
|
||||||
|
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
||||||
|
content = header.encode() + b'\n' + b'\n'.join(line.encode() for line in lines)
|
||||||
|
log.write(content)
|
||||||
|
print(f'Written {os.path.relpath(log.name, os.curdir)}')
|
225
tools/pyflix/README.md
Normal file
225
tools/pyflix/README.md
Normal file
@ -0,0 +1,225 @@
|
|||||||
|
# Flix Python library
|
||||||
|
|
||||||
|
The Flix Python library allows you to remotely connect to a Flix quadcopter. It provides access to telemetry data, supports executing CLI commands, and controlling the drone's flight.
|
||||||
|
|
||||||
|
To use the library, connect to the drone's Wi-Fi. To use it with the simulator, ensure the script runs on the same local network as the simulator.
|
||||||
|
|
||||||
|
## Installation
|
||||||
|
|
||||||
|
If you have cloned the [repo](https://github.com/okalachev/flix), install the library from the repo:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /path/to/flix/repo
|
||||||
|
pip install -e tools
|
||||||
|
```
|
||||||
|
|
||||||
|
Alternatively, install from pip:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
pip install pyflix
|
||||||
|
```
|
||||||
|
|
||||||
|
## Usage
|
||||||
|
|
||||||
|
The API is accessed through the `Flix` class:
|
||||||
|
|
||||||
|
```python
|
||||||
|
from flix import Flix
|
||||||
|
flix = Flix() # create a Flix object and wait for connection
|
||||||
|
```
|
||||||
|
|
||||||
|
### Telemetry
|
||||||
|
|
||||||
|
Basic telemetry is available through object properties. The properties names generally match the corresponding variables in the firmware itself:
|
||||||
|
|
||||||
|
```python
|
||||||
|
print(flix.connected) # True if connected to the drone
|
||||||
|
print(flix.mode) # current flight mode (str)
|
||||||
|
print(flix.armed) # True if the drone is armed
|
||||||
|
print(flix.landed) # True if the drone is landed
|
||||||
|
print(flix.attitude) # attitude quaternion [w, x, y, z]
|
||||||
|
print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw]
|
||||||
|
print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate]
|
||||||
|
print(flix.channels) # raw RC channels (list)
|
||||||
|
print(flix.motors) # motors outputs (list)
|
||||||
|
print(flix.acc) # accelerometer output (list)
|
||||||
|
print(flix.gyro) # gyroscope output (list)
|
||||||
|
```
|
||||||
|
|
||||||
|
> [!NOTE]
|
||||||
|
> The library uses the Front-Left-Up coordinate system — the same as in the firmware. All angles are in radians.
|
||||||
|
|
||||||
|
### Events
|
||||||
|
|
||||||
|
The Flix object implements the *Observable* pattern, allowing to listen for events. You can subscribe to events using `on` method:
|
||||||
|
|
||||||
|
```python
|
||||||
|
flix.on('connected', lambda: print('Connected to Flix'))
|
||||||
|
flix.on('disconnected', lambda: print('Disconnected from Flix'))
|
||||||
|
flix.on('print', lambda text: print(f'Flix says: {text}'))
|
||||||
|
```
|
||||||
|
|
||||||
|
You can also wait for specific events using `wait` method. This method returns the data associated with the event:
|
||||||
|
|
||||||
|
```python
|
||||||
|
gyro = flix.wait('gyro') # wait for gyroscope update
|
||||||
|
attitude = flix.wait('attitude', timeout=3) # wait for attitude update, raise TimeoutError after 3 seconds
|
||||||
|
```
|
||||||
|
|
||||||
|
The `value` argument specifies a condition for filtering events. It can be either an expected value or a callable:
|
||||||
|
|
||||||
|
```python
|
||||||
|
flix.wait('armed', value=True) # wait until armed
|
||||||
|
flix.wait('armed', value=False) # wait until disarmed
|
||||||
|
flix.wait('motors', value=lambda motors: all(m == 0 for m in motors)) # wait until all motors stop
|
||||||
|
flix.wait('attitude_euler', value=lambda att: att[0] > 0) # wait until roll angle is positive
|
||||||
|
```
|
||||||
|
|
||||||
|
Full list of events:
|
||||||
|
|
||||||
|
|Event|Description|Associated data|
|
||||||
|
|-----|-----------|----------------|
|
||||||
|
|`connected`|Connected to the drone||
|
||||||
|
|`disconnected`|Connection is lost||
|
||||||
|
|`armed`|Armed state update|Armed state (*bool*)|
|
||||||
|
|`mode`|Flight mode update|Flight mode (*str*)|
|
||||||
|
|`landed`|Landed state update|Landed state (*bool*)|
|
||||||
|
|`print`|The drone sends text to the console|Text|
|
||||||
|
|`attitude`|Attitude update|Attitude quaternion (*list*)|
|
||||||
|
|`attitude_euler`|Attitude update|Euler angles (*list*)|
|
||||||
|
|`rates`|Angular rates update|Angular rates (*list*)|
|
||||||
|
|`channels`|Raw RC channels update|Raw RC channels (*list*)|
|
||||||
|
|`motors`|Motors outputs update|Motors outputs (*list*)|
|
||||||
|
|`acc`|Accelerometer update|Accelerometer output (*list*)|
|
||||||
|
|`gyro`|Gyroscope update|Gyroscope output (*list*)|
|
||||||
|
|`mavlink`|Received MAVLink message|Message object|
|
||||||
|
|`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|
||||||
|
|`mavlink.<message_id>`|Received specific MAVLink message|Message object|
|
||||||
|
|`value`|Named value update (see below)|Name, value|
|
||||||
|
|`value.<name>`|Specific named value update (see bellow)|Value|
|
||||||
|
|
||||||
|
> [!NOTE]
|
||||||
|
> Update events trigger on every new data from the drone, and do not mean the value is changed.
|
||||||
|
|
||||||
|
### Common methods
|
||||||
|
|
||||||
|
Get and set firmware parameters using `get_param` and `set_param` methods:
|
||||||
|
|
||||||
|
```python
|
||||||
|
print(flix.get_param('PITCH_P')) # get parameter value
|
||||||
|
flix.set_param('PITCH_P', 5) # set parameter value
|
||||||
|
```
|
||||||
|
|
||||||
|
Execute CLI commands using `cli` method. This method returns command response:
|
||||||
|
|
||||||
|
```python
|
||||||
|
imu = flix.cli('imu') # get detailed IMU data
|
||||||
|
time = flix.cli('time') # get detailed time data
|
||||||
|
flix.cli('reboot') # reboot the drone
|
||||||
|
```
|
||||||
|
|
||||||
|
> [!TIP]
|
||||||
|
> Use `help` command to get the list of available commands.
|
||||||
|
|
||||||
|
### Automatic flight
|
||||||
|
|
||||||
|
The flight control feature is in development. List of methods intended for automatic flight control:
|
||||||
|
|
||||||
|
* `set_position`
|
||||||
|
* `set_attitude`
|
||||||
|
* `set_rates`
|
||||||
|
* `set_motors`
|
||||||
|
* `set_controls`
|
||||||
|
* `set_mode`
|
||||||
|
|
||||||
|
## Usage alongside QGroundControl
|
||||||
|
|
||||||
|
You can use the Flix library alongside the QGroundControl app, using a proxy mode. To do that:
|
||||||
|
|
||||||
|
1. Run proxy for `pyflix` and QGroundControl in background:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
flix-proxy
|
||||||
|
```
|
||||||
|
|
||||||
|
2. Go to QGroundControl settings ⇒ *Comm Links*.
|
||||||
|
3. Add new link with the following settings:
|
||||||
|
* *Name*: Proxy
|
||||||
|
* *Automatically Connect on Start*: ✓
|
||||||
|
* *Type*: UDP
|
||||||
|
* *Port*: 14560
|
||||||
|
4. Restart QGroundControl.
|
||||||
|
|
||||||
|
Now you can run `pyflix` scripts and QGroundControl simultaneously.
|
||||||
|
|
||||||
|
## Tools
|
||||||
|
|
||||||
|
The following scripts demonstrate how to use the library:
|
||||||
|
|
||||||
|
* [`cli.py`](../cli.py) — remote access to the drone's command line interface.
|
||||||
|
* [`log.py`](../log.py) — download flight logs from the drone.
|
||||||
|
* [`example.py`](../example.py) — a simple example, prints telemetry data and waits for events.
|
||||||
|
|
||||||
|
## Advanced usage
|
||||||
|
|
||||||
|
### MAVLink
|
||||||
|
|
||||||
|
You can access the most recently received messages using `messages` property:
|
||||||
|
|
||||||
|
```python
|
||||||
|
print(flix.messages.get('HEARTBEAT')) # print the latest HEARTBEAT message
|
||||||
|
```
|
||||||
|
|
||||||
|
You can wait for a specific message using `wait` method:
|
||||||
|
|
||||||
|
```python
|
||||||
|
heartbeat = flix.wait('mavlink.HEARTBEAT')
|
||||||
|
```
|
||||||
|
|
||||||
|
You can send raw messages using `mavlink` property:
|
||||||
|
|
||||||
|
```python
|
||||||
|
from pymavlink.dialects.v20 import common as mavlink
|
||||||
|
|
||||||
|
flix.mavlink.heartbeat_send(mavlink.MAV_TYPE_GCS, mavlink.MAV_AUTOPILOT_INVALID,
|
||||||
|
mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 0, 0)
|
||||||
|
```
|
||||||
|
|
||||||
|
### Named values
|
||||||
|
|
||||||
|
You can pass arbitrary named values from the firmware to the Python script using `NAMED_VALUE_FLOAT`, `NAMED_VALUE_INT`, `DEBUG`, `DEBUG_VECT`, and `DEBUG_FLOAT_ARRAY` MAVLink messages.
|
||||||
|
|
||||||
|
All these named values will appear in the `values` dictionary:
|
||||||
|
|
||||||
|
```python
|
||||||
|
print(flix.values['some_value'])
|
||||||
|
print(flix.values['some_vector'])
|
||||||
|
```
|
||||||
|
|
||||||
|
You can send values from the firmware like this (`mavlink.ino`):
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
// Send float named value
|
||||||
|
mavlink_msg_named_value_float_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, t, "some_value", loopRate);
|
||||||
|
sendMessage(&msg);
|
||||||
|
|
||||||
|
// Send vector named value
|
||||||
|
mavlink_msg_debug_vect_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "some_vector", t, gyroBias.x, gyroBias.y, gyroBias.z);
|
||||||
|
sendMessage(&msg);
|
||||||
|
```
|
||||||
|
|
||||||
|
### Logging
|
||||||
|
|
||||||
|
You can control Flix library verbosity using Python's `logging` module:
|
||||||
|
|
||||||
|
```python
|
||||||
|
import logging
|
||||||
|
|
||||||
|
logger = logging.getLogger('flix')
|
||||||
|
logger.setLevel(logging.DEBUG) # be more verbose
|
||||||
|
logger.setLevel(logging.WARNING) # be less verbose
|
||||||
|
```
|
||||||
|
|
||||||
|
## Stability
|
||||||
|
|
||||||
|
The library is in development stage. The API is not stable.
|
1
tools/pyflix/__init__.py
Normal file
1
tools/pyflix/__init__.py
Normal file
@ -0,0 +1 @@
|
|||||||
|
from .flix import Flix
|
324
tools/pyflix/flix.py
Normal file
324
tools/pyflix/flix.py
Normal file
@ -0,0 +1,324 @@
|
|||||||
|
# Copyright (c) 2025 Oleg Kalachev <okalachev@gmail.com>
|
||||||
|
# Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
|
"""Python API for Flix drone."""
|
||||||
|
|
||||||
|
import os
|
||||||
|
import time
|
||||||
|
from queue import Queue, Empty
|
||||||
|
from typing import Literal, Optional, Callable, List, Dict, Any, Union
|
||||||
|
import logging
|
||||||
|
import errno
|
||||||
|
from threading import Thread, Timer
|
||||||
|
from pymavlink import mavutil
|
||||||
|
from pymavlink.quaternion import Quaternion
|
||||||
|
from pymavlink.dialects.v20 import common as mavlink
|
||||||
|
|
||||||
|
logger = logging.getLogger('flix')
|
||||||
|
if not logger.hasHandlers():
|
||||||
|
handler = logging.StreamHandler()
|
||||||
|
handler.setFormatter(logging.Formatter('%(name)s - %(levelname)s - %(message)s'))
|
||||||
|
logger.addHandler(handler)
|
||||||
|
logger.setLevel(logging.INFO)
|
||||||
|
|
||||||
|
class Flix:
|
||||||
|
connected: bool = False
|
||||||
|
mode: str = ''
|
||||||
|
armed: bool = False
|
||||||
|
landed: bool = False
|
||||||
|
attitude: List[float]
|
||||||
|
attitude_euler: List[float] # roll, pitch, yaw
|
||||||
|
rates: List[float]
|
||||||
|
channels: List[int]
|
||||||
|
motors: List[float]
|
||||||
|
acc: List[float]
|
||||||
|
gyro: List[float]
|
||||||
|
|
||||||
|
mav_id: int
|
||||||
|
messages: Dict[str, Dict[str, Any]] # MAVLink messages storage
|
||||||
|
values: Dict[Union[str, int], Union[float, List[float]]] = {} # named values
|
||||||
|
|
||||||
|
_connection_timeout = 3
|
||||||
|
_print_buffer: str = ''
|
||||||
|
|
||||||
|
def __init__(self, mav_id: int=1, wait_connection: bool=True):
|
||||||
|
if not (0 <= mav_id < 256):
|
||||||
|
raise ValueError('mav_id must be in range [0, 255]')
|
||||||
|
self._setup_mavlink()
|
||||||
|
self.mav_id = mav_id
|
||||||
|
self._init_state()
|
||||||
|
try:
|
||||||
|
# Direct connection
|
||||||
|
logger.debug('Listening on port 14550')
|
||||||
|
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14550', source_system=255) # type: ignore
|
||||||
|
except OSError as e:
|
||||||
|
if e.errno != errno.EADDRINUSE:
|
||||||
|
raise
|
||||||
|
# Port busy - using proxy
|
||||||
|
logger.debug('Listening on port 14560 (proxy)')
|
||||||
|
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254) # type: ignore
|
||||||
|
self.connection.target_system = mav_id
|
||||||
|
self.mavlink: mavlink.MAVLink = self.connection.mav
|
||||||
|
self._event_listeners: Dict[str, List[Callable[..., Any]]] = {}
|
||||||
|
self.messages = {}
|
||||||
|
self._disconnected_timer = Timer(0, self._disconnected)
|
||||||
|
self._reader_thread = Thread(target=self._read_mavlink, daemon=True)
|
||||||
|
self._reader_thread.start()
|
||||||
|
self._heartbeat_thread = Thread(target=self._send_heartbeat, daemon=True)
|
||||||
|
self._heartbeat_thread.start()
|
||||||
|
if wait_connection:
|
||||||
|
self.wait('mavlink.HEARTBEAT')
|
||||||
|
time.sleep(0.2) # give some time to receive initial state
|
||||||
|
|
||||||
|
def _init_state(self):
|
||||||
|
self.attitude = [1, 0, 0, 0]
|
||||||
|
self.attitude_euler = [0, 0, 0]
|
||||||
|
self.rates = [0, 0, 0]
|
||||||
|
self.channels = [0, 0, 0, 0, 0, 0, 0, 0]
|
||||||
|
self.motors = [0, 0, 0, 0]
|
||||||
|
self.acc = [0, 0, 0]
|
||||||
|
self.gyro = [0, 0, 0]
|
||||||
|
|
||||||
|
def on(self, event: str, callback: Callable):
|
||||||
|
event = event.lower()
|
||||||
|
if event not in self._event_listeners:
|
||||||
|
self._event_listeners[event] = []
|
||||||
|
self._event_listeners[event].append(callback)
|
||||||
|
|
||||||
|
def off(self, callback: Callable):
|
||||||
|
for event in self._event_listeners:
|
||||||
|
if callback in self._event_listeners[event]:
|
||||||
|
self._event_listeners[event].remove(callback)
|
||||||
|
|
||||||
|
def _trigger(self, event: str, *args):
|
||||||
|
event = event.lower()
|
||||||
|
for callback in self._event_listeners.get(event, []):
|
||||||
|
try:
|
||||||
|
callback(*args)
|
||||||
|
except Exception as e:
|
||||||
|
logger.error(f'Error in event listener for event {event}: {e}')
|
||||||
|
|
||||||
|
def wait(self, event: str, value: Union[Any, Callable[..., bool]] = lambda *args: True, timeout=None) -> Any:
|
||||||
|
"""Wait for an event"""
|
||||||
|
event = event.lower()
|
||||||
|
q = Queue()
|
||||||
|
def callback(*args):
|
||||||
|
if len(args) == 0:
|
||||||
|
result = None
|
||||||
|
elif len(args) == 1:
|
||||||
|
result = args[0]
|
||||||
|
else:
|
||||||
|
result = args
|
||||||
|
if callable(value) and value(*args):
|
||||||
|
q.put_nowait(result)
|
||||||
|
elif value == result:
|
||||||
|
q.put_nowait(result)
|
||||||
|
self.on(event, callback)
|
||||||
|
try:
|
||||||
|
return q.get(timeout=timeout)
|
||||||
|
except Empty:
|
||||||
|
raise TimeoutError
|
||||||
|
finally:
|
||||||
|
self.off(callback)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _setup_mavlink():
|
||||||
|
# otherwise it will use MAVLink 1.0 until connected
|
||||||
|
os.environ['MAVLINK20'] = '1'
|
||||||
|
mavutil.set_dialect('common')
|
||||||
|
|
||||||
|
def _read_mavlink(self):
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
msg: Optional[mavlink.MAVLink_message] = self.connection.recv_match(blocking=True)
|
||||||
|
if msg is None:
|
||||||
|
continue
|
||||||
|
self._connected()
|
||||||
|
msg_dict = msg.to_dict()
|
||||||
|
msg_dict['_timestamp'] = time.time() # add timestamp
|
||||||
|
self.messages[msg.get_type()] = msg_dict
|
||||||
|
self._trigger('mavlink', msg)
|
||||||
|
self._trigger(f'mavlink.{msg.get_type()}', msg) # trigger mavlink.<message_type>
|
||||||
|
self._trigger(f'mavlink.{msg.get_msgId()}', msg) # trigger mavlink.<message_id>
|
||||||
|
self._handle_mavlink_message(msg)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
logger.error(f'Error reading MAVLink message: {e}')
|
||||||
|
|
||||||
|
def _handle_mavlink_message(self, msg: mavlink.MAVLink_message):
|
||||||
|
if isinstance(msg, mavlink.MAVLink_heartbeat_message):
|
||||||
|
self.mode = ['MANUAL', 'ACRO', 'STAB', 'USER'][msg.custom_mode]
|
||||||
|
self.armed = msg.base_mode & mavlink.MAV_MODE_FLAG_SAFETY_ARMED != 0
|
||||||
|
self._trigger('mode', self.mode)
|
||||||
|
self._trigger('armed', self.armed)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_extended_sys_state_message):
|
||||||
|
self.landed = msg.landed_state == mavlink.MAV_LANDED_STATE_ON_GROUND
|
||||||
|
self._trigger('landed', self.landed)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_attitude_quaternion_message):
|
||||||
|
self.attitude = self._mavlink_to_flu([msg.q1, msg.q2, msg.q3, msg.q4])
|
||||||
|
self.rates = self._mavlink_to_flu([msg.rollspeed, msg.pitchspeed, msg.yawspeed])
|
||||||
|
self.attitude_euler = list(Quaternion(self.attitude).euler) # type: ignore
|
||||||
|
self._trigger('attitude', self.attitude)
|
||||||
|
self._trigger('attitude_euler', self.attitude_euler)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_rc_channels_raw_message):
|
||||||
|
self.channels = [msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw,
|
||||||
|
msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]
|
||||||
|
self._trigger('channels', self.channels)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_actuator_output_status_message):
|
||||||
|
self.motors = msg.actuator[:4] # type: ignore
|
||||||
|
self._trigger('motors', self.motors)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
|
||||||
|
self.acc = self._mavlink_to_flu([msg.xacc / 1000, msg.yacc / 1000, msg.zacc / 1000])
|
||||||
|
self.gyro = self._mavlink_to_flu([msg.xgyro / 1000, msg.ygyro / 1000, msg.zgyro / 1000])
|
||||||
|
self._trigger('acc', self.acc)
|
||||||
|
self._trigger('gyro', self.gyro)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_serial_control_message):
|
||||||
|
# new chunk of data
|
||||||
|
text = bytes(msg.data)[:msg.count].decode('utf-8', errors='ignore')
|
||||||
|
logger.debug(f'Console: {repr(text)}')
|
||||||
|
self._trigger('print', text)
|
||||||
|
self._print_buffer += text
|
||||||
|
if msg.flags & mavlink.SERIAL_CONTROL_FLAG_MULTI == 0:
|
||||||
|
# last chunk
|
||||||
|
self._trigger('print_full', self._print_buffer)
|
||||||
|
self._print_buffer = ''
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_statustext_message):
|
||||||
|
logger.info(f'Flix #{msg.get_srcSystem()}: {msg.text}')
|
||||||
|
self._trigger('status', msg.text)
|
||||||
|
|
||||||
|
if isinstance(msg, (mavlink.MAVLink_named_value_float_message, mavlink.MAVLink_named_value_int_message)):
|
||||||
|
self.values[msg.name] = msg.value
|
||||||
|
self._trigger('value', msg.name, msg.value)
|
||||||
|
self._trigger(f'value.{msg.name}', msg.value)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_debug_message):
|
||||||
|
self.values[msg.ind] = msg.value
|
||||||
|
self._trigger('value', msg.ind, msg.value)
|
||||||
|
self._trigger(f'value.{msg.ind}', msg.value)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_debug_vect_message):
|
||||||
|
self.values[msg.name] = [msg.x, msg.y, msg.z]
|
||||||
|
self._trigger('value', msg.name, self.values[msg.name])
|
||||||
|
self._trigger(f'value.{msg.name}', self.values[msg.name])
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_debug_float_array_message):
|
||||||
|
self.values[msg.name] = list(msg.data)
|
||||||
|
self._trigger('value', msg.name, self.values[msg.name])
|
||||||
|
self._trigger(f'value.{msg.name}', self.values[msg.name])
|
||||||
|
|
||||||
|
def _send_heartbeat(self):
|
||||||
|
while True:
|
||||||
|
self.mavlink.heartbeat_send(mavlink.MAV_TYPE_GCS, mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
|
||||||
|
time.sleep(1)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _mavlink_to_flu(v: List[float]) -> List[float]:
|
||||||
|
if len(v) == 3: # vector
|
||||||
|
return [v[0], -v[1], -v[2]]
|
||||||
|
elif len(v) == 4: # quaternion
|
||||||
|
return [v[0], v[1], -v[2], -v[3]]
|
||||||
|
else:
|
||||||
|
raise ValueError(f'List must have 3 (vector) or 4 (quaternion) elements')
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _flu_to_mavlink(v: List[float]) -> List[float]:
|
||||||
|
return Flix._mavlink_to_flu(v)
|
||||||
|
|
||||||
|
def _connected(self):
|
||||||
|
# Reset disconnection timer
|
||||||
|
self._disconnected_timer.cancel()
|
||||||
|
self._disconnected_timer = Timer(self._connection_timeout, self._disconnected)
|
||||||
|
self._disconnected_timer.start()
|
||||||
|
|
||||||
|
if not self.connected:
|
||||||
|
logger.info('Connection is established')
|
||||||
|
self.connected = True
|
||||||
|
self._trigger('connected')
|
||||||
|
|
||||||
|
def _disconnected(self):
|
||||||
|
logger.info('Connection is lost')
|
||||||
|
self.connected = False
|
||||||
|
self._trigger('disconnected')
|
||||||
|
|
||||||
|
def get_param(self, name: str) -> float:
|
||||||
|
if len(name.encode('ascii')) > 16:
|
||||||
|
raise ValueError('Parameter name must be 16 characters or less')
|
||||||
|
for attempt in range(3):
|
||||||
|
try:
|
||||||
|
logger.debug(f'Get param {name} (attempt #{attempt + 1})')
|
||||||
|
self.mavlink.param_request_read_send(self.mav_id, 0, name.encode('ascii'), -1)
|
||||||
|
msg: mavlink.MAVLink_param_value_message = \
|
||||||
|
self.wait('mavlink.PARAM_VALUE', value=lambda msg: msg.param_id == name, timeout=0.1)
|
||||||
|
return msg.param_value
|
||||||
|
except TimeoutError:
|
||||||
|
continue
|
||||||
|
raise RuntimeError(f'Failed to get parameter {name} after 3 attempts')
|
||||||
|
|
||||||
|
def set_param(self, name: str, value: float):
|
||||||
|
if len(name.encode('ascii')) > 16:
|
||||||
|
raise ValueError('Parameter name must be 16 characters or less')
|
||||||
|
for attempt in range(3):
|
||||||
|
try:
|
||||||
|
logger.debug(f'Set param {name} to {value} (attempt #{attempt + 1})')
|
||||||
|
self.mavlink.param_set_send(self.mav_id, 0, name.encode('ascii'), value, mavlink.MAV_PARAM_TYPE_REAL32)
|
||||||
|
self.wait('mavlink.PARAM_VALUE', value=lambda msg: msg.param_id == name, timeout=0.1)
|
||||||
|
return
|
||||||
|
except TimeoutError:
|
||||||
|
# on timeout try again
|
||||||
|
continue
|
||||||
|
raise RuntimeError(f'Failed to set parameter {name} to {value} after 3 attempts')
|
||||||
|
|
||||||
|
def set_position(self, position: List[float], yaw: Optional[float] = None, wait: bool = False, tolerance: float = 0.1):
|
||||||
|
raise NotImplementedError('Position control is not implemented yet')
|
||||||
|
|
||||||
|
def set_attitude(self, attitude: List[float], thrust: float):
|
||||||
|
raise NotImplementedError('Automatic flight is not implemented yet')
|
||||||
|
|
||||||
|
def set_rates(self, rates: List[float], thrust: float):
|
||||||
|
raise NotImplementedError('Automatic flight is not implemented yet')
|
||||||
|
|
||||||
|
def set_motors(self, motors: List[float]):
|
||||||
|
if len(motors) != 4:
|
||||||
|
raise ValueError('motors must have 4 values')
|
||||||
|
if not all(0 <= m <= 1 for m in motors):
|
||||||
|
raise ValueError('motors must be in range [0, 1]')
|
||||||
|
raise NotImplementedError
|
||||||
|
|
||||||
|
def set_controls(self, roll: float, pitch: float, yaw: float, throttle: float):
|
||||||
|
"""Send pilot's controls. Warning: not intended for automatic control"""
|
||||||
|
if not (-1 <= roll <= 1 and -1 <= pitch <= 1 and -1 <= yaw <= 1):
|
||||||
|
raise ValueError('roll, pitch, yaw must be in range [-1, 1]')
|
||||||
|
if not 0 <= throttle <= 1:
|
||||||
|
raise ValueError('throttle must be in range [0, 1]')
|
||||||
|
self.mavlink.manual_control_send(self.mav_id, roll * 1000, pitch * 1000, yaw * 1000, throttle * 1000, 0) # type: ignore
|
||||||
|
|
||||||
|
def set_mode(self, mode: Literal['MANUAL', 'ACRO', 'STAB', 'USER']):
|
||||||
|
raise NotImplementedError('Setting mode is not implemented yet')
|
||||||
|
|
||||||
|
def cli(self, cmd: str, wait_response: bool = True) -> str:
|
||||||
|
cmd = cmd.strip()
|
||||||
|
if cmd == 'reboot':
|
||||||
|
wait_response = False # reboot command doesn't respond
|
||||||
|
cmd_bytes = (cmd + '\n').encode('utf-8')
|
||||||
|
if len(cmd_bytes) > 70:
|
||||||
|
raise ValueError(f'Command is too long: {len(cmd_bytes)} > 70')
|
||||||
|
cmd_bytes = cmd_bytes.ljust(70, b'\0')
|
||||||
|
response_prefix = f'> {cmd}\n'
|
||||||
|
for attempt in range(3):
|
||||||
|
logger.debug(f'Send command {cmd} (attempt #{attempt + 1})')
|
||||||
|
try:
|
||||||
|
self.mavlink.serial_control_send(0, 0, 0, 0, len(cmd_bytes), cmd_bytes)
|
||||||
|
if not wait_response:
|
||||||
|
return ''
|
||||||
|
response = self.wait('print_full', timeout=0.1, value=lambda text: text.startswith(response_prefix))
|
||||||
|
return response[len(response_prefix):].strip()
|
||||||
|
except TimeoutError:
|
||||||
|
continue
|
||||||
|
raise RuntimeError(f'Failed to send command {cmd} after 3 attempts')
|
37
tools/pyflix/proxy.py
Executable file
37
tools/pyflix/proxy.py
Executable file
@ -0,0 +1,37 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
"""Proxy for running pyflix library alongside QGroundControl app."""
|
||||||
|
|
||||||
|
import socket
|
||||||
|
|
||||||
|
LOCAL = ('0.0.0.0', 14550) # from Flix
|
||||||
|
TARGETS = (
|
||||||
|
('127.0.0.1', 14560), # to QGroundControl
|
||||||
|
('127.0.0.1', 14555), # to pyflix
|
||||||
|
)
|
||||||
|
|
||||||
|
def main():
|
||||||
|
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
sock.bind(LOCAL)
|
||||||
|
|
||||||
|
source_addr = None
|
||||||
|
packets = 0
|
||||||
|
|
||||||
|
print('Proxy started - run QGroundControl')
|
||||||
|
|
||||||
|
while True:
|
||||||
|
data, addr = sock.recvfrom(1024) # read entire UDP packet
|
||||||
|
if addr in TARGETS: # packet from target
|
||||||
|
if source_addr is None:
|
||||||
|
continue
|
||||||
|
sock.sendto(data, source_addr)
|
||||||
|
else: # packet from source
|
||||||
|
source_addr = addr
|
||||||
|
for target in TARGETS:
|
||||||
|
sock.sendto(data, target)
|
||||||
|
|
||||||
|
packets += 1
|
||||||
|
print(f'\rPackets: {packets}', end='')
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
28
tools/pyproject.toml
Normal file
28
tools/pyproject.toml
Normal file
@ -0,0 +1,28 @@
|
|||||||
|
[project]
|
||||||
|
name = "pyflix"
|
||||||
|
version = "0.5"
|
||||||
|
description = "Python API for Flix drone"
|
||||||
|
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
||||||
|
license = "MIT"
|
||||||
|
readme = "pyflix/README.md"
|
||||||
|
requires-python = ">=3.8"
|
||||||
|
dependencies = [
|
||||||
|
"pymavlink",
|
||||||
|
]
|
||||||
|
|
||||||
|
[project.scripts]
|
||||||
|
flix-proxy = "pyflix.proxy:main"
|
||||||
|
|
||||||
|
[build-system]
|
||||||
|
requires = ["setuptools>=61.0"]
|
||||||
|
build-backend = "setuptools.build_meta"
|
||||||
|
|
||||||
|
[tool.setuptools]
|
||||||
|
packages = ["pyflix"]
|
||||||
|
|
||||||
|
[tool.setuptools.package-data]
|
||||||
|
pyflix = ["README.md"]
|
||||||
|
|
||||||
|
[project.urls]
|
||||||
|
Homepage = "https://github.com/okalachev/flix/tree/master/tools/pyflix"
|
||||||
|
Issues = "https://github.com/okalachev/flix/issues"
|
Loading…
x
Reference in New Issue
Block a user