Compare commits
94 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 64b21a3a6b | |||
| 545eed8944 | |||
| 518abf1555 | |||
| 17df1c5396 | |||
| 8e2ffd7c69 | |||
| 52b74afba6 | |||
| 0ca2473655 | |||
| b4c2fe3988 | |||
| e51b47b798 | |||
| 71abe1bcdb | |||
| 0f2e384ce6 | |||
| 5e153a210d | |||
| 9d47bcb82e | |||
| 1fafc27b39 | |||
| faca48ced3 | |||
| a5dbd2c829 | |||
| 59f9528d34 | |||
| 607b2ff0b7 | |||
| 22c06f76c4 | |||
| 488ceb3004 | |||
| b83c9b3845 | |||
| 2f4b1423e6 | |||
| 4e32414dae | |||
| a294883dea | |||
| cdfba72a0b | |||
| 18e81720e0 | |||
| 91173d06c9 | |||
| fdcc9533b3 | |||
| bd2b1bd5de | |||
| 4530c05b5c | |||
| 3816ae376f | |||
| 72a72fde80 | |||
| e53051a349 | |||
| f8a9f1f838 | |||
| 76af83fc88 | |||
| dd176180a7 | |||
| 48c33c7050 | |||
| 35e94f6ea6 | |||
| 1f48e379e3 | |||
| ee3c6999ab | |||
| 34c6993842 | |||
| b62f2f9427 | |||
| 7dfef17165 | |||
| 8c8046676b | |||
| 702ec9792e | |||
| 06e2047097 | |||
| 87480476c2 | |||
| 68271c508c | |||
| e81e84e7fc | |||
| 5f1a938d4f | |||
| bd270db493 | |||
| dbf24ea611 | |||
| 08683d696d | |||
| 9ca6841558 | |||
| 28da2d3c8e | |||
| c6632ae6e4 | |||
| 35ca754583 | |||
| 2ccda03573 | |||
| 485a39e740 | |||
| 9bffe5b52f | |||
| d6a79d6c66 | |||
| 350a82bfed | |||
| 6e439859bc | |||
| 835b2243e8 | |||
| ed4e2d87d1 | |||
| 51cd5fc691 | |||
| d8591ea2a9 | |||
| c434107eaf | |||
| 814427dbfd | |||
| 0730ceeffa | |||
| a687303062 | |||
| b2daf2587f | |||
| a8c25d8ac0 | |||
| 3e49d41986 | |||
| 67430c7aac | |||
| 3631743a29 | |||
| 3dde380bb7 | |||
| 377b21429b | |||
| 1ac443d6f8 | |||
| 964c0f7bc1 | |||
| 40bdaacedb | |||
| 7d74f3d5cd | |||
| 9fd35ba361 | |||
| ca50f75576 | |||
| e47a31f981 | |||
| 7ad3022798 | |||
| 5b654e4d8e | |||
| cf10ec6161 | |||
| 6d01cd2e79 | |||
| 0abb18c616 | |||
| 30326a5662 | |||
| dd3575174b | |||
| c0f3301da4 | |||
| a6bad3a69b |
@@ -23,10 +23,10 @@ jobs:
|
|||||||
with:
|
with:
|
||||||
name: firmware-binary
|
name: firmware-binary
|
||||||
path: flix/build
|
path: flix/build
|
||||||
|
- name: Build firmware for ESP32-C3
|
||||||
|
run: make BOARD=esp32:esp32:esp32c3
|
||||||
- name: Build firmware for ESP32-S3
|
- name: Build firmware for ESP32-S3
|
||||||
run: make BOARD=esp32:esp32:esp32s3
|
run: make BOARD=esp32:esp32:esp32s3
|
||||||
- name: Build firmware with WiFi disabled
|
|
||||||
run: sed -i 's/^#define WIFI_ENABLED 1$/#define WIFI_ENABLED 0/' flix/flix.ino && make
|
|
||||||
- name: Check c_cpp_properties.json
|
- name: Check c_cpp_properties.json
|
||||||
run: tools/check_c_cpp_properties.py
|
run: tools/check_c_cpp_properties.py
|
||||||
|
|
||||||
|
|||||||
@@ -4,9 +4,10 @@ build/
|
|||||||
tools/log/
|
tools/log/
|
||||||
tools/dist/
|
tools/dist/
|
||||||
*.egg-info/
|
*.egg-info/
|
||||||
.dependencies
|
.core
|
||||||
|
.libs
|
||||||
.vscode/*
|
.vscode/*
|
||||||
!.vscode/settings.json
|
!.vscode/settings.default.json
|
||||||
!.vscode/c_cpp_properties.json
|
!.vscode/c_cpp_properties.json
|
||||||
!.vscode/tasks.json
|
!.vscode/tasks.json
|
||||||
!.vscode/launch.json
|
!.vscode/launch.json
|
||||||
|
|||||||
@@ -7,6 +7,7 @@
|
|||||||
"MD024": false,
|
"MD024": false,
|
||||||
"MD033": false,
|
"MD033": false,
|
||||||
"MD034": false,
|
"MD034": false,
|
||||||
|
"MD040": false,
|
||||||
"MD059": false,
|
"MD059": false,
|
||||||
"MD044": {
|
"MD044": {
|
||||||
"html_elements": false,
|
"html_elements": false,
|
||||||
|
|||||||
@@ -6,19 +6,18 @@
|
|||||||
"${workspaceFolder}/flix",
|
"${workspaceFolder}/flix",
|
||||||
"${workspaceFolder}/gazebo",
|
"${workspaceFolder}/gazebo",
|
||||||
"${workspaceFolder}/tools/**",
|
"${workspaceFolder}/tools/**",
|
||||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
|
||||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
|
||||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
|
||||||
"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
|
"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
|
||||||
"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
|
||||||
"~/Arduino/libraries/**",
|
"~/Arduino/libraries/**",
|
||||||
"/usr/include/gazebo-11/",
|
"/usr/include/gazebo-11/",
|
||||||
"/usr/include/ignition/math6/"
|
"/usr/include/ignition/math6/"
|
||||||
],
|
],
|
||||||
"forcedInclude": [
|
"forcedInclude": [
|
||||||
"${workspaceFolder}/.vscode/intellisense.h",
|
"${workspaceFolder}/.vscode/intellisense.h",
|
||||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h",
|
||||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h",
|
||||||
"${workspaceFolder}/flix/cli.ino",
|
"${workspaceFolder}/flix/cli.ino",
|
||||||
"${workspaceFolder}/flix/control.ino",
|
"${workspaceFolder}/flix/control.ino",
|
||||||
"${workspaceFolder}/flix/estimate.ino",
|
"${workspaceFolder}/flix/estimate.ino",
|
||||||
@@ -31,9 +30,10 @@
|
|||||||
"${workspaceFolder}/flix/rc.ino",
|
"${workspaceFolder}/flix/rc.ino",
|
||||||
"${workspaceFolder}/flix/time.ino",
|
"${workspaceFolder}/flix/time.ino",
|
||||||
"${workspaceFolder}/flix/wifi.ino",
|
"${workspaceFolder}/flix/wifi.ino",
|
||||||
"${workspaceFolder}/flix/parameters.ino"
|
"${workspaceFolder}/flix/parameters.ino",
|
||||||
|
"${workspaceFolder}/flix/safety.ino"
|
||||||
],
|
],
|
||||||
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2601/bin/xtensa-esp32-elf-g++",
|
||||||
"cStandard": "c11",
|
"cStandard": "c11",
|
||||||
"cppStandard": "c++17",
|
"cppStandard": "c++17",
|
||||||
"defines": [
|
"defines": [
|
||||||
@@ -53,19 +53,18 @@
|
|||||||
"name": "Mac",
|
"name": "Mac",
|
||||||
"includePath": [
|
"includePath": [
|
||||||
"${workspaceFolder}/flix",
|
"${workspaceFolder}/flix",
|
||||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
|
||||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
|
||||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
|
||||||
"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/include/**",
|
"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
|
||||||
"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
|
||||||
"~/Documents/Arduino/libraries/**",
|
"~/Documents/Arduino/libraries/**",
|
||||||
"/opt/homebrew/include/gazebo-11/",
|
"/opt/homebrew/include/gazebo-11/",
|
||||||
"/opt/homebrew/include/ignition/math6/"
|
"/opt/homebrew/include/ignition/math6/"
|
||||||
],
|
],
|
||||||
"forcedInclude": [
|
"forcedInclude": [
|
||||||
"${workspaceFolder}/.vscode/intellisense.h",
|
"${workspaceFolder}/.vscode/intellisense.h",
|
||||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h",
|
||||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h",
|
||||||
"${workspaceFolder}/flix/flix.ino",
|
"${workspaceFolder}/flix/flix.ino",
|
||||||
"${workspaceFolder}/flix/cli.ino",
|
"${workspaceFolder}/flix/cli.ino",
|
||||||
"${workspaceFolder}/flix/control.ino",
|
"${workspaceFolder}/flix/control.ino",
|
||||||
@@ -78,9 +77,10 @@
|
|||||||
"${workspaceFolder}/flix/rc.ino",
|
"${workspaceFolder}/flix/rc.ino",
|
||||||
"${workspaceFolder}/flix/time.ino",
|
"${workspaceFolder}/flix/time.ino",
|
||||||
"${workspaceFolder}/flix/wifi.ino",
|
"${workspaceFolder}/flix/wifi.ino",
|
||||||
"${workspaceFolder}/flix/parameters.ino"
|
"${workspaceFolder}/flix/parameters.ino",
|
||||||
|
"${workspaceFolder}/flix/safety.ino"
|
||||||
],
|
],
|
||||||
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2601/bin/xtensa-esp32-elf-g++",
|
||||||
"cStandard": "c11",
|
"cStandard": "c11",
|
||||||
"cppStandard": "c++17",
|
"cppStandard": "c++17",
|
||||||
"defines": [
|
"defines": [
|
||||||
@@ -103,17 +103,16 @@
|
|||||||
"${workspaceFolder}/flix",
|
"${workspaceFolder}/flix",
|
||||||
"${workspaceFolder}/gazebo",
|
"${workspaceFolder}/gazebo",
|
||||||
"${workspaceFolder}/tools/**",
|
"${workspaceFolder}/tools/**",
|
||||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
|
||||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
|
||||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
|
||||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
|
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
|
||||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
|
||||||
"~/Documents/Arduino/libraries/**"
|
"~/Documents/Arduino/libraries/**"
|
||||||
],
|
],
|
||||||
"forcedInclude": [
|
"forcedInclude": [
|
||||||
"${workspaceFolder}/.vscode/intellisense.h",
|
"${workspaceFolder}/.vscode/intellisense.h",
|
||||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h",
|
||||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h",
|
||||||
"${workspaceFolder}/flix/cli.ino",
|
"${workspaceFolder}/flix/cli.ino",
|
||||||
"${workspaceFolder}/flix/control.ino",
|
"${workspaceFolder}/flix/control.ino",
|
||||||
"${workspaceFolder}/flix/estimate.ino",
|
"${workspaceFolder}/flix/estimate.ino",
|
||||||
@@ -126,9 +125,10 @@
|
|||||||
"${workspaceFolder}/flix/rc.ino",
|
"${workspaceFolder}/flix/rc.ino",
|
||||||
"${workspaceFolder}/flix/time.ino",
|
"${workspaceFolder}/flix/time.ino",
|
||||||
"${workspaceFolder}/flix/wifi.ino",
|
"${workspaceFolder}/flix/wifi.ino",
|
||||||
"${workspaceFolder}/flix/parameters.ino"
|
"${workspaceFolder}/flix/parameters.ino",
|
||||||
|
"${workspaceFolder}/flix/safety.ino"
|
||||||
],
|
],
|
||||||
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
|
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2601/bin/xtensa-esp32-elf-g++.exe",
|
||||||
"cStandard": "c11",
|
"cStandard": "c11",
|
||||||
"cppStandard": "c++17",
|
"cppStandard": "c++17",
|
||||||
"defines": [
|
"defines": [
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
{
|
{
|
||||||
// See https://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
|
// See https://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
|
||||||
"recommendations": [
|
"recommendations": [
|
||||||
|
"dangmai.workspace-default-settings",
|
||||||
"ms-vscode.cpptools",
|
"ms-vscode.cpptools",
|
||||||
"ms-vscode.cmake-tools",
|
"ms-vscode.cmake-tools",
|
||||||
"ms-python.python"
|
"ms-python.python"
|
||||||
|
|||||||
@@ -1,29 +1,40 @@
|
|||||||
BOARD = esp32:esp32:d1_mini32
|
BOARD = esp32:esp32:d1_mini32:DebugLevel=error
|
||||||
PORT := $(wildcard /dev/serial/by-id/usb-Silicon_Labs_CP21* /dev/serial/by-id/usb-1a86_USB_Single_Serial_* /dev/cu.usbserial-*)
|
PORT := $(strip $(wildcard /dev/serial/by-id/usb-Silicon_Labs_CP21* /dev/serial/by-id/usb-1a86_USB_Single_Serial_* /dev/cu.usbserial-* /dev/cu.usbmodem*))
|
||||||
PORT := $(strip $(PORT))
|
|
||||||
|
|
||||||
build: .dependencies
|
export ARDUINO_NETWORK_CONNECTION_TIMEOUT := 1h
|
||||||
|
|
||||||
|
build: .core .libs
|
||||||
arduino-cli compile --fqbn $(BOARD) flix
|
arduino-cli compile --fqbn $(BOARD) flix
|
||||||
|
|
||||||
upload: build
|
upload: build
|
||||||
arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" flix
|
arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" flix
|
||||||
|
|
||||||
|
erase:
|
||||||
|
arduino-cli burn-bootloader --fqbn $(BOARD) -p "$(PORT)" -P esptool
|
||||||
|
|
||||||
monitor:
|
monitor:
|
||||||
arduino-cli monitor -p "$(PORT)" -c baudrate=115200
|
arduino-cli monitor -p "$(PORT)" -c baudrate=115200
|
||||||
|
|
||||||
dependencies .dependencies:
|
core .core:
|
||||||
arduino-cli core update-index --config-file arduino-cli.yaml
|
arduino-cli core update-index --additional-urls https://espressif.github.io/arduino-esp32/package_esp32_index.json
|
||||||
arduino-cli core install esp32:esp32@3.2.0 --config-file arduino-cli.yaml
|
arduino-cli core install esp32:esp32@3.3.10 --additional-urls https://espressif.github.io/arduino-esp32/package_esp32_index.json
|
||||||
|
touch .core
|
||||||
|
|
||||||
|
libs .libs:
|
||||||
arduino-cli lib update-index
|
arduino-cli lib update-index
|
||||||
arduino-cli lib install "FlixPeriph"
|
arduino-cli lib install "FlixPeriph"
|
||||||
arduino-cli lib install "MAVLink"@2.0.16
|
arduino-cli lib install "MAVLink"@2.0.25
|
||||||
touch .dependencies
|
touch .libs
|
||||||
|
|
||||||
|
upload_proxy: .core .libs
|
||||||
|
arduino-cli compile --fqbn $(BOARD) tools/espnow-proxy
|
||||||
|
arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" tools/espnow-proxy
|
||||||
|
|
||||||
gazebo/build cmake: gazebo/CMakeLists.txt
|
gazebo/build cmake: gazebo/CMakeLists.txt
|
||||||
mkdir -p gazebo/build
|
mkdir -p gazebo/build
|
||||||
cd gazebo/build && cmake ..
|
cd gazebo/build && cmake ..
|
||||||
|
|
||||||
build_simulator: .dependencies gazebo/build
|
build_simulator: .libs gazebo/build
|
||||||
make -C gazebo/build
|
make -C gazebo/build
|
||||||
|
|
||||||
simulator: build_simulator
|
simulator: build_simulator
|
||||||
@@ -38,6 +49,6 @@ plot:
|
|||||||
plotjuggler -d $(shell ls -t tools/log/*.csv | head -n1)
|
plotjuggler -d $(shell ls -t tools/log/*.csv | head -n1)
|
||||||
|
|
||||||
clean:
|
clean:
|
||||||
rm -rf gazebo/build flix/build flix/cache .dependencies
|
rm -rf gazebo/build flix/build flix/cache .core .libs
|
||||||
|
|
||||||
.PHONY: build upload monitor dependencies cmake build_simulator simulator log clean
|
.PHONY: build upload monitor core libs cmake build_simulator simulator log clean
|
||||||
|
|||||||
@@ -21,8 +21,8 @@
|
|||||||
* Dedicated for education and research.
|
* Dedicated for education and research.
|
||||||
* Made from general-purpose components.
|
* Made from general-purpose components.
|
||||||
* Simple and clean source code in Arduino (<2k lines firmware).
|
* Simple and clean source code in Arduino (<2k lines firmware).
|
||||||
* Connectivity using Wi-Fi and MAVLink protocol.
|
* Communication using MAVLink protocol over Wi-Fi or ESP-NOW.
|
||||||
* Control using USB gamepad, remote control or smartphone.
|
* Control with USB gamepad, remote control or smartphone.
|
||||||
* Wireless command line interface and analyzing.
|
* Wireless command line interface and analyzing.
|
||||||
* Precise simulation with Gazebo.
|
* Precise simulation with Gazebo.
|
||||||
* Python library for scripting and automatic flights.
|
* Python library for scripting and automatic flights.
|
||||||
@@ -47,13 +47,21 @@ See the [user builds gallery](docs/user.md):
|
|||||||
|
|
||||||
<a href="docs/user.md"><img src="docs/img/user/user.jpg" width=500></a>
|
<a href="docs/user.md"><img src="docs/img/user/user.jpg" width=500></a>
|
||||||
|
|
||||||
|
### PCB
|
||||||
|
|
||||||
|
The official PCB *(Flix2)* is in development now. Follow the [project's channel](https://t.me/opensourcequadcopter) to track the progress.
|
||||||
|
|
||||||
|
Outdoor flights demo video of the current prototype:
|
||||||
|
|
||||||
|
<a href="https://youtu.be/KXlNmvUTi4g"><img width=300 src="https://i3.ytimg.com/vi/KXlNmvUTi4g/maxresdefault.jpg"></a>
|
||||||
|
|
||||||
## Simulation
|
## Simulation
|
||||||
|
|
||||||
The simulator is implemented using Gazebo and runs the original Arduino code:
|
The simulator is implemented using Gazebo and runs the original Arduino code:
|
||||||
|
|
||||||
<img src="docs/img/simulator1.png" width=500 alt="Flix simulator">
|
<img src="docs/img/simulator1.png" width=500 alt="Flix simulator">
|
||||||
|
|
||||||
## Documentation
|
## Documentation articles
|
||||||
|
|
||||||
1. [Assembly instructions](docs/assembly.md).
|
1. [Assembly instructions](docs/assembly.md).
|
||||||
2. [Usage: build, setup and flight](docs/usage.md).
|
2. [Usage: build, setup and flight](docs/usage.md).
|
||||||
@@ -71,14 +79,14 @@ Additional articles:
|
|||||||
|
|
||||||
|Type|Part|Image|Quantity|
|
|Type|Part|Image|Quantity|
|
||||||
|-|-|:-:|:-:|
|
|-|-|:-:|:-:|
|
||||||
|Microcontroller board|ESP32 Mini|<img src="docs/img/esp32.jpg" width=100>|1|
|
|Microcontroller board|ESP32 Mini.<br>ESP32-S3/ESP32-C3 boards are also supported.|<img src="docs/img/esp32.jpg" width=100>|1|
|
||||||
|IMU (and barometer¹) board|GY‑91, MPU-9265 (or other MPU‑9250/MPU‑6500 board)<br>ICM20948V2 (ICM‑20948)³<br>GY-521 (MPU-6050)³⁻¹|<img src="docs/img/gy-91.jpg" width=90 align=center><br><img src="docs/img/icm-20948.jpg" width=100><br><img src="docs/img/gy-521.jpg" width=100>|1|
|
|IMU (and barometer¹) board|GY‑91, MPU-9265 (or other MPU‑9250/MPU‑6500 board)<br>ICM20948V2 (ICM‑20948)<br>GY-521 (MPU-6050)|<img src="docs/img/gy-91.jpg" width=90 align=center><br><img src="docs/img/icm-20948.jpg" width=100><br><img src="docs/img/gy-521.jpg" width=100>|1|
|
||||||
|Boost converter (optional, for more stable power supply)|5V output|<img src="docs/img/buck-boost.jpg" width=100>|1|
|
|*Boost converter (optional, for more stable power supply)*|*5V output*|<img src="docs/img/buck-boost.jpg" width=100>|1|
|
||||||
|Motor|8520 3.7V brushed motor.<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).<br>Make sure the motor shaft diameter and propeller hole diameter match!|<img src="docs/img/motor.jpeg" width=100>|4|
|
|Motor|8520 3.7V brushed motor.<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).<br>Make sure the motor shaft diameter and propeller hole diameter match!|<img src="docs/img/motor.jpeg" width=100>|4|
|
||||||
|Propeller|55 mm (alternatively 65 mm)|<img src="docs/img/prop.jpg" width=100>|4|
|
|Propeller|55 mm or 65 mm|<img src="docs/img/prop.jpg" width=100>|4|
|
||||||
|MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|
|
|MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|
|
||||||
|Pull-down resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|4|
|
|Pull-down resistor<br>Voltage measurement resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|6|
|
||||||
|3.7V Li-Po battery|LW 952540 (or any compatible by the size)|<img src="docs/img/battery.jpg" width=100>|1|
|
|3.7V Li-Po battery|LW 952540 (or any compatible by the size).<br>Make sure the battery has enough discharge rate — 25C or more!|<img src="docs/img/battery.jpg" width=100>|1|
|
||||||
|Battery connector cable|MX2.0 2P female|<img src="docs/img/mx.png" width=100>|1|
|
|Battery connector cable|MX2.0 2P female|<img src="docs/img/mx.png" width=100>|1|
|
||||||
|Li-Po Battery charger|Any|<img src="docs/img/charger.jpg" width=100>|1|
|
|Li-Po Battery charger|Any|<img src="docs/img/charger.jpg" width=100>|1|
|
||||||
|Screws for IMU board mounting|M3x5|<img src="docs/img/screw-m3.jpg" width=100>|2|
|
|Screws for IMU board mounting|M3x5|<img src="docs/img/screw-m3.jpg" width=100>|2|
|
||||||
@@ -138,10 +146,10 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir
|
|||||||
|
|
||||||
|Motor|Position|Direction|Prop type|Motor wires|GPIO|
|
|Motor|Position|Direction|Prop type|Motor wires|GPIO|
|
||||||
|-|-|-|-|-|-|
|
|-|-|-|-|-|-|
|
||||||
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*|
|
||||||
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*|
|
||||||
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*|
|
||||||
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*|
|
||||||
|
|
||||||
Clockwise motors have blue & red wires and correspond to propeller type A (marked on the propeller).
|
Clockwise motors have blue & red wires and correspond to propeller type A (marked on the propeller).
|
||||||
Counter-clockwise motors have black & white wires correspond to propeller type B.
|
Counter-clockwise motors have black & white wires correspond to propeller type B.
|
||||||
@@ -152,14 +160,16 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir
|
|||||||
|-|-|
|
|-|-|
|
||||||
|GND|GND|
|
|GND|GND|
|
||||||
|VIN|VCC (or 3.3V depending on the receiver)|
|
|VIN|VCC (or 3.3V depending on the receiver)|
|
||||||
|Signal (TX)|GPIO4¹|
|
|Signal (TX)|GPIO4|
|
||||||
|
|
||||||
*¹ — UART2 RX pin was [changed](https://docs.espressif.com/projects/arduino-esp32/en/latest/migration_guides/2.x_to_3.0.html#id14) to GPIO4 in Arduino ESP32 core 3.0.*
|
* Optionally connect the battery voltage divider for voltage monitoring to any ADC1 pin (e. g. *GPIO32* on ESP32, *GPIO3* on ESP32-S3).
|
||||||
|
|
||||||
|
ESP32 and ESP32-S3 [can measure](https://docs.espressif.com/projects/arduino-esp32/en/latest/api/adc.html#analogsetattenuation) up to 3.1 V and ESP32-S3/ESP32-C3 can measure up to 2.5 V, so choose the voltage divider resistors accordingly.
|
||||||
|
|
||||||
## Resources
|
## Resources
|
||||||
|
|
||||||
* Telegram channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter.
|
* Telegram channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter.
|
||||||
* Official Telegram chat: https://t.me/opensourcequadcopterchat.
|
* Official Telegram chat: https://t.me/opensourcequadcopterchat (English / Russian).
|
||||||
* Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/.
|
* Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/.
|
||||||
|
|
||||||
## Disclaimer
|
## Disclaimer
|
||||||
|
|||||||
@@ -1,5 +0,0 @@
|
|||||||
board_manager:
|
|
||||||
additional_urls:
|
|
||||||
- https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json
|
|
||||||
network:
|
|
||||||
connection_timeout: 1h
|
|
||||||
@@ -28,6 +28,8 @@ Soldered components ([schematics variant](https://miro.com/app/board/uXjVN-dTjoo
|
|||||||
|
|
||||||
<img src="img/assembly/7.jpg" width=600>
|
<img src="img/assembly/7.jpg" width=600>
|
||||||
|
|
||||||
|
See an alternative assembly process photos here: https://drive.google.com/drive/folders/1FG5BH9RCzdf1XmJcC70PymiRMXcz6Fx7?usp=sharing.
|
||||||
|
|
||||||
## Motor directions
|
## Motor directions
|
||||||
|
|
||||||
> [!WARNING]
|
> [!WARNING]
|
||||||
@@ -41,10 +43,10 @@ Motors connection table:
|
|||||||
|
|
||||||
|Motor|Position|Direction|Prop type|Motor wires|GPIO|
|
|Motor|Position|Direction|Prop type|Motor wires|GPIO|
|
||||||
|-|-|-|-|-|-|
|
|-|-|-|-|-|-|
|
||||||
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*|
|
||||||
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*|
|
||||||
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*|
|
||||||
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*|
|
||||||
|
|
||||||
## Motors tightening
|
## Motors tightening
|
||||||
|
|
||||||
|
|||||||
@@ -110,7 +110,7 @@ float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов)
|
|||||||
|
|
||||||
#### Скалярное произведение
|
#### Скалярное произведение
|
||||||
|
|
||||||
Скалярное произведение векторов (*dot product*) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
|
Скалярное произведение векторов *(dot product)* — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
|
||||||
|
|
||||||
В Flix используется статический метод `Vector::dot()`:
|
В Flix используется статический метод `Vector::dot()`:
|
||||||
|
|
||||||
@@ -124,7 +124,7 @@ float dotProduct = Vector::dot(a, b); // 32
|
|||||||
|
|
||||||
#### Векторное произведение
|
#### Векторное произведение
|
||||||
|
|
||||||
Векторное произведение (*cross product*) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`:
|
Векторное произведение *(cross product)* позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`:
|
||||||
|
|
||||||
```cpp
|
```cpp
|
||||||
Vector a(1, 2, 3);
|
Vector a(1, 2, 3);
|
||||||
@@ -144,9 +144,9 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
|
|||||||
|
|
||||||
В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, строго говоря, не являются вектором):
|
В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, строго говоря, не являются вектором):
|
||||||
|
|
||||||
* Угол по крену (*roll*) — `vector.x`.
|
* Угол по крену *(roll)* — `vector.x`.
|
||||||
* Угол по тангажу (*pitch*) — `vector.y`.
|
* Угол по тангажу *(pitch)* — `vector.y`.
|
||||||
* Угол по рысканию (*yaw*) — `vector.z`.
|
* Угол по рысканию *(yaw)* — `vector.z`.
|
||||||
|
|
||||||
Особенности углов Эйлера:
|
Особенности углов Эйлера:
|
||||||
|
|
||||||
@@ -162,8 +162,8 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
|
|||||||
|
|
||||||
Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами:
|
Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами:
|
||||||
|
|
||||||
* **Ось вращения** (*axis*) — единичный вектор, определяющий ось вращения.
|
* **Ось вращения** *(axis)* — единичный вектор, определяющий ось вращения.
|
||||||
* **Угол поворота** (*angle* или *θ*) — угол, на который нужно повернуть объект вокруг этой оси.
|
* **Угол поворота** *(angle* или *θ)* — угол, на который нужно повернуть объект вокруг этой оси.
|
||||||
|
|
||||||
В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах:
|
В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах:
|
||||||
|
|
||||||
@@ -177,7 +177,7 @@ float angle = radians(45);
|
|||||||
|
|
||||||
### Вектор вращения
|
### Вектор вращения
|
||||||
|
|
||||||
Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** (*rotation vector*). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
|
Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** *(rotation vector)*. Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
|
||||||
|
|
||||||
Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями.
|
Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями.
|
||||||
|
|
||||||
@@ -198,7 +198,7 @@ Vector rotation = radians(45) * Vector(1, 2, 3);
|
|||||||
<a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br>
|
<a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения (*axis*) и угла поворота (*θ*) по формуле:
|
Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения *(axis)* и угла поворота *(θ)* по формуле:
|
||||||
|
|
||||||
\\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\]
|
\\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\]
|
||||||
|
|
||||||
|
|||||||
@@ -177,7 +177,7 @@ imu.setDLPF(imu.DLPF_MAX);
|
|||||||
|
|
||||||
## Калибровка гироскопа
|
## Калибровка гироскопа
|
||||||
|
|
||||||
Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения (*bias*) и случайный шум (*noise*):
|
Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения *(bias)* и случайный шум *(noise)*:
|
||||||
|
|
||||||
\\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]
|
\\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]
|
||||||
|
|
||||||
|
|||||||
@@ -67,6 +67,38 @@ In order to add a console command, modify the `doCommand()` function in `cli.ino
|
|||||||
>
|
>
|
||||||
> For on-the-ground commands, use `pause()` function, instead of `delay()`. This function allows to pause in a way that MAVLink connection will continue working.
|
> For on-the-ground commands, use `pause()` function, instead of `delay()`. This function allows to pause in a way that MAVLink connection will continue working.
|
||||||
|
|
||||||
|
### Parameter subsystem
|
||||||
|
|
||||||
|
Parameters subsystem (`parameters.ino`) uses standard [Preferences.h](https://docs.espressif.com/projects/arduino-esp32/en/latest/tutorials/preferences.html) ESP32 library to store parameters in non-volatile memory. Each parameter is a regular global variable, which is registered in the `parameters` array.
|
||||||
|
|
||||||
|
To add a new parameter:
|
||||||
|
|
||||||
|
1. Define a global variable for the parameter, two types are supported: `float` and `int`.
|
||||||
|
2. Add an entry to the `parameters` array, with the parameter name, a pointer to the variable, and optionally a callback function to call when the parameter is changed.
|
||||||
|
3. Everything else will be handled automatically.
|
||||||
|
|
||||||
|
See examples of adding new parameters in commits: [c434107](https://github.com/okalachev/flix/commit/c434107), [a687303](https://github.com/okalachev/flix/commit/a687303).
|
||||||
|
|
||||||
|
> [!NOTE]
|
||||||
|
> Since all the parameters are internally stored and passed as floats, the safe range for `int` parameters is -16777216 to 16777215.
|
||||||
|
|
||||||
|
## Adding a subsystem
|
||||||
|
|
||||||
|
To add a new subsystem:
|
||||||
|
|
||||||
|
1. Create a new `*.ino` file for your subsystem.
|
||||||
|
2. Define setup and loop functions for the subsystem, for example `setupMySubsystem()` and `loopMySubsystem()`.
|
||||||
|
3. Use `Rate` class if you need to limit the loop frequency, for example:
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
Rate mySubsystemRate(100); // 100 Hz
|
||||||
|
|
||||||
|
void loopMySubsystem() {
|
||||||
|
if (!mySubsystemRate) return;
|
||||||
|
// Do something...
|
||||||
|
}
|
||||||
|
4. Add setup and loop calls in to `setup()` and `loop()` functions in `flix.ino`.
|
||||||
|
|
||||||
## Building the firmware
|
## Building the firmware
|
||||||
|
|
||||||
See build instructions in [usage.md](usage.md).
|
See build instructions in [usage.md](usage.md).
|
||||||
|
|||||||
|
After Width: | Height: | Size: 46 KiB |
|
After Width: | Height: | Size: 101 KiB |
|
Before Width: | Height: | Size: 23 KiB After Width: | Height: | Size: 33 KiB |
|
After Width: | Height: | Size: 62 KiB |
|
After Width: | Height: | Size: 48 KiB |
|
After Width: | Height: | Size: 50 KiB |
|
After Width: | Height: | Size: 17 KiB |
|
After Width: | Height: | Size: 62 KiB |
|
After Width: | Height: | Size: 44 KiB |
|
After Width: | Height: | Size: 55 KiB |
|
After Width: | Height: | Size: 60 KiB |
|
After Width: | Height: | Size: 38 KiB |
|
After Width: | Height: | Size: 50 KiB |
|
After Width: | Height: | Size: 49 KiB |
|
After Width: | Height: | Size: 41 KiB |
|
After Width: | Height: | Size: 56 KiB |
|
After Width: | Height: | Size: 60 KiB |
|
After Width: | Height: | Size: 54 KiB |
|
After Width: | Height: | Size: 69 KiB |
|
After Width: | Height: | Size: 58 KiB |
|
After Width: | Height: | Size: 105 KiB |
|
After Width: | Height: | Size: 34 KiB |
|
After Width: | Height: | Size: 36 KiB |
@@ -5,27 +5,32 @@
|
|||||||
Do the following:
|
Do the following:
|
||||||
|
|
||||||
* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#building-the-firmware).
|
* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#building-the-firmware).
|
||||||
* **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
|
* **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU-9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
|
||||||
* **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
|
* **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
|
||||||
|
|
||||||
## The drone doesn't fly
|
## The drone doesn't fly
|
||||||
|
|
||||||
Do the following:
|
Do the following:
|
||||||
|
|
||||||
* **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
|
* **Check the battery voltage**. Use a multimeter to measure the battery voltage. The fully charged battery should have about 4.2V.
|
||||||
* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
|
* **Check the battery you use has enough discharge current**. The battery should be able to provide 15A of current. So the C-rating for a 1000 mAh battery should be at least 15C (higher is better).
|
||||||
|
* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button or `reboot` command to see the whole startup output.
|
||||||
* **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
|
* **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
|
||||||
* **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
|
* **Check if the console is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the console using QGroundControl *(Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console)*.
|
||||||
* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
|
|
||||||
* **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
|
* **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
|
||||||
* **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.
|
* **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.
|
||||||
|
* **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
|
||||||
* **Check the IMU is working**. Perform `imu` command and check its output:
|
* **Check the IMU is working**. Perform `imu` command and check its output:
|
||||||
* The `status` field should be `OK`.
|
* The `status` field should be `OK`.
|
||||||
* The `rate` field should be about 1000 (Hz).
|
* The `rate` field should be about 1000 (Hz).
|
||||||
* The `accel` and `gyro` fields should change as you move the drone.
|
* The `accel` and `gyro` fields should change as you move the drone.
|
||||||
* **Calibrate the accelerometer.** if is wasn't done before. Type `ca` command in Serial Monitor and follow the instructions.
|
|
||||||
* **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct.
|
|
||||||
* **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, set the correct IMU orientation as described in the [tutorial](usage.md#define-imu-orientation).
|
* **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, set the correct IMU orientation as described in the [tutorial](usage.md#define-imu-orientation).
|
||||||
|
* **Calibrate the accelerometer.** if is wasn't done before. Type `ca` command in Serial Monitor and follow the instructions.
|
||||||
|
* **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation is shown exactly as on the video below:
|
||||||
|
|
||||||
|
<a href="https://youtu.be/yVRN23-GISU"><img width=200 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
|
||||||
|
|
||||||
|
* **Check the IMU output**. Connect to the drone using QGroundControl on your computer. Go to the *Analyze* tab, *MAVLINK Inspector*. Plot the data from the `SCALED_IMU` message. The gyroscope and accelerometer data should change according to the drone movement.
|
||||||
* **Check the motors type**. Motors with exact 3.7V voltage are needed, not ranged working voltage (3.7V — 6V).
|
* **Check the motors type**. Motors with exact 3.7V voltage are needed, not ranged working voltage (3.7V — 6V).
|
||||||
* **Check the motors**. Perform the following commands using Serial Monitor:
|
* **Check the motors**. Perform the following commands using Serial Monitor:
|
||||||
* `mfr` — should rotate front right motor (counter-clockwise).
|
* `mfr` — should rotate front right motor (counter-clockwise).
|
||||||
@@ -33,7 +38,10 @@ Do the following:
|
|||||||
* `mrl` — should rotate rear left motor (counter-clockwise).
|
* `mrl` — should rotate rear left motor (counter-clockwise).
|
||||||
* `mrr` — should rotate rear right motor (clockwise).
|
* `mrr` — should rotate rear right motor (clockwise).
|
||||||
* **Check the propeller directions are correct**. Make sure your propeller types (A or B) are installed as on the picture:
|
* **Check the propeller directions are correct**. Make sure your propeller types (A or B) are installed as on the picture:
|
||||||
|
|
||||||
<img src="img/user/peter_ukhov-2/1.jpg" width="200">
|
<img src="img/user/peter_ukhov-2/1.jpg" width="200">
|
||||||
* **Check the remote control**. Using `rc` command, check the control values reflect your sticks movement. All the controls should change between -1 and 1, and throttle between 0 and 1.
|
|
||||||
* If using SBUS receiver, **calibrate the RC**. Type `cr` command in Serial Monitor and follow the instructions.
|
* **If using an SBUS receiver**:
|
||||||
* **Check the IMU output using QGroundControl**. Connect to the drone using QGroundControl on your computer. Go to the *Analyze* tab, *MAVLINK Inspector*. Plot the data from the `SCALED_IMU` message. The gyroscope and accelerometer data should change according to the drone movement.
|
* **Define the used GPIO pin** in `RC_RX_PIN` parameter.
|
||||||
|
* **Calibrate the RC** using `cr` command in the console.
|
||||||
|
* **Check the controls** using `rc` command. All the controls should change between -1 and 1, and the throttle between 0 and 1.
|
||||||
|
|||||||
@@ -20,13 +20,14 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
|
|||||||
|
|
||||||
1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
|
1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
|
||||||
2. *Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).*
|
2. *Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).*
|
||||||
3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
|
3. Install ESP32 core, version 3.3.10. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
|
||||||
4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
|
4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
|
||||||
* `FlixPeriph`, the latest version.
|
* `FlixPeriph`, the latest version.
|
||||||
* `MAVLink`, version 2.0.16.
|
* `MAVLink`, version 2.0.25.
|
||||||
5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE.
|
5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE.
|
||||||
6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
|
6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
|
||||||
7. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
|
7. Set *Tools* ⇒ *Core Debug Level* to *Error* to see the errors in the serial console. Set *Tools* ⇒ *USB CDC on Boot* to *Enabled* for ESP32-S3/ESP32-C3 boards.
|
||||||
|
8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
|
||||||
|
|
||||||
### Command line (Windows, Linux, macOS)
|
### Command line (Windows, Linux, macOS)
|
||||||
|
|
||||||
@@ -57,6 +58,12 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
|
|||||||
make upload monitor
|
make upload monitor
|
||||||
```
|
```
|
||||||
|
|
||||||
|
For ESP32-S3/ESP32-C3 boards, set the appropriate [FQBN](https://docs.arduino.cc/arduino-cli/FAQ/#whats-the-fqbn-string) using `BOARD` parameter:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
make BOARD=esp32:esp32:esp32s3:DebugLevel=error,FlashSize=4M,CDCOnBoot=cdc upload
|
||||||
|
```
|
||||||
|
|
||||||
See other available Make commands in [Makefile](../Makefile).
|
See other available Make commands in [Makefile](../Makefile).
|
||||||
|
|
||||||
> [!TIP]
|
> [!TIP]
|
||||||
@@ -82,6 +89,9 @@ QGroundControl is a ground control station software that can be used to monitor
|
|||||||
3. Connect your computer or smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
3. Connect your computer or smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
||||||
4. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
|
4. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
|
||||||
|
|
||||||
|
> [!TIP]
|
||||||
|
> If QGroundControl doesn't connect, try to disable the firewall and/or VPN on your computer, as they may block the connection.
|
||||||
|
|
||||||
### Access console
|
### Access console
|
||||||
|
|
||||||
The console is a command line interface (CLI) that allows to interact with the drone, change parameters, and perform various actions. There are two ways of accessing the console: using **serial port** or using **QGroundControl (wirelessly)**.
|
The console is a command line interface (CLI) that allows to interact with the drone, change parameters, and perform various actions. There are two ways of accessing the console: using **serial port** or using **QGroundControl (wirelessly)**.
|
||||||
@@ -108,13 +118,13 @@ The drone is configured using parameters. To access and modify them, go to the Q
|
|||||||
|
|
||||||
<img src="img/parameters.png" width="400">
|
<img src="img/parameters.png" width="400">
|
||||||
|
|
||||||
You can also work with parameters using `p` command in the console.
|
You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
|
||||||
|
|
||||||
### Define IMU orientation
|
### Define IMU orientation
|
||||||
|
|
||||||
Use parameters, to define the IMU board axes orientation relative to the drone's axes: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
|
The IMU orientation (relative to the drone's axes) is defined using the parameters: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
|
||||||
|
|
||||||
The drone has *X* axis pointing forward, *Y* axis pointing left, and *Z* axis pointing up, and the supported IMU boards have *X* axis pointing to the pins side and *Z* axis pointing up from the component side:
|
The drone has *X* axis pointing forward, *Y* axis pointing left, and *Z* axis pointing up, and the supported IMU boards have *X* axis pointing to the mounting holes side and *Z* axis pointing up from the component side:
|
||||||
|
|
||||||
<img src="img/imu-axes.png" width="200">
|
<img src="img/imu-axes.png" width="200">
|
||||||
|
|
||||||
@@ -122,10 +132,10 @@ Use the following table to set the parameters for common IMU orientations:
|
|||||||
|
|
||||||
|Orientation|Parameters|Orientation|Parameters|
|
|Orientation|Parameters|Orientation|Parameters|
|
||||||
|:-:|-|-|-|
|
|:-:|-|-|-|
|
||||||
|<img src="img/imu-rot-1.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0 |<img src="img/imu-rot-5.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0|
|
|<img src="img/imu-rot-3.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0 |<img src="img/imu-rot-7.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0|
|
||||||
|<img src="img/imu-rot-2.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|<img src="img/imu-rot-6.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|
|
|<img src="img/imu-rot-2.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|<img src="img/imu-rot-6.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|
|
||||||
|<img src="img/imu-rot-3.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|<img src="img/imu-rot-7.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|
|
|<img src="img/imu-rot-1.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|<img src="img/imu-rot-5.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|
|
||||||
|<img src="img/imu-rot-4.png" width="180"><br>☑️ **Default**|<br>`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|<img src="img/imu-rot-8.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|
|
|<img src="img/imu-rot-4.png" width="180"><br>☑️ **Default**|<br>`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|<img src="img/imu-rot-8.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|
|
||||||
|
|
||||||
### Calibrate accelerometer
|
### Calibrate accelerometer
|
||||||
|
|
||||||
@@ -134,27 +144,52 @@ Before flight you need to calibrate the accelerometer:
|
|||||||
1. Access the console using QGroundControl (recommended) or Serial Monitor.
|
1. Access the console using QGroundControl (recommended) or Serial Monitor.
|
||||||
2. Type `ca` command there and follow the instructions.
|
2. Type `ca` command there and follow the instructions.
|
||||||
|
|
||||||
### Check everything works
|
### Setup motors
|
||||||
|
|
||||||
1. Check the IMU is working: perform `imu` command and check its output:
|
If using non-default motor pins, set the pin numbers using the parameters: `MOTOR_PIN_FL`, `MOTOR_PIN_FR`, `MOTOR_PIN_RL`, `MOTOR_PIN_RR` (front-left, front-right, rear-left, rear-right respectively).
|
||||||
|
|
||||||
|
Certain ESP32 models (such as ESP32-S3 and ESP32-C3) support a lower maximum PWM frequency; on these boards the parameter `MOT_PWM_FREQ` should be set to 38000 Hz.
|
||||||
|
|
||||||
|
If using brushless motors and ESCs:
|
||||||
|
|
||||||
|
1. Set the appropriate PWM using the parameters: `MOT_PWM_STOP`, `MOT_PWM_MIN`, and `MOT_PWM_MAX` (1000, 1000, and 2000 is typical).
|
||||||
|
2. Decrease the PWM frequency using the `MOT_PWM_FREQ` parameter (400 is typical).
|
||||||
|
|
||||||
|
> [!CAUTION]
|
||||||
|
> **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
|
||||||
|
|
||||||
|
### Battery voltage monitoring
|
||||||
|
|
||||||
|
ESP32 ADC can measure only up to 3.3 V, so you need to use a voltage divider to monitor the battery voltage. To enable voltage measurement, set the following parameters:
|
||||||
|
|
||||||
|
1. `PWR_VOLT_PIN` — GPIO pin number where the voltage divider is connected (*-1* to disable).
|
||||||
|
2. `PWR_VOLT_SCALE` — voltage divider coefficient (*2* for two equal resistors).
|
||||||
|
|
||||||
|
After this setup, you should see the battery voltage in QGroundControl top panel or using `pw` command in the console.
|
||||||
|
|
||||||
|
### Important: check everything works
|
||||||
|
|
||||||
|
1. Check the IMU is working: perform `imu` command in the console and check the output:
|
||||||
|
|
||||||
* The `status` field should be `OK`.
|
* The `status` field should be `OK`.
|
||||||
* The `rate` field should be about 1000 (Hz).
|
* The `rate` field should be about 1000 (Hz).
|
||||||
* The `accel` and `gyro` fields should change as you move the drone.
|
* The `accel` and `gyro` fields should change as you move the drone.
|
||||||
|
* The `accel bias` and `accel scale` fields should contain calibration parameters (not zeros and ones).
|
||||||
|
* The `gyro bias` field should contain estimated gyro bias (not zeros).
|
||||||
* The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
|
* The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
|
||||||
|
|
||||||
2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video:
|
2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video:
|
||||||
|
|
||||||
<a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
|
<a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
|
||||||
|
|
||||||
3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!**
|
3. Perform motor tests. Use the following commands **— remove the propellers before running the tests!**
|
||||||
|
|
||||||
* `mfr` — should rotate front right motor (counter-clockwise).
|
* `mfr` — rotate front right motor (counter-clockwise).
|
||||||
* `mfl` — should rotate front left motor (clockwise).
|
* `mfl` — rotate front left motor (clockwise).
|
||||||
* `mrl` — should rotate rear left motor (counter-clockwise).
|
* `mrl` — rotate rear left motor (counter-clockwise).
|
||||||
* `mrr` — should rotate rear right motor (clockwise).
|
* `mrr` — rotate rear right motor (clockwise).
|
||||||
|
|
||||||
Rotation diagram:
|
Make sure rotation directions and propeller types match the following diagram:
|
||||||
|
|
||||||
<img src="img/motors.svg" width=200>
|
<img src="img/motors.svg" width=200>
|
||||||
|
|
||||||
@@ -167,6 +202,18 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
|
|||||||
|
|
||||||
### Control with a smartphone
|
### Control with a smartphone
|
||||||
|
|
||||||
|
#### Using Mavlink Joystick app (Android)
|
||||||
|
|
||||||
|
<img src="https://github.com/goldarte/mavlink-joystick/blob/master/app_screen.png?raw=true" width="400">
|
||||||
|
|
||||||
|
1. Download and install [Mavlink Joystick app](https://github.com/goldarte/mavlink-joystick/releases/latest).
|
||||||
|
2. Power the drone using the battery.
|
||||||
|
3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
||||||
|
4. Open Mavlink Joystick app. It should connect and begin showing the drone's telemetry automatically.
|
||||||
|
5. Use the virtual joystick to fly the drone!
|
||||||
|
|
||||||
|
#### Using QGroundControl app
|
||||||
|
|
||||||
1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
|
1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
|
||||||
2. Power the drone using the battery.
|
2. Power the drone using the battery.
|
||||||
3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
||||||
@@ -179,11 +226,13 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
|
|||||||
|
|
||||||
### Control with a remote control
|
### Control with a remote control
|
||||||
|
|
||||||
Before using remote SBUS-connected remote control, you need to calibrate it:
|
If using SBUS-connected remote control you need to enable SBUS and calibrate it:
|
||||||
|
|
||||||
1. Access the console using QGroundControl (recommended) or Serial Monitor.
|
1. Connect to the drone using QGroundControl.
|
||||||
2. Type `cr` command and follow the instructions.
|
2. In parameters, set the `RC_RX_PIN` parameter to the GPIO pin number where the SBUS signal is connected, for example: 4. Negative value disables SBUS.
|
||||||
3. Use the remote control to fly the drone!
|
3. Check if the receiver is working using `rc` command in the console.
|
||||||
|
4. Open the console, type `cr` command and follow the instructions to calibrate the remote control.
|
||||||
|
5. Use the remote control to fly the drone!
|
||||||
|
|
||||||
### Control with a USB remote control
|
### Control with a USB remote control
|
||||||
|
|
||||||
@@ -220,11 +269,11 @@ When finished flying, **disarm** the drone, moving the left stick to the bottom
|
|||||||
|
|
||||||
### Flight modes
|
### Flight modes
|
||||||
|
|
||||||
Flight mode is changed using mode switch on the remote control or using the command line.
|
Flight mode is changed using mode switch on the remote control (if configured) or using the console commands. The main flight mode is *STAB*. In order to change modes using SBUS remote control, set the parameters: `CTL_FLT_MODE_0`, `CTL_FLT_MODE_1`, and `CTL_FLT_MODE_2` to required mode numbers (0 for *RAW*, 1 for *ACRO*, 2 for *STAB*, 3 for *AUTO*).
|
||||||
|
|
||||||
#### STAB
|
#### STAB
|
||||||
|
|
||||||
The default mode is *STAB*. In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
|
In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
|
||||||
|
|
||||||
> [!IMPORTANT]
|
> [!IMPORTANT]
|
||||||
> The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually.
|
> The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually.
|
||||||
@@ -239,13 +288,75 @@ In this mode, the pilot controls the angular rates. This control method is diffi
|
|||||||
|
|
||||||
#### AUTO
|
#### AUTO
|
||||||
|
|
||||||
In this mode, the pilot inputs are ignored (except the mode switch, if configured). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed autonomous behavior.
|
In this mode, the pilot inputs are ignored (except the mode switch). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed behavior.
|
||||||
|
|
||||||
If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
|
If the pilot moves the control sticks and mode switch is not configured, the drone will switch back to *STAB* mode.
|
||||||
|
|
||||||
|
## Wi-Fi configuration
|
||||||
|
|
||||||
|
You can configure the Wi-Fi using parameters and console commands.
|
||||||
|
|
||||||
|
The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the console:
|
||||||
|
|
||||||
|
* `0` — Wi-Fi is disabled.
|
||||||
|
* `1` — Access Point mode *(AP)* — the drone creates a Wi-Fi network.
|
||||||
|
* `2` — Client mode *(STA)* — the drone connects to an existing Wi-Fi network (may cause additional delays, so generally not recommended).
|
||||||
|
* `3` — ESP-NOW mode — the drone uses ESP-NOW protocol for communication.
|
||||||
|
|
||||||
|
The SSID and password are configured using the `ap` and `sta` console commands:
|
||||||
|
|
||||||
|
```
|
||||||
|
ap <ssid> <password>
|
||||||
|
sta <ssid> <password>
|
||||||
|
```
|
||||||
|
|
||||||
|
Example of configuring the Access Point mode:
|
||||||
|
|
||||||
|
```
|
||||||
|
ap my-flix-ssid mypassword123
|
||||||
|
p WIFI_MODE 1
|
||||||
|
```
|
||||||
|
|
||||||
|
Disabling Wi-Fi:
|
||||||
|
|
||||||
|
```
|
||||||
|
p WIFI_MODE 0
|
||||||
|
```
|
||||||
|
|
||||||
|
### Using ESP-NOW
|
||||||
|
|
||||||
|
[ESP-NOW](https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/network/esp_now.html) is a low level wireless communication protocol. It can provide lower latency, better reliability, and longer range than Wi-Fi. However, it requires a second ESP32 board to be used as a proxy for the computer.
|
||||||
|
|
||||||
|
<img src="img/espnow-connection.jpg" width="600">
|
||||||
|
|
||||||
|
To setup ESP-NOW communication:
|
||||||
|
|
||||||
|
1. Flash the second ESP32 board with ESP-NOW proxy sketch: [`tools/espnow-proxy/espnow-proxy.ino`](../tools/espnow-proxy/espnow-proxy.ino). Use Arduino IDE or command line: `make upload_proxy`.
|
||||||
|
|
||||||
|
2. Open Serial Monitor or use `make monitor` command. The ESP32 will print its MAC address and generated encryption key, for example:
|
||||||
|
|
||||||
|
```
|
||||||
|
espnow 7a:c8:e3:eb:bf:e9 &PiuSysxP9+$L&5E
|
||||||
|
```
|
||||||
|
|
||||||
|
Run this line as a console command on each drone you want to bind to this proxy board. [The maximum number](https://github.com/espressif/esp-idf/blob/e95cab4be8fd293e3f3323181e7a2280874da6f7/components/esp_wifi/include/esp_now.h#L32-L33) of simultaneously connected drones is 20 (unencrypted) io 6 (encrypted).
|
||||||
|
|
||||||
|
3. Set the `WIFI_MODE` parameter to `3` on the drone:
|
||||||
|
|
||||||
|
```
|
||||||
|
p WIFI_MODE 3
|
||||||
|
```
|
||||||
|
|
||||||
|
4. Go to the QGroundControl menu ⇒ *Application Settings* ⇒ *Comm Links*, add new link with the following settings:
|
||||||
|
* Name: ESP32.
|
||||||
|
* Type: Serial.
|
||||||
|
* Serial Port: choose the port of the proxy ESP32 board, e. g. `/dev/cu.usbserial-0001`.
|
||||||
|
* Baud Rate: 115200.
|
||||||
|
5. Click *Save*. QGroundControl should connect to the drone using ESP-NOW and begin showing the telemetry.
|
||||||
|
|
||||||
## Flight log
|
## Flight log
|
||||||
|
|
||||||
After the flight, you can download the flight log for analysis wirelessly. Use the following for that:
|
After the flight, you can download the flight log for analysis wirelessly. Use the following command on your computer for that:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
make log
|
make log
|
||||||
|
|||||||
@@ -4,6 +4,79 @@ This page contains user-built drones based on the Flix project. Publish your pro
|
|||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
Author: [Неруш Михаил](https://t.me/NerushMV).<br>
|
||||||
|
Description: custom frame made of 4 mm plywood, 8520 brushed motors, 75 mm propellers, MPU-6500. FlySky FS-i6X with ESP32-based adapter for ESP-NOW communication (using PPM output).
|
||||||
|
|
||||||
|
<img src="img/user/nerush/1.jpg" height=200> <img src="img/user/nerush/2.jpg" height=200>
|
||||||
|
|
||||||
|
[Flight video](https://drive.google.com/file/d/1jRXeGx34lJpUfw0GKLQeIzkWZvooQJSE/view?usp=sharing).
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
Author: [Konstantinos Paraskevas](https://github.com/Frapais).<br>
|
||||||
|
Description: drone with a custom single-boarded airframe, extending the [Sprig-C3 module](https://github.com/Frapais/Sprig-C3).
|
||||||
|
ESP32-C3 microcontroller, ICM-20948 IMU, on-board fuel-gauge, status LED indicator.<br>
|
||||||
|
Repository with all the code and PCB sources: https://github.com/Frapais/Sprig-Drone.
|
||||||
|
|
||||||
|
<img src="img/user/kostas/1.jpg" height=150> <img src="img/user/kostas/2.jpg" height=150>
|
||||||
|
|
||||||
|
Detailed video about making the drone:
|
||||||
|
|
||||||
|
<a href="https://youtu.be/82Q-uBq6s48"><img width=400 src="https://i3.ytimg.com/vi/82Q-uBq6s48/maxresdefault.jpg"></a>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
Author: [Awab Anas](http://t.me/AW_VENOM).<br>
|
||||||
|
Description: ESP32 D1 Mini, MPU-6050, 8520 3.7V brushed motors, 55 mm propellers, battery li-po 1200 mAh, controlling via [Mavlink Joystick app](https://github.com/goldarte/mavlink-joystick/releases/latest).<br>
|
||||||
|
[Flight validation](https://drive.google.com/file/d/12z0jfctZDBA6b5UKCG0Uje5rAxj6DhF-/view?usp=sharing).
|
||||||
|
|
||||||
|
<img src="img/user/aw_venom/1.jpg" height=200>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
Author: [Ina Tix](https://t.me/ina_tix).<br>
|
||||||
|
Description: XR2981 based DC-DC converter, ELRS MINI 2.4GHz RX SX1280 receiver (SBUS interface), Radiomaster TX12 remote control.<br>
|
||||||
|
[Flight validation](https://drive.google.com/file/d/1yqkKNuz4R_yxGqUNQxVpixJbXqEEcUSj/view?usp=share_link).
|
||||||
|
|
||||||
|
<img src="img/user/ina_tix/1.jpg" height=200> <img src="img/user/ina_tix/2.jpg" height=200> <img src="img/user/ina_tix/3.jpg" height=200>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
Author: Oleg Kalachev.<br>
|
||||||
|
Description: the first attempt on making an official PCB based Flix drone (Flix2 board). The IMU is not working on this version, so an external MPU-6050 board was used, therefore considered as **Flix version 1.5**.<br>
|
||||||
|
[Flight video](https://drive.google.com/file/d/1R7tuUsFmPY0CGcOCFfMFaCp9kR49K3bl/view?usp=sharing).
|
||||||
|
|
||||||
|
<img src="img/flix1.5.jpg" width=300>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
Author: [FanBy0ru](https://https://github.com/FanBy0ru).<br>
|
||||||
|
Description: custom 3D-printed frame.<br>
|
||||||
|
Frame STLs and flight validation: https://cults3d.com/en/3d-model/gadget/armature-pour-flix-drone.
|
||||||
|
|
||||||
|
<img src="img/user/fanby0ru/1.jpg" height=200> <img src="img/user/fanby0ru/2.jpg" height=200>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
Author: Ivan44 Phalko.<br>
|
||||||
|
Description: custom PCB, cusom test bench.<br>
|
||||||
|
[Flight validation](https://drive.google.com/file/d/17DNDJ1gPmCmDRAwjedCbJ9RXAyqMqqcX/view?usp=sharing).
|
||||||
|
|
||||||
|
<img src="img/user/phalko/1.jpg" height=200> <img src="img/user/phalko/2.jpg" height=200> <img src="img/user/phalko/3.jpg" height=200>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
Author: **Arkadiy "Arky" Matsekh**, Foucault Dynamics, Gold Coast, Australia.<br>
|
||||||
|
The drone was built for the University of Queensland industry-led Master's capstone project.
|
||||||
|
|
||||||
|
**Flight video:**
|
||||||
|
|
||||||
|
<a href="https://drive.google.com/file/d/1NNYSVXBY-w0JjCo07D8-PgnVq3ca9plj/view?usp=sharing"><img height=300 src="img/user/arkymatsekh/video.jpg"></a>
|
||||||
|
|
||||||
|
<img src="img/user/arkymatsekh/1.jpg" height=150> <img src="img/user/arkymatsekh/2.jpg" height=150> <img src="img/user/arkymatsekh/3.jpg" height=150>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
Author: [goldarte](https://t.me/goldarte).<br>
|
Author: [goldarte](https://t.me/goldarte).<br>
|
||||||
|
|
||||||
<img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150>
|
<img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150>
|
||||||
@@ -14,6 +87,17 @@ Author: [goldarte](https://t.me/goldarte).<br>
|
|||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
Author: [malagis](https://oshwhub.com/malagis).<br>
|
||||||
|
|
||||||
|
A Chinese custom PCB version of Flix with a big community of users, lots of materials and modifications.
|
||||||
|
|
||||||
|
Main project's page: https://oshwhub.com/malagis/esp32-mini-plane.<br>
|
||||||
|
Video about the project: https://www.bilibili.com/video/BV14vyqBFEJn/.
|
||||||
|
|
||||||
|
<img src="img/user/malagis/1.jpg" height=200> <img src="img/user/malagis/2.jpg" height=200> <img src="img/user/malagis/3.jpg" height=200>
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
## School 548 course
|
## School 548 course
|
||||||
|
|
||||||
Special course on quadcopter design and engineering took place in october-november 2025 in School 548, Moscow. The course included UAV control theory, electronics, drone assembly and setup practice, using the Flix project.
|
Special course on quadcopter design and engineering took place in october-november 2025 in School 548, Moscow. The course included UAV control theory, electronics, drone assembly and setup practice, using the Flix project.
|
||||||
|
|||||||
@@ -6,57 +6,64 @@
|
|||||||
#include "pid.h"
|
#include "pid.h"
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
#include "lpf.h"
|
||||||
|
|
||||||
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
||||||
extern const int RAW, ACRO, STAB, AUTO;
|
extern const int RAW, ACRO, STAB, AUTO;
|
||||||
|
extern const int W_AP, W_STA, W_ESPNOW;
|
||||||
extern float t, dt, loopRate;
|
extern float t, dt, loopRate;
|
||||||
extern uint16_t channels[16];
|
extern uint16_t channels[16];
|
||||||
extern float controlTime;
|
extern float controlTime;
|
||||||
extern int mode;
|
extern int mode;
|
||||||
extern bool armed;
|
extern bool armed;
|
||||||
|
extern LowPassFilter<Vector> gyroBiasFilter;
|
||||||
|
extern float voltage;
|
||||||
|
|
||||||
const char* motd =
|
const char* motd =
|
||||||
"\nWelcome to\n"
|
|
||||||
" _______ __ __ ___ ___\n"
|
" _______ __ __ ___ ___\n"
|
||||||
"| ____|| | | | \\ \\ / /\n"
|
"| ____|| | | | \\ \\ / /\n"
|
||||||
"| |__ | | | | \\ V /\n"
|
"| |__ | | | | \\ V /\n"
|
||||||
"| __| | | | | > <\n"
|
"| __| | | | | > <\n"
|
||||||
"| | | `----.| | / . \\\n"
|
"| | | `----.| | / . \\\n"
|
||||||
"|__| |_______||__| /__/ \\__\\\n\n"
|
"|__| |_______||__| /__/ \\__\\\n\n"
|
||||||
|
"(C) Oleg Kalachev\n"
|
||||||
|
"https://github.com/okalachev/flix\n\n"
|
||||||
"Commands:\n\n"
|
"Commands:\n\n"
|
||||||
"help - show help\n"
|
"help - show help\n"
|
||||||
"p - show all parameters\n"
|
"p - show all parameters\n"
|
||||||
"p <name> - show parameter\n"
|
"p <str> - show parameters starting with str\n"
|
||||||
"p <name> <value> - set parameter\n"
|
"p <name> <value> - set parameter\n"
|
||||||
"preset - reset parameters\n"
|
"preset - reset parameters\n"
|
||||||
"time - show time info\n"
|
"time - show time info\n"
|
||||||
"ps - show pitch/roll/yaw\n"
|
|
||||||
"psq - show attitude quaternion\n"
|
|
||||||
"imu - show IMU data\n"
|
"imu - show IMU data\n"
|
||||||
|
"ca - calibrate accel\n"
|
||||||
|
"st - show state estimation\n"
|
||||||
"arm - arm the drone\n"
|
"arm - arm the drone\n"
|
||||||
"disarm - disarm the drone\n"
|
"disarm - disarm the drone\n"
|
||||||
"raw/stab/acro/auto - set mode\n"
|
"raw/stab/acro/auto - set mode\n"
|
||||||
"rc - show RC data\n"
|
"rc - show RC data\n"
|
||||||
|
"cr - calibrate RC\n"
|
||||||
|
"pw - show power info\n"
|
||||||
"wifi - show Wi-Fi info\n"
|
"wifi - show Wi-Fi info\n"
|
||||||
|
"wifi ap/sta/espnow/off - set Wi-Fi mode\n"
|
||||||
|
"ap <ssid> <password> - configure Wi-Fi access point\n"
|
||||||
|
"sta <ssid> <password> - configure Wi-Fi client mode\n"
|
||||||
|
"espnow <mac> [<key>] - configure ESP-NOW peer\n"
|
||||||
"mot - show motor output\n"
|
"mot - show motor output\n"
|
||||||
"log [dump] - print log header [and data]\n"
|
"log [dump] - print log header [and data]\n"
|
||||||
"cr - calibrate RC\n"
|
|
||||||
"ca - calibrate accel\n"
|
|
||||||
"mfr, mfl, mrr, mrl - test motor (remove props)\n"
|
"mfr, mfl, mrr, mrl - test motor (remove props)\n"
|
||||||
"sys - show system info\n"
|
"sys - show system info\n"
|
||||||
"reset - reset drone's state\n"
|
"reset - reset drone's state\n"
|
||||||
"reboot - reboot the drone\n";
|
"reboot - reboot the drone\n";
|
||||||
|
|
||||||
void print(const char* format, ...) {
|
void print(const char* format, ...) {
|
||||||
char buf[1000];
|
char buf[3000];
|
||||||
va_list args;
|
va_list args;
|
||||||
va_start(args, format);
|
va_start(args, format);
|
||||||
vsnprintf(buf, sizeof(buf), format, args);
|
vsnprintf(buf, sizeof(buf), format, args);
|
||||||
va_end(args);
|
va_end(args);
|
||||||
Serial.print(buf);
|
Serial.print(buf);
|
||||||
#if WIFI_ENABLED
|
|
||||||
mavlinkPrint(buf);
|
mavlinkPrint(buf);
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void pause(float duration) {
|
void pause(float duration) {
|
||||||
@@ -64,9 +71,7 @@ void pause(float duration) {
|
|||||||
while (t - start < duration) {
|
while (t - start < duration) {
|
||||||
step();
|
step();
|
||||||
handleInput();
|
handleInput();
|
||||||
#if WIFI_ENABLED
|
|
||||||
processMavlink();
|
processMavlink();
|
||||||
#endif
|
|
||||||
delay(50);
|
delay(50);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -87,14 +92,12 @@ void doCommand(String str, bool echo = false) {
|
|||||||
// execute command
|
// execute command
|
||||||
if (command == "help" || command == "motd") {
|
if (command == "help" || command == "motd") {
|
||||||
print("%s\n", motd);
|
print("%s\n", motd);
|
||||||
} else if (command == "p" && arg0 == "") {
|
} else if (command == "p" && arg1 == "") {
|
||||||
printParameters();
|
printParameters(arg0.c_str());
|
||||||
} else if (command == "p" && arg0 != "" && arg1 == "") {
|
|
||||||
print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
|
|
||||||
} else if (command == "p") {
|
} else if (command == "p") {
|
||||||
bool success = setParameter(arg0.c_str(), arg1.toFloat());
|
bool success = setParameter(arg0.c_str(), arg1.toFloat());
|
||||||
if (success) {
|
if (success) {
|
||||||
print("%s = %g\n", arg0.c_str(), arg1.toFloat());
|
print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
|
||||||
} else {
|
} else {
|
||||||
print("Parameter not found: %s\n", arg0.c_str());
|
print("Parameter not found: %s\n", arg0.c_str());
|
||||||
}
|
}
|
||||||
@@ -104,15 +107,15 @@ void doCommand(String str, bool echo = false) {
|
|||||||
print("Time: %f\n", t);
|
print("Time: %f\n", t);
|
||||||
print("Loop rate: %.0f\n", loopRate);
|
print("Loop rate: %.0f\n", loopRate);
|
||||||
print("dt: %f\n", dt);
|
print("dt: %f\n", dt);
|
||||||
} else if (command == "ps") {
|
|
||||||
Vector a = attitude.toEuler();
|
|
||||||
print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
|
|
||||||
} else if (command == "psq") {
|
|
||||||
print("qw: %f qx: %f qy: %f qz: %f\n", attitude.w, attitude.x, attitude.y, attitude.z);
|
|
||||||
} else if (command == "imu") {
|
} else if (command == "imu") {
|
||||||
printIMUInfo();
|
printIMUInfo();
|
||||||
printIMUCalibration();
|
printIMUCalibration();
|
||||||
print("landed: %d\n", landed);
|
print("landed: %d\n", landed);
|
||||||
|
} else if (command == "st") {
|
||||||
|
print("rates: %g %g %g\n", rates.x, rates.y, rates.z);
|
||||||
|
print("attitude: %g %g %g %g\n", attitude.w, attitude.x, attitude.y, attitude.z);
|
||||||
|
print("roll: %g° pitch: %g° yaw: %g°\n", degrees(attitude.getRoll()), degrees(attitude.getPitch()), degrees(attitude.getYaw()));
|
||||||
|
print("landed: %d\n", landed);
|
||||||
} else if (command == "arm") {
|
} else if (command == "arm") {
|
||||||
armed = true;
|
armed = true;
|
||||||
} else if (command == "disarm") {
|
} else if (command == "disarm") {
|
||||||
@@ -135,10 +138,18 @@ void doCommand(String str, bool echo = false) {
|
|||||||
print("time: %.1f\n", controlTime);
|
print("time: %.1f\n", controlTime);
|
||||||
print("mode: %s\n", getModeName());
|
print("mode: %s\n", getModeName());
|
||||||
print("armed: %d\n", armed);
|
print("armed: %d\n", armed);
|
||||||
} else if (command == "wifi") {
|
} else if (command == "pw") {
|
||||||
#if WIFI_ENABLED
|
print("Voltage: %.1f V\n", voltage);
|
||||||
|
} else if (command == "wifi" && arg0 == "") {
|
||||||
printWiFiInfo();
|
printWiFiInfo();
|
||||||
#endif
|
} else if (command == "wifi") {
|
||||||
|
setWiFiMode(arg0);
|
||||||
|
} else if (command == "ap") {
|
||||||
|
configWiFi(W_AP, arg0.c_str(), arg1.c_str());
|
||||||
|
} else if (command == "sta") {
|
||||||
|
configWiFi(W_STA, arg0.c_str(), arg1.c_str());
|
||||||
|
} else if (command == "espnow") {
|
||||||
|
configWiFi(W_ESPNOW, arg0.c_str(), arg1.c_str());
|
||||||
} else if (command == "mot") {
|
} else if (command == "mot") {
|
||||||
print("front-right %g front-left %g rear-right %g rear-left %g\n",
|
print("front-right %g front-left %g rear-right %g rear-left %g\n",
|
||||||
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
|
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
|
||||||
@@ -161,9 +172,11 @@ void doCommand(String str, bool echo = false) {
|
|||||||
#ifdef ESP32
|
#ifdef ESP32
|
||||||
print("Chip: %s\n", ESP.getChipModel());
|
print("Chip: %s\n", ESP.getChipModel());
|
||||||
print("Temperature: %.1f °C\n", temperatureRead());
|
print("Temperature: %.1f °C\n", temperatureRead());
|
||||||
print("Free heap: %d\n", ESP.getFreeHeap());
|
print("Total RAM: %d KB\n", ESP.getHeapSize() / 1024);
|
||||||
|
print("Free heap: %d KB\n", ESP.getFreeHeap() / 1024);
|
||||||
|
print("Firmware: " __DATE__ " " __TIME__ "\n");
|
||||||
// Print tasks table
|
// Print tasks table
|
||||||
print("Num Task Stack Prio Core CPU%%\n");
|
print("Num Task MinSt Prio Core CPU%%\n");
|
||||||
int taskCount = uxTaskGetNumberOfTasks();
|
int taskCount = uxTaskGetNumberOfTasks();
|
||||||
TaskStatus_t *systemState = new TaskStatus_t[taskCount];
|
TaskStatus_t *systemState = new TaskStatus_t[taskCount];
|
||||||
uint32_t totalRunTime;
|
uint32_t totalRunTime;
|
||||||
@@ -172,12 +185,13 @@ void doCommand(String str, bool echo = false) {
|
|||||||
String core = systemState[i].xCoreID == tskNO_AFFINITY ? "*" : String(systemState[i].xCoreID);
|
String core = systemState[i].xCoreID == tskNO_AFFINITY ? "*" : String(systemState[i].xCoreID);
|
||||||
int cpuPercentage = systemState[i].ulRunTimeCounter / (totalRunTime / 100);
|
int cpuPercentage = systemState[i].ulRunTimeCounter / (totalRunTime / 100);
|
||||||
print("%-5d%-20s%-7d%-6d%-6s%d\n",systemState[i].xTaskNumber, systemState[i].pcTaskName,
|
print("%-5d%-20s%-7d%-6d%-6s%d\n",systemState[i].xTaskNumber, systemState[i].pcTaskName,
|
||||||
systemState[i].usStackHighWaterMark, systemState[i].uxCurrentPriority, core, cpuPercentage);
|
systemState[i].usStackHighWaterMark, systemState[i].uxCurrentPriority, core.c_str(), cpuPercentage);
|
||||||
}
|
}
|
||||||
delete[] systemState;
|
delete[] systemState;
|
||||||
#endif
|
#endif
|
||||||
} else if (command == "reset") {
|
} else if (command == "reset") {
|
||||||
attitude = Quaternion();
|
attitude = Quaternion();
|
||||||
|
gyroBiasFilter.reset();
|
||||||
} else if (command == "reboot") {
|
} else if (command == "reboot") {
|
||||||
ESP.restart();
|
ESP.restart();
|
||||||
} else {
|
} else {
|
||||||
@@ -196,7 +210,7 @@ void handleInput() {
|
|||||||
|
|
||||||
while (Serial.available()) {
|
while (Serial.available()) {
|
||||||
char c = Serial.read();
|
char c = Serial.read();
|
||||||
if (c == '\n') {
|
if (c == '\n' || c == '\r') {
|
||||||
doCommand(input);
|
doCommand(input);
|
||||||
input.clear();
|
input.clear();
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -52,6 +52,7 @@ PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
|
|||||||
PID yawPID(YAW_P, 0, 0);
|
PID yawPID(YAW_P, 0, 0);
|
||||||
Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
|
Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
|
||||||
float tiltMax = TILT_MAX;
|
float tiltMax = TILT_MAX;
|
||||||
|
int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch
|
||||||
|
|
||||||
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
||||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||||
@@ -65,9 +66,9 @@ void control() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void interpretControls() {
|
void interpretControls() {
|
||||||
if (controlMode < 0.25) mode = STAB;
|
if (controlMode < 0.25) mode = flightModes[0];
|
||||||
if (controlMode < 0.75) mode = STAB;
|
else if (controlMode <= 0.75) mode = flightModes[1];
|
||||||
if (controlMode > 0.75) mode = STAB;
|
else if (controlMode > 0.75) mode = flightModes[2];
|
||||||
|
|
||||||
if (mode == AUTO) return; // pilot is not effective in AUTO mode
|
if (mode == AUTO) return; // pilot is not effective in AUTO mode
|
||||||
|
|
||||||
@@ -148,12 +149,26 @@ void controlTorque() {
|
|||||||
motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
|
motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
|
||||||
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
|
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
|
||||||
|
|
||||||
|
// Prioritize angle control over thrust control
|
||||||
|
desaturate(motors[MOTOR_FRONT_LEFT], motors[MOTOR_FRONT_RIGHT], motors[MOTOR_REAR_LEFT], motors[MOTOR_REAR_RIGHT]);
|
||||||
|
|
||||||
motors[0] = constrain(motors[0], 0, 1);
|
motors[0] = constrain(motors[0], 0, 1);
|
||||||
motors[1] = constrain(motors[1], 0, 1);
|
motors[1] = constrain(motors[1], 0, 1);
|
||||||
motors[2] = constrain(motors[2], 0, 1);
|
motors[2] = constrain(motors[2], 0, 1);
|
||||||
motors[3] = constrain(motors[3], 0, 1);
|
motors[3] = constrain(motors[3], 0, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void desaturate(float& a, float& b, float& c, float& d) {
|
||||||
|
float maxThrust = max(max(a, b), max(c, d));
|
||||||
|
if (maxThrust > 1) {
|
||||||
|
float diff = maxThrust - 1;
|
||||||
|
a -= diff;
|
||||||
|
b -= diff;
|
||||||
|
c -= diff;
|
||||||
|
d -= diff;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
const char* getModeName() {
|
const char* getModeName() {
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case RAW: return "RAW";
|
case RAW: return "RAW";
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
// Attitude estimation from gyro and accelerometer
|
// Attitude estimation using gyro and accelerometer
|
||||||
|
|
||||||
#include "quaternion.h"
|
#include "quaternion.h"
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
@@ -13,11 +13,13 @@ Quaternion attitude; // estimated attitude
|
|||||||
bool landed;
|
bool landed;
|
||||||
|
|
||||||
float accWeight = 0.003;
|
float accWeight = 0.003;
|
||||||
|
float levelWeight = 0.0002;
|
||||||
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
||||||
|
|
||||||
void estimate() {
|
void estimate() {
|
||||||
applyGyro();
|
applyGyro();
|
||||||
applyAcc();
|
applyAcc();
|
||||||
|
applyLevel();
|
||||||
}
|
}
|
||||||
|
|
||||||
void applyGyro() {
|
void applyGyro() {
|
||||||
@@ -30,8 +32,7 @@ void applyGyro() {
|
|||||||
|
|
||||||
void applyAcc() {
|
void applyAcc() {
|
||||||
// test should we apply accelerometer gravity correction
|
// test should we apply accelerometer gravity correction
|
||||||
float accNorm = acc.norm();
|
landed = !motorsActive() && abs(acc.norm() - ONE_G) < ONE_G * 0.1f;
|
||||||
landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
|
|
||||||
|
|
||||||
if (!landed) return;
|
if (!landed) return;
|
||||||
|
|
||||||
@@ -42,3 +43,13 @@ void applyAcc() {
|
|||||||
// apply correction
|
// apply correction
|
||||||
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void applyLevel() {
|
||||||
|
if (landed) return;
|
||||||
|
if (thrustTarget < 0.1) return; // skip at idle thrust
|
||||||
|
|
||||||
|
// assume the pilot keeps the drone more or less level in flight
|
||||||
|
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
|
||||||
|
Vector correction = Vector::rotationVectorBetween(Vector(0, 0, 1), up) * levelWeight;
|
||||||
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
||||||
|
}
|
||||||
|
|||||||
@@ -7,8 +7,6 @@
|
|||||||
#include "quaternion.h"
|
#include "quaternion.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
#define WIFI_ENABLED 1
|
|
||||||
|
|
||||||
extern float t, dt;
|
extern float t, dt;
|
||||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||||
extern Vector gyro, acc;
|
extern Vector gyro, acc;
|
||||||
@@ -20,14 +18,12 @@ extern float motors[4];
|
|||||||
void setup() {
|
void setup() {
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
print("Initializing flix\n");
|
print("Initializing flix\n");
|
||||||
disableBrownOut();
|
|
||||||
setupParameters();
|
setupParameters();
|
||||||
|
setupPower();
|
||||||
setupLED();
|
setupLED();
|
||||||
setupMotors();
|
|
||||||
setLED(true);
|
setLED(true);
|
||||||
#if WIFI_ENABLED
|
setupMotors();
|
||||||
setupWiFi();
|
setupWiFi();
|
||||||
#endif
|
|
||||||
setupIMU();
|
setupIMU();
|
||||||
setupRC();
|
setupRC();
|
||||||
setLED(false);
|
setLED(false);
|
||||||
@@ -42,9 +38,8 @@ void loop() {
|
|||||||
control();
|
control();
|
||||||
sendMotors();
|
sendMotors();
|
||||||
handleInput();
|
handleInput();
|
||||||
#if WIFI_ENABLED
|
|
||||||
processMavlink();
|
processMavlink();
|
||||||
#endif
|
readVoltage();
|
||||||
logData();
|
logData();
|
||||||
syncParameters();
|
syncParameters();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -10,7 +10,7 @@
|
|||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
MPU9250 imu(SPI);
|
MPU9250 imu(SPI);
|
||||||
Vector imuRotation(0, 0, -PI / 2); // imu orientation as Euler angles
|
Vector imuRotation(0, 0, PI / 2); // imu orientation as Euler angles
|
||||||
|
|
||||||
Vector gyro; // gyroscope output, rad/s
|
Vector gyro; // gyroscope output, rad/s
|
||||||
Vector gyroBias;
|
Vector gyroBias;
|
||||||
@@ -19,6 +19,8 @@ Vector acc; // accelerometer output, m/s/s
|
|||||||
Vector accBias;
|
Vector accBias;
|
||||||
Vector accScale(1, 1, 1);
|
Vector accScale(1, 1, 1);
|
||||||
|
|
||||||
|
LowPassFilter<Vector> gyroBiasFilter(0.001);
|
||||||
|
|
||||||
void setupIMU() {
|
void setupIMU() {
|
||||||
print("Setup IMU\n");
|
print("Setup IMU\n");
|
||||||
imu.begin();
|
imu.begin();
|
||||||
@@ -38,10 +40,12 @@ void readIMU() {
|
|||||||
imu.getGyro(gyro.x, gyro.y, gyro.z);
|
imu.getGyro(gyro.x, gyro.y, gyro.z);
|
||||||
imu.getAccel(acc.x, acc.y, acc.z);
|
imu.getAccel(acc.x, acc.y, acc.z);
|
||||||
calibrateGyroOnce();
|
calibrateGyroOnce();
|
||||||
// apply scale and bias
|
|
||||||
|
// Apply scale and bias
|
||||||
acc = (acc - accBias) / accScale;
|
acc = (acc - accBias) / accScale;
|
||||||
gyro = gyro - gyroBias;
|
gyro = gyro - gyroBias;
|
||||||
// rotate to body frame
|
|
||||||
|
// Rotate to body frame
|
||||||
Quaternion rotation = Quaternion::fromEuler(imuRotation);
|
Quaternion rotation = Quaternion::fromEuler(imuRotation);
|
||||||
acc = Quaternion::rotateVector(acc, rotation.inversed());
|
acc = Quaternion::rotateVector(acc, rotation.inversed());
|
||||||
gyro = Quaternion::rotateVector(gyro, rotation.inversed());
|
gyro = Quaternion::rotateVector(gyro, rotation.inversed());
|
||||||
@@ -51,7 +55,6 @@ void calibrateGyroOnce() {
|
|||||||
static Delay landedDelay(2);
|
static Delay landedDelay(2);
|
||||||
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
|
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
|
||||||
|
|
||||||
static LowPassFilter<Vector> gyroBiasFilter(0.001);
|
|
||||||
gyroBias = gyroBiasFilter.update(gyro);
|
gyroBias = gyroBiasFilter.update(gyro);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -105,6 +108,7 @@ void calibrateAccelOnce() {
|
|||||||
if (acc.x < accMin.x) accMin.x = acc.x;
|
if (acc.x < accMin.x) accMin.x = acc.x;
|
||||||
if (acc.y < accMin.y) accMin.y = acc.y;
|
if (acc.y < accMin.y) accMin.y = acc.y;
|
||||||
if (acc.z < accMin.z) accMin.z = acc.z;
|
if (acc.z < accMin.z) accMin.z = acc.z;
|
||||||
|
|
||||||
// Compute scale and bias
|
// Compute scale and bias
|
||||||
accScale = (accMax - accMin) / 2 / ONE_G;
|
accScale = (accMax - accMin) / 2 / ONE_G;
|
||||||
accBias = (accMax + accMin) / 2;
|
accBias = (accMax + accMin) / 2;
|
||||||
@@ -121,7 +125,8 @@ void printIMUInfo() {
|
|||||||
print("model: %s\n", imu.getModel());
|
print("model: %s\n", imu.getModel());
|
||||||
print("who am I: 0x%02X\n", imu.whoAmI());
|
print("who am I: 0x%02X\n", imu.whoAmI());
|
||||||
print("rate: %.0f\n", loopRate);
|
print("rate: %.0f\n", loopRate);
|
||||||
print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
|
print("temperature: %.1f °C\n", imu.getTemp());
|
||||||
|
print("gyro: %f %f %f\n", gyro.x, gyro.y, gyro.z);
|
||||||
print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
|
print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
|
||||||
imu.waitForData();
|
imu.waitForData();
|
||||||
Vector rawGyro, rawAcc;
|
Vector rawGyro, rawAcc;
|
||||||
|
|||||||
@@ -14,15 +14,10 @@ public:
|
|||||||
LowPassFilter(float alpha): alpha(alpha) {};
|
LowPassFilter(float alpha): alpha(alpha) {};
|
||||||
|
|
||||||
T update(const T input) {
|
T update(const T input) {
|
||||||
if (alpha == 1) { // filter disabled
|
if (!init) {
|
||||||
return input;
|
init = true;
|
||||||
|
return output = input;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!initialized) {
|
|
||||||
output = input;
|
|
||||||
initialized = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
return output += alpha * (input - output);
|
return output += alpha * (input - output);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -31,9 +26,9 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
void reset() {
|
void reset() {
|
||||||
initialized = false;
|
init = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool initialized = false;
|
bool init = false;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -3,16 +3,19 @@
|
|||||||
|
|
||||||
// MAVLink communication
|
// MAVLink communication
|
||||||
|
|
||||||
#if WIFI_ENABLED
|
|
||||||
|
|
||||||
#include <MAVLink.h>
|
#include <MAVLink.h>
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
#define SYSTEM_ID 1
|
|
||||||
#define MAVLINK_RATE_SLOW 1
|
|
||||||
#define MAVLINK_RATE_FAST 10
|
|
||||||
|
|
||||||
extern float controlTime;
|
extern float controlTime;
|
||||||
|
extern float voltage;
|
||||||
|
|
||||||
|
int mavlinkSysId = 1;
|
||||||
|
|
||||||
|
Rate telemetrySlow(2);
|
||||||
|
Rate telemetryAttitude(20);
|
||||||
|
Rate telemetryRC(10);
|
||||||
|
Rate telemetryMotors(10);
|
||||||
|
Rate telemetryIMU(15);
|
||||||
|
|
||||||
bool mavlinkConnected = false;
|
bool mavlinkConnected = false;
|
||||||
String mavlinkPrintBuffer;
|
String mavlinkPrintBuffer;
|
||||||
@@ -28,40 +31,55 @@ void sendMavlink() {
|
|||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
uint32_t time = t * 1000;
|
uint32_t time = t * 1000;
|
||||||
|
|
||||||
static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST);
|
if (telemetrySlow) {
|
||||||
|
mavlink_msg_heartbeat_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
|
||||||
if (slow) {
|
|
||||||
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
|
|
||||||
(armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) |
|
(armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) |
|
||||||
((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
|
((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
|
||||||
((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
|
((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
|
||||||
mode, MAV_STATE_STANDBY);
|
mode, MAV_STATE_STANDBY);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
|
}
|
||||||
|
|
||||||
if (!mavlinkConnected) return; // send only heartbeat until connected
|
if (!mavlinkConnected) return; // send only heartbeat until connected
|
||||||
|
|
||||||
mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
if (telemetrySlow) {
|
||||||
|
mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
|
MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fast && mavlinkConnected) {
|
if (telemetrySlow && valid(voltage)) {
|
||||||
const float zeroQuat[] = {0, 0, 0, 0};
|
uint16_t voltages[] = {(uint16_t)(voltage * 1000), UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX};
|
||||||
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
uint16_t voltagesExt[] = {0, 0, 0, 0};
|
||||||
time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
|
float remaining = constrain(mapf(voltage, 3.4, 4.2, 0, 1), 0, 1);
|
||||||
|
mavlink_msg_battery_status_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, MAV_BATTERY_FUNCTION_ALL,
|
||||||
|
MAV_BATTERY_TYPE_LIPO, INT16_MAX, voltages, -1, -1, -1, remaining * 100, 0, MAV_BATTERY_CHARGE_STATE_OK, voltagesExt, 0, 0);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
|
}
|
||||||
|
|
||||||
mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
|
if (telemetryAttitude) {
|
||||||
|
const float offset[] = {0, 0, 0, 0};
|
||||||
|
mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
|
time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, offset); // convert to frd
|
||||||
|
sendMessage(&msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (telemetryRC && channels[0]) { // 0 means no RC input
|
||||||
|
mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
|
||||||
channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
|
channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
|
||||||
if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
|
sendMessage(&msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (telemetryMotors) {
|
||||||
float controls[8];
|
float controls[8];
|
||||||
memcpy(controls, motors, sizeof(motors));
|
memcpy(controls, motors, sizeof(motors));
|
||||||
mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
|
mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
|
}
|
||||||
|
|
||||||
mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
|
if (telemetryIMU) {
|
||||||
acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd
|
mavlink_msg_scaled_imu_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time,
|
||||||
|
acc.x / ONE_G * 1000, -acc.y / ONE_G * 1000, -acc.z / ONE_G * 1000, // convert to frd
|
||||||
gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
|
gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
|
||||||
0, 0, 0, 0);
|
0, 0, 0, 0);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
@@ -77,13 +95,13 @@ void sendMessage(const void *msg) {
|
|||||||
void receiveMavlink() {
|
void receiveMavlink() {
|
||||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||||
int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);
|
int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);
|
||||||
if (len) mavlinkConnected = true;
|
|
||||||
|
|
||||||
// New packet, parse it
|
// New packet, parse it
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_status_t status;
|
mavlink_status_t status;
|
||||||
for (int i = 0; i < len; i++) {
|
for (int i = 0; i < len; i++) {
|
||||||
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
|
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
|
||||||
|
mavlinkConnected = true;
|
||||||
handleMavlink(&msg);
|
handleMavlink(&msg);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -95,7 +113,7 @@ void handleMavlink(const void *_msg) {
|
|||||||
if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
|
if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
|
||||||
mavlink_manual_control_t m;
|
mavlink_manual_control_t m;
|
||||||
mavlink_msg_manual_control_decode(&msg, &m);
|
mavlink_msg_manual_control_decode(&msg, &m);
|
||||||
if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast
|
if (m.target && m.target != mavlinkSysId) return; // 0 is broadcast
|
||||||
|
|
||||||
controlThrottle = m.z / 1000.0f;
|
controlThrottle = m.z / 1000.0f;
|
||||||
controlPitch = m.x / 1000.0f;
|
controlPitch = m.x / 1000.0f;
|
||||||
@@ -108,11 +126,11 @@ void handleMavlink(const void *_msg) {
|
|||||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
||||||
mavlink_param_request_list_t m;
|
mavlink_param_request_list_t m;
|
||||||
mavlink_msg_param_request_list_decode(&msg, &m);
|
mavlink_msg_param_request_list_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
for (int i = 0; i < parametersCount(); i++) {
|
for (int i = 0; i < parametersCount(); i++) {
|
||||||
mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
|
getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
@@ -121,7 +139,7 @@ void handleMavlink(const void *_msg) {
|
|||||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
|
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
|
||||||
mavlink_param_request_read_t m;
|
mavlink_param_request_read_t m;
|
||||||
mavlink_msg_param_request_read_decode(&msg, &m);
|
mavlink_msg_param_request_read_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
|
char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
|
||||||
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
|
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
|
||||||
@@ -130,7 +148,7 @@ void handleMavlink(const void *_msg) {
|
|||||||
memcpy(name, getParameterName(m.param_index), 16);
|
memcpy(name, getParameterName(m.param_index), 16);
|
||||||
}
|
}
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
|
name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
@@ -138,32 +156,33 @@ void handleMavlink(const void *_msg) {
|
|||||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
|
if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
|
||||||
mavlink_param_set_t m;
|
mavlink_param_set_t m;
|
||||||
mavlink_msg_param_set_decode(&msg, &m);
|
mavlink_msg_param_set_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
|
char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
|
||||||
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
|
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
|
||||||
setParameter(name, m.param_value);
|
bool success = setParameter(name, m.param_value);
|
||||||
|
if (!success) return;
|
||||||
// send ack
|
// send ack
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
|
m.param_id, getParameter(name), MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
|
if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
|
||||||
mavlink_mission_request_list_t m;
|
mavlink_mission_request_list_t m;
|
||||||
mavlink_msg_mission_request_list_decode(&msg, &m);
|
mavlink_msg_mission_request_list_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
|
mavlink_msg_mission_count_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
|
if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
|
||||||
mavlink_serial_control_t m;
|
mavlink_serial_control_t m;
|
||||||
mavlink_msg_serial_control_decode(&msg, &m);
|
mavlink_msg_serial_control_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
|
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
|
||||||
strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated
|
strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated
|
||||||
@@ -175,7 +194,7 @@ void handleMavlink(const void *_msg) {
|
|||||||
|
|
||||||
mavlink_set_attitude_target_t m;
|
mavlink_set_attitude_target_t m;
|
||||||
mavlink_msg_set_attitude_target_decode(&msg, &m);
|
mavlink_msg_set_attitude_target_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
// copy attitude, rates and thrust targets
|
// copy attitude, rates and thrust targets
|
||||||
ratesTarget.x = m.body_roll_rate;
|
ratesTarget.x = m.body_roll_rate;
|
||||||
@@ -197,7 +216,7 @@ void handleMavlink(const void *_msg) {
|
|||||||
|
|
||||||
mavlink_set_actuator_control_target_t m;
|
mavlink_set_actuator_control_target_t m;
|
||||||
mavlink_msg_set_actuator_control_target_decode(&msg, &m);
|
mavlink_msg_set_actuator_control_target_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
attitudeTarget.invalidate();
|
attitudeTarget.invalidate();
|
||||||
ratesTarget.invalidate();
|
ratesTarget.invalidate();
|
||||||
@@ -209,12 +228,12 @@ void handleMavlink(const void *_msg) {
|
|||||||
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
|
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
|
||||||
mavlink_log_request_data_t m;
|
mavlink_log_request_data_t m;
|
||||||
mavlink_msg_log_request_data_decode(&msg, &m);
|
mavlink_msg_log_request_data_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
|
|
||||||
// Send all log records
|
// Send all log records
|
||||||
for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
|
for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_msg_log_data_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
|
mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
|
||||||
sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
|
sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
@@ -224,19 +243,19 @@ void handleMavlink(const void *_msg) {
|
|||||||
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
||||||
mavlink_command_long_t m;
|
mavlink_command_long_t m;
|
||||||
mavlink_msg_command_long_decode(&msg, &m);
|
mavlink_msg_command_long_decode(&msg, &m);
|
||||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
if (m.target_system && m.target_system != mavlinkSysId) return;
|
||||||
mavlink_message_t response;
|
mavlink_message_t response;
|
||||||
bool accepted = false;
|
bool accepted = false;
|
||||||
|
|
||||||
if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
|
if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
|
||||||
accepted = true;
|
accepted = true;
|
||||||
mavlink_msg_autopilot_version_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &response,
|
mavlink_msg_autopilot_version_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
|
||||||
MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
|
MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
|
||||||
sendMessage(&response);
|
sendMessage(&response);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
|
if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
|
||||||
if (m.param1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
|
if (m.param1 == 1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
|
||||||
accepted = true;
|
accepted = true;
|
||||||
armed = m.param1 == 1;
|
armed = m.param1 == 1;
|
||||||
}
|
}
|
||||||
@@ -249,7 +268,7 @@ void handleMavlink(const void *_msg) {
|
|||||||
|
|
||||||
// send command ack
|
// send command ack
|
||||||
mavlink_message_t ack;
|
mavlink_message_t ack;
|
||||||
mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
|
mavlink_msg_command_ack_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
|
||||||
sendMessage(&ack);
|
sendMessage(&ack);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -266,7 +285,7 @@ void sendMavlinkPrint() {
|
|||||||
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
|
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
|
||||||
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(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
SERIAL_CONTROL_DEV_SHELL,
|
SERIAL_CONTROL_DEV_SHELL,
|
||||||
i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go
|
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);
|
0, 0, strlen(data), (uint8_t *)data, 0, 0);
|
||||||
@@ -274,5 +293,3 @@ void sendMavlinkPrint() {
|
|||||||
}
|
}
|
||||||
mavlinkPrintBuffer.clear();
|
mavlinkPrintBuffer.clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|||||||
@@ -1,55 +1,51 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
// Motors output control using MOSFETs
|
// PWM control for motors
|
||||||
// In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
|
|
||||||
|
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
#define MOTOR_0_PIN 12 // rear left
|
|
||||||
#define MOTOR_1_PIN 13 // rear right
|
|
||||||
#define MOTOR_2_PIN 14 // front right
|
|
||||||
#define MOTOR_3_PIN 15 // front left
|
|
||||||
|
|
||||||
#define PWM_FREQUENCY 78000
|
|
||||||
#define PWM_RESOLUTION 10
|
|
||||||
#define PWM_STOP 0
|
|
||||||
#define PWM_MIN 0
|
|
||||||
#define PWM_MAX 1000000 / PWM_FREQUENCY
|
|
||||||
|
|
||||||
float motors[4]; // normalized motor thrusts in range [0..1]
|
float motors[4]; // normalized motor thrusts in range [0..1]
|
||||||
|
|
||||||
const int MOTOR_REAR_LEFT = 0;
|
int motorPins[4] = {12, 13, 14, 15}; // default pin numbers
|
||||||
const int MOTOR_REAR_RIGHT = 1;
|
int pwmFrequency = 78000;
|
||||||
const int MOTOR_FRONT_RIGHT = 2;
|
int pwmResolution = 10;
|
||||||
const int MOTOR_FRONT_LEFT = 3;
|
int pwmStop = 0;
|
||||||
|
int pwmMin = 0;
|
||||||
|
int pwmMax = -1; // -1 means duty cycle mode
|
||||||
|
|
||||||
|
const int MOTOR_REAR_LEFT = 0, MOTOR_REAR_RIGHT = 1, MOTOR_FRONT_RIGHT = 2, MOTOR_FRONT_LEFT = 3;
|
||||||
|
|
||||||
void setupMotors() {
|
void setupMotors() {
|
||||||
print("Setup Motors\n");
|
print("Setup Motors\n");
|
||||||
|
// Configure pins
|
||||||
// configure pins
|
for (int i = 0; i < 4; i++) {
|
||||||
ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
|
if (motorPins[i] < 0) continue; // skip unassigned motors
|
||||||
ledcAttach(MOTOR_1_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
|
ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
|
||||||
ledcAttach(MOTOR_2_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
|
pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // when reconfiguring
|
||||||
ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
|
}
|
||||||
|
|
||||||
sendMotors();
|
sendMotors();
|
||||||
print("Motors initialized\n");
|
print("Motors initialized\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
int getDutyCycle(float value) {
|
void sendMotors() {
|
||||||
value = constrain(value, 0, 1);
|
for (int i = 0; i < 4; i++) {
|
||||||
float pwm = mapf(value, 0, 1, PWM_MIN, PWM_MAX);
|
if (motorPins[i] < 0) continue; // skip unassigned motors
|
||||||
if (value == 0) pwm = PWM_STOP;
|
ledcWrite(motorPins[i], getDutyCycle(motors[i]));
|
||||||
float duty = mapf(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
|
}
|
||||||
return round(duty);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void sendMotors() {
|
int getDutyCycle(float value) {
|
||||||
ledcWrite(MOTOR_0_PIN, getDutyCycle(motors[0]));
|
value = constrain(value, 0, 1);
|
||||||
ledcWrite(MOTOR_1_PIN, getDutyCycle(motors[1]));
|
|
||||||
ledcWrite(MOTOR_2_PIN, getDutyCycle(motors[2]));
|
if (pwmMax >= 0) { // pwm mode
|
||||||
ledcWrite(MOTOR_3_PIN, getDutyCycle(motors[3]));
|
float pwm = mapf(value, 0, 1, pwmMin, pwmMax);
|
||||||
|
if (value == 0) pwm = pwmStop;
|
||||||
|
float duty = mapf(pwm, 0, 1000000 / pwmFrequency, 0, (1 << pwmResolution) - 1);
|
||||||
|
return round(duty);
|
||||||
|
} else { // duty cycle mode
|
||||||
|
return round(value * ((1 << pwmResolution) - 1));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool motorsActive() {
|
bool motorsActive() {
|
||||||
@@ -58,7 +54,7 @@ bool motorsActive() {
|
|||||||
|
|
||||||
void testMotor(int n) {
|
void testMotor(int n) {
|
||||||
print("Testing motor %d\n", n);
|
print("Testing motor %d\n", n);
|
||||||
motors[n] = 1;
|
motors[n] = 0.2;
|
||||||
delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306
|
delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306
|
||||||
sendMotors();
|
sendMotors();
|
||||||
pause(3);
|
pause(3);
|
||||||
|
|||||||
@@ -6,16 +6,27 @@
|
|||||||
#include <Preferences.h>
|
#include <Preferences.h>
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
extern float channelZero[16];
|
extern int channelZero[16], channelMax[16];
|
||||||
extern float channelMax[16];
|
extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
|
||||||
extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
|
extern int rcRxPin, voltagePin;
|
||||||
|
extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel;
|
||||||
|
extern float rcLossTimeout, descendTime;
|
||||||
|
extern float voltageScale;
|
||||||
|
extern LowPassFilter<float> voltageFilter;
|
||||||
|
|
||||||
Preferences storage;
|
Preferences storage;
|
||||||
|
|
||||||
struct Parameter {
|
struct Parameter {
|
||||||
const char *name; // max length is 15 (Preferences key limit)
|
const char *name; // max length is 15
|
||||||
float *variable;
|
bool integer;
|
||||||
float value; // cache
|
union { float *f; int *i; }; // pointer to the variable
|
||||||
|
float inital; // default value
|
||||||
|
float cache; // what's stored in flash
|
||||||
|
void (*callback)(); // called after parameter change
|
||||||
|
Parameter(const char *name, float *variable, void (*callback)() = nullptr) : name(name), integer(false), f(variable), callback(callback) {};
|
||||||
|
Parameter(const char *name, int *variable, void (*callback)() = nullptr) : name(name), integer(true), i(variable), callback(callback) {};
|
||||||
|
float getValue() const { return integer ? *i : *f; };
|
||||||
|
void setValue(const float value) { if (integer) *i = value; else *f = value; };
|
||||||
};
|
};
|
||||||
|
|
||||||
Parameter parameters[] = {
|
Parameter parameters[] = {
|
||||||
@@ -24,13 +35,16 @@ Parameter parameters[] = {
|
|||||||
{"CTL_R_RATE_I", &rollRatePID.i},
|
{"CTL_R_RATE_I", &rollRatePID.i},
|
||||||
{"CTL_R_RATE_D", &rollRatePID.d},
|
{"CTL_R_RATE_D", &rollRatePID.d},
|
||||||
{"CTL_R_RATE_WU", &rollRatePID.windup},
|
{"CTL_R_RATE_WU", &rollRatePID.windup},
|
||||||
|
{"CTL_R_RATE_D_A", &rollRatePID.lpf.alpha},
|
||||||
{"CTL_P_RATE_P", &pitchRatePID.p},
|
{"CTL_P_RATE_P", &pitchRatePID.p},
|
||||||
{"CTL_P_RATE_I", &pitchRatePID.i},
|
{"CTL_P_RATE_I", &pitchRatePID.i},
|
||||||
{"CTL_P_RATE_D", &pitchRatePID.d},
|
{"CTL_P_RATE_D", &pitchRatePID.d},
|
||||||
{"CTL_P_RATE_WU", &pitchRatePID.windup},
|
{"CTL_P_RATE_WU", &pitchRatePID.windup},
|
||||||
|
{"CTL_P_RATE_D_A", &pitchRatePID.lpf.alpha},
|
||||||
{"CTL_Y_RATE_P", &yawRatePID.p},
|
{"CTL_Y_RATE_P", &yawRatePID.p},
|
||||||
{"CTL_Y_RATE_I", &yawRatePID.i},
|
{"CTL_Y_RATE_I", &yawRatePID.i},
|
||||||
{"CTL_Y_RATE_D", &yawRatePID.d},
|
{"CTL_Y_RATE_D", &yawRatePID.d},
|
||||||
|
{"CTL_Y_RATE_D_A", &yawRatePID.lpf.alpha},
|
||||||
{"CTL_R_P", &rollPID.p},
|
{"CTL_R_P", &rollPID.p},
|
||||||
{"CTL_R_I", &rollPID.i},
|
{"CTL_R_I", &rollPID.i},
|
||||||
{"CTL_R_D", &rollPID.d},
|
{"CTL_R_D", &rollPID.d},
|
||||||
@@ -42,6 +56,9 @@ Parameter parameters[] = {
|
|||||||
{"CTL_R_RATE_MAX", &maxRate.x},
|
{"CTL_R_RATE_MAX", &maxRate.x},
|
||||||
{"CTL_Y_RATE_MAX", &maxRate.z},
|
{"CTL_Y_RATE_MAX", &maxRate.z},
|
||||||
{"CTL_TILT_MAX", &tiltMax},
|
{"CTL_TILT_MAX", &tiltMax},
|
||||||
|
{"CTL_FLT_MODE_0", &flightModes[0]},
|
||||||
|
{"CTL_FLT_MODE_1", &flightModes[1]},
|
||||||
|
{"CTL_FLT_MODE_2", &flightModes[2]},
|
||||||
// imu
|
// imu
|
||||||
{"IMU_ROT_ROLL", &imuRotation.x},
|
{"IMU_ROT_ROLL", &imuRotation.x},
|
||||||
{"IMU_ROT_PITCH", &imuRotation.y},
|
{"IMU_ROT_PITCH", &imuRotation.y},
|
||||||
@@ -52,10 +69,23 @@ Parameter parameters[] = {
|
|||||||
{"IMU_ACC_SCALE_X", &accScale.x},
|
{"IMU_ACC_SCALE_X", &accScale.x},
|
||||||
{"IMU_ACC_SCALE_Y", &accScale.y},
|
{"IMU_ACC_SCALE_Y", &accScale.y},
|
||||||
{"IMU_ACC_SCALE_Z", &accScale.z},
|
{"IMU_ACC_SCALE_Z", &accScale.z},
|
||||||
|
{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
|
||||||
// estimate
|
// estimate
|
||||||
{"EST_ACC_WEIGHT", &accWeight},
|
{"EST_ACC_WEIGHT", &accWeight},
|
||||||
|
{"EST_LVL_WEIGHT", &levelWeight},
|
||||||
{"EST_RATES_LPF_A", &ratesFilter.alpha},
|
{"EST_RATES_LPF_A", &ratesFilter.alpha},
|
||||||
|
// motors
|
||||||
|
{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},
|
||||||
|
{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},
|
||||||
|
{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT], setupMotors},
|
||||||
|
{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT], setupMotors},
|
||||||
|
{"MOT_PWM_FREQ", &pwmFrequency, setupMotors},
|
||||||
|
{"MOT_PWM_RES", &pwmResolution, setupMotors},
|
||||||
|
{"MOT_PWM_STOP", &pwmStop},
|
||||||
|
{"MOT_PWM_MIN", &pwmMin},
|
||||||
|
{"MOT_PWM_MAX", &pwmMax},
|
||||||
// rc
|
// rc
|
||||||
|
{"RC_RX_PIN", &rcRxPin, setupRC},
|
||||||
{"RC_ZERO_0", &channelZero[0]},
|
{"RC_ZERO_0", &channelZero[0]},
|
||||||
{"RC_ZERO_1", &channelZero[1]},
|
{"RC_ZERO_1", &channelZero[1]},
|
||||||
{"RC_ZERO_2", &channelZero[2]},
|
{"RC_ZERO_2", &channelZero[2]},
|
||||||
@@ -77,17 +107,40 @@ Parameter parameters[] = {
|
|||||||
{"RC_THROTTLE", &throttleChannel},
|
{"RC_THROTTLE", &throttleChannel},
|
||||||
{"RC_YAW", &yawChannel},
|
{"RC_YAW", &yawChannel},
|
||||||
{"RC_MODE", &modeChannel},
|
{"RC_MODE", &modeChannel},
|
||||||
|
// wifi
|
||||||
|
{"WIFI_MODE", &wifiMode},
|
||||||
|
{"WIFI_PORT_LOC", &udpLocalPort},
|
||||||
|
{"WIFI_PORT_REM", &udpRemotePort},
|
||||||
|
{"WIFI_LONG_RANGE", &wifiLongRange},
|
||||||
|
// espnow
|
||||||
|
{"ESPNOW_CHANNEL", &espnowChannel},
|
||||||
|
// mavlink
|
||||||
|
{"MAV_SYS_ID", &mavlinkSysId},
|
||||||
|
{"MAV_RATE_SLOW", &telemetrySlow.rate},
|
||||||
|
{"MAV_RATE_ATT", &telemetryAttitude.rate},
|
||||||
|
{"MAV_RATE_RC", &telemetryRC.rate},
|
||||||
|
{"MAV_RATE_MOT", &telemetryMotors.rate},
|
||||||
|
{"MAV_RATE_IMU", &telemetryIMU.rate},
|
||||||
|
// power
|
||||||
|
{"PWR_VOLT_PIN", &voltagePin, setupPower},
|
||||||
|
{"PWR_VOLT_SCALE", &voltageScale},
|
||||||
|
{"PWR_VOLT_LPF_A", &voltageFilter.alpha},
|
||||||
|
// safety
|
||||||
|
{"SF_RC_LOSS_TIME", &rcLossTimeout},
|
||||||
|
{"SF_DESCEND_TIME", &descendTime},
|
||||||
};
|
};
|
||||||
|
|
||||||
void setupParameters() {
|
void setupParameters() {
|
||||||
storage.begin("flix", false);
|
print("Setup parameters\n");
|
||||||
|
storage.begin("flix");
|
||||||
// Read parameters from storage
|
// Read parameters from storage
|
||||||
for (auto ¶meter : parameters) {
|
for (auto ¶meter : parameters) {
|
||||||
if (!storage.isKey(parameter.name)) {
|
if (!storage.isKey(parameter.name)) {
|
||||||
storage.putFloat(parameter.name, *parameter.variable);
|
storage.putFloat(parameter.name, parameter.getValue()); // store default value
|
||||||
}
|
}
|
||||||
*parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
|
parameter.inital = parameter.getValue();
|
||||||
parameter.value = *parameter.variable;
|
parameter.setValue(storage.getFloat(parameter.name, 0));
|
||||||
|
parameter.cache = parameter.getValue();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -102,13 +155,13 @@ const char *getParameterName(int index) {
|
|||||||
|
|
||||||
float getParameter(int index) {
|
float getParameter(int index) {
|
||||||
if (index < 0 || index >= parametersCount()) return NAN;
|
if (index < 0 || index >= parametersCount()) return NAN;
|
||||||
return *parameters[index].variable;
|
return parameters[index].getValue();
|
||||||
}
|
}
|
||||||
|
|
||||||
float getParameter(const char *name) {
|
float getParameter(const char *name) {
|
||||||
for (auto ¶meter : parameters) {
|
for (auto ¶meter : parameters) {
|
||||||
if (strcmp(parameter.name, name) == 0) {
|
if (strcasecmp(parameter.name, name) == 0) {
|
||||||
return *parameter.variable;
|
return parameter.getValue();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return NAN;
|
return NAN;
|
||||||
@@ -116,8 +169,10 @@ float getParameter(const char *name) {
|
|||||||
|
|
||||||
bool setParameter(const char *name, const float value) {
|
bool setParameter(const char *name, const float value) {
|
||||||
for (auto ¶meter : parameters) {
|
for (auto ¶meter : parameters) {
|
||||||
if (strcmp(parameter.name, name) == 0) {
|
if (strcasecmp(parameter.name, name) == 0) {
|
||||||
*parameter.variable = value;
|
if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
|
||||||
|
parameter.setValue(value);
|
||||||
|
if (parameter.callback) parameter.callback();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -130,16 +185,23 @@ void syncParameters() {
|
|||||||
if (motorsActive()) return; // don't use flash while flying, it may cause a delay
|
if (motorsActive()) return; // don't use flash while flying, it may cause a delay
|
||||||
|
|
||||||
for (auto ¶meter : parameters) {
|
for (auto ¶meter : parameters) {
|
||||||
if (parameter.value == *parameter.variable) continue;
|
if (floatEquals(parameter.getValue(), parameter.cache)) continue; // no change
|
||||||
if (isnan(parameter.value) && isnan(*parameter.variable)) continue; // handle NAN != NAN
|
|
||||||
storage.putFloat(parameter.name, *parameter.variable);
|
storage.putFloat(parameter.name, parameter.getValue());
|
||||||
parameter.value = *parameter.variable;
|
parameter.cache = parameter.getValue(); // update cache
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void printParameters() {
|
void printParameters(const char *filter) {
|
||||||
|
print("Name Value [Default]\n");
|
||||||
for (auto ¶meter : parameters) {
|
for (auto ¶meter : parameters) {
|
||||||
print("%s = %g\n", parameter.name, *parameter.variable);
|
if (strncasecmp(parameter.name, filter, strlen(filter))) continue;
|
||||||
|
|
||||||
|
if (floatEquals(parameter.getValue(), parameter.inital)) { // parameter changed
|
||||||
|
print("%-15s %-13g\n", parameter.name, parameter.getValue());
|
||||||
|
} else {
|
||||||
|
print("%-15s %-13g [%g]\n", parameter.name, parameter.getValue(), parameter.inital);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,29 @@
|
|||||||
|
// Copyright (c) 2026 Oleg Kalachev <okalachev@gmail.com>
|
||||||
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
|
// Power management
|
||||||
|
|
||||||
|
#include <soc/soc.h>
|
||||||
|
#include <soc/rtc_cntl_reg.h>
|
||||||
|
#include "lpf.h"
|
||||||
|
#include "util.h"
|
||||||
|
|
||||||
|
float voltage = NAN;
|
||||||
|
LowPassFilter<float> voltageFilter(0.2);
|
||||||
|
int voltagePin = -1;
|
||||||
|
float voltageScale = 2;
|
||||||
|
|
||||||
|
void setupPower() {
|
||||||
|
REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA); // disable reset on low voltage
|
||||||
|
if (digitalPinToAnalogChannel(voltagePin) == -1) voltagePin = -1; // test ADC pin
|
||||||
|
}
|
||||||
|
|
||||||
|
void readVoltage() {
|
||||||
|
if (voltagePin < 0) return;
|
||||||
|
|
||||||
|
static Rate rate(10);
|
||||||
|
if (!rate) return;
|
||||||
|
|
||||||
|
float v = analogReadMilliVolts(voltagePin) * voltageScale / 1000.0f;
|
||||||
|
voltage = voltageFilter.update(v);
|
||||||
|
}
|
||||||
@@ -6,34 +6,34 @@
|
|||||||
#include <SBUS.h>
|
#include <SBUS.h>
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
SBUS rc(Serial2);
|
SBUS rc(Serial1);
|
||||||
|
int rcRxPin = -1; // -1 means disabled
|
||||||
|
|
||||||
uint16_t channels[16]; // raw rc channels
|
uint16_t channels[16]; // raw rc channels
|
||||||
float channelZero[16]; // calibration zero values
|
int channelZero[16]; // calibration zero values
|
||||||
float channelMax[16]; // calibration max values
|
int channelMax[16]; // calibration max values
|
||||||
|
|
||||||
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
||||||
float controlMode = NAN; //
|
float controlMode = NAN;
|
||||||
float controlTime; // time of the last controls update (0 when no RC)
|
float controlTime = NAN; // time of the last controls update
|
||||||
|
|
||||||
// Channels mapping (using float to store in parameters):
|
int rollChannel = -1, pitchChannel = -1, throttleChannel = -1, yawChannel = -1, modeChannel = -1; // channel mapping
|
||||||
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
|
|
||||||
|
|
||||||
void setupRC() {
|
void setupRC() {
|
||||||
|
if (rcRxPin < 0) return;
|
||||||
print("Setup RC\n");
|
print("Setup RC\n");
|
||||||
rc.begin();
|
rc.begin(rcRxPin);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool readRC() {
|
bool readRC() {
|
||||||
if (rc.read()) {
|
if (rcRxPin < 0) return false;
|
||||||
SBUSData data = rc.data();
|
if (!rc.read()) return false;
|
||||||
for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
|
|
||||||
|
rc.getChannels(channels);
|
||||||
normalizeRC();
|
normalizeRC();
|
||||||
controlTime = t;
|
controlTime = t;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
void normalizeRC() {
|
void normalizeRC() {
|
||||||
float controls[16];
|
float controls[16];
|
||||||
@@ -41,30 +41,35 @@ void normalizeRC() {
|
|||||||
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
|
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
|
||||||
}
|
}
|
||||||
// Update control values
|
// Update control values
|
||||||
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
|
controlRoll = rollChannel < 0 ? 0 : controls[rollChannel];
|
||||||
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
|
controlPitch = pitchChannel < 0 ? 0 : controls[pitchChannel];
|
||||||
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
|
controlYaw = yawChannel < 0 ? 0 : controls[yawChannel];
|
||||||
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
|
controlThrottle = throttleChannel < 0 ? 0 : controls[throttleChannel];
|
||||||
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
|
controlMode = modeChannel < 0 ? NAN : controls[modeChannel]; // mode control is ineffective if not mapped
|
||||||
}
|
}
|
||||||
|
|
||||||
void calibrateRC() {
|
void calibrateRC() {
|
||||||
uint16_t zero[16];
|
if (rcRxPin < 0) {
|
||||||
uint16_t center[16];
|
print("RC_RX_PIN = %d, set the RC pin!\n", rcRxPin);
|
||||||
uint16_t max[16];
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t zero[16]; // for zero positions
|
||||||
|
uint16_t center[16]; // for center positions
|
||||||
|
uint16_t _[16]; // for unused data
|
||||||
print("1/8 Calibrating RC: put all switches to default positions [3 sec]\n");
|
print("1/8 Calibrating RC: put all switches to default positions [3 sec]\n");
|
||||||
pause(3);
|
pause(3);
|
||||||
calibrateRCChannel(NULL, zero, zero, "2/8 Move sticks [3 sec]\n... ...\n... .o.\n.o. ...\n");
|
calibrateRCChannel(NULL, _, zero, "2/8 Move sticks [3 sec]\n... ...\n... .o.\n.o. ...\n");
|
||||||
calibrateRCChannel(NULL, center, center, "3/8 Move sticks [3 sec]\n... ...\n.o. .o.\n... ...\n");
|
calibrateRCChannel(&throttleChannel, zero, _, "3/8 Move sticks [3 sec]\n.o. ...\n... .o.\n... ...\n");
|
||||||
calibrateRCChannel(&throttleChannel, zero, max, "4/8 Move sticks [3 sec]\n.o. ...\n... .o.\n... ...\n");
|
calibrateRCChannel(NULL, _, center, "4/8 Move sticks [3 sec]\n... ...\n.o. .o.\n... ...\n");
|
||||||
calibrateRCChannel(&yawChannel, center, max, "5/8 Move sticks [3 sec]\n... ...\n..o .o.\n... ...\n");
|
calibrateRCChannel(&yawChannel, center, _, "5/8 Move sticks [3 sec]\n... ...\n..o .o.\n... ...\n");
|
||||||
calibrateRCChannel(&pitchChannel, zero, max, "6/8 Move sticks [3 sec]\n... .o.\n... ...\n.o. ...\n");
|
calibrateRCChannel(&pitchChannel, zero, _, "6/8 Move sticks [3 sec]\n... .o.\n... ...\n.o. ...\n");
|
||||||
calibrateRCChannel(&rollChannel, zero, max, "7/8 Move sticks [3 sec]\n... ...\n... ..o\n.o. ...\n");
|
calibrateRCChannel(&rollChannel, zero, _, "7/8 Move sticks [3 sec]\n... ...\n... ..o\n.o. ...\n");
|
||||||
calibrateRCChannel(&modeChannel, zero, max, "8/8 Put mode switch to max [3 sec]\n");
|
calibrateRCChannel(&modeChannel, zero, _, "8/8 Put mode switch to max [3 sec]\n");
|
||||||
printRCCalibration();
|
printRCCalibration();
|
||||||
}
|
}
|
||||||
|
|
||||||
void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {
|
void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const char *str) {
|
||||||
print("%s", str);
|
print("%s", str);
|
||||||
pause(3);
|
pause(3);
|
||||||
for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
|
for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
|
||||||
@@ -85,15 +90,15 @@ void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const
|
|||||||
channelZero[ch] = in[ch];
|
channelZero[ch] = in[ch];
|
||||||
channelMax[ch] = out[ch];
|
channelMax[ch] = out[ch];
|
||||||
} else {
|
} else {
|
||||||
*channel = NAN;
|
*channel = -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void printRCCalibration() {
|
void printRCCalibration() {
|
||||||
print("Control Ch Zero Max\n");
|
print("Control Ch Zero Max\n");
|
||||||
print("Roll %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);
|
print("Roll %-7d%-7d%-7d\n", rollChannel, rollChannel < 0 ? 0 : channelZero[rollChannel], rollChannel < 0 ? 0 : channelMax[rollChannel]);
|
||||||
print("Pitch %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
|
print("Pitch %-7d%-7d%-7d\n", pitchChannel, pitchChannel < 0 ? 0 : channelZero[pitchChannel], pitchChannel < 0 ? 0 : channelMax[pitchChannel]);
|
||||||
print("Yaw %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
|
print("Yaw %-7d%-7d%-7d\n", yawChannel, yawChannel < 0 ? 0 : channelZero[yawChannel], yawChannel < 0 ? 0 : channelMax[yawChannel]);
|
||||||
print("Throttle %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
|
print("Throttle %-7d%-7d%-7d\n", throttleChannel, throttleChannel < 0 ? 0 : channelZero[throttleChannel], throttleChannel < 0 ? 0 : channelMax[throttleChannel]);
|
||||||
print("Mode %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
|
print("Mode %-7d%-7d%-7d\n", modeChannel, modeChannel < 0 ? 0 : channelZero[modeChannel], modeChannel < 0 ? 0 : channelMax[modeChannel]);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -3,12 +3,12 @@
|
|||||||
|
|
||||||
// Fail-safe functions
|
// Fail-safe functions
|
||||||
|
|
||||||
#define RC_LOSS_TIMEOUT 1
|
|
||||||
#define DESCEND_TIME 10
|
|
||||||
|
|
||||||
extern float controlTime;
|
extern float controlTime;
|
||||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
||||||
|
|
||||||
|
float rcLossTimeout = 1;
|
||||||
|
float descendTime = 10;
|
||||||
|
|
||||||
void failsafe() {
|
void failsafe() {
|
||||||
rcLossFailsafe();
|
rcLossFailsafe();
|
||||||
autoFailsafe();
|
autoFailsafe();
|
||||||
@@ -16,9 +16,8 @@ void failsafe() {
|
|||||||
|
|
||||||
// RC loss failsafe
|
// RC loss failsafe
|
||||||
void rcLossFailsafe() {
|
void rcLossFailsafe() {
|
||||||
if (controlTime == 0) return; // no RC at all
|
|
||||||
if (!armed) return;
|
if (!armed) return;
|
||||||
if (t - controlTime > RC_LOSS_TIMEOUT) {
|
if (t - controlTime > rcLossTimeout) {
|
||||||
descend();
|
descend();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -27,7 +26,7 @@ void rcLossFailsafe() {
|
|||||||
void descend() {
|
void descend() {
|
||||||
mode = AUTO;
|
mode = AUTO;
|
||||||
attitudeTarget = Quaternion();
|
attitudeTarget = Quaternion();
|
||||||
thrustTarget -= dt / DESCEND_TIME;
|
thrustTarget -= dt / descendTime;
|
||||||
if (thrustTarget < 0) {
|
if (thrustTarget < 0) {
|
||||||
thrustTarget = 0;
|
thrustTarget = 0;
|
||||||
armed = false;
|
armed = false;
|
||||||
@@ -38,8 +37,8 @@ void descend() {
|
|||||||
void autoFailsafe() {
|
void autoFailsafe() {
|
||||||
static float roll, pitch, yaw, throttle;
|
static float roll, pitch, yaw, throttle;
|
||||||
if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
|
if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
|
||||||
// controls changed
|
// controls changed and mode switch is not configured
|
||||||
if (mode == AUTO) mode = STAB; // regain control by the pilot
|
if (mode == AUTO && invalid(controlMode)) mode = STAB; // regain control by the pilot
|
||||||
}
|
}
|
||||||
roll = controlRoll;
|
roll = controlRoll;
|
||||||
pitch = controlPitch;
|
pitch = controlPitch;
|
||||||
|
|||||||
@@ -6,8 +6,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <soc/soc.h>
|
#include <ESP32_NOW_Serial.h>
|
||||||
#include <soc/rtc_cntl_reg.h>
|
|
||||||
|
|
||||||
const float ONE_G = 9.80665;
|
const float ONE_G = 9.80665;
|
||||||
extern float t;
|
extern float t;
|
||||||
@@ -24,6 +23,12 @@ bool valid(float x) {
|
|||||||
return isfinite(x);
|
return isfinite(x);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool floatEquals(float a, float b, float epsilon = 0) {
|
||||||
|
if (isnan(a) && isnan(b)) return true;
|
||||||
|
if (a == b) return true;
|
||||||
|
return fabsf(a - b) <= epsilon;
|
||||||
|
}
|
||||||
|
|
||||||
// Wrap angle to [-PI, PI)
|
// Wrap angle to [-PI, PI)
|
||||||
float wrapAngle(float angle) {
|
float wrapAngle(float angle) {
|
||||||
angle = fmodf(angle, 2 * PI);
|
angle = fmodf(angle, 2 * PI);
|
||||||
@@ -35,21 +40,30 @@ float wrapAngle(float angle) {
|
|||||||
return angle;
|
return angle;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Disable reset on low voltage
|
|
||||||
void disableBrownOut() {
|
|
||||||
REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Trim and split string by spaces
|
// Trim and split string by spaces
|
||||||
void splitString(String& str, String& token0, String& token1, String& token2) {
|
void splitString(String& str, String& token0, String& token1, String& token2) {
|
||||||
str.trim();
|
str.trim();
|
||||||
|
if (str.isEmpty()) return;
|
||||||
char chars[str.length() + 1];
|
char chars[str.length() + 1];
|
||||||
str.toCharArray(chars, str.length() + 1);
|
str.toCharArray(chars, str.length() + 1);
|
||||||
token0 = strtok(chars, " ");
|
token0 = strtok(chars, " ");
|
||||||
token1 = strtok(NULL, " "); // String(NULL) creates empty string
|
token1 = strtok(NULL, " ");
|
||||||
token2 = strtok(NULL, "");
|
token2 = strtok(NULL, "");
|
||||||
|
if (token1.c_str() == NULL) token1 = "";
|
||||||
|
if (token2.c_str() == NULL) token2 = "";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Simplified ESP-NOW Serial without resends
|
||||||
|
class ESPNOWSerial : public ESP_NOW_Serial_Class {
|
||||||
|
public:
|
||||||
|
int lost = 0;
|
||||||
|
using ESP_NOW_Serial_Class::ESP_NOW_Serial_Class;
|
||||||
|
void onSent(bool success) override {
|
||||||
|
if (!success) lost++;
|
||||||
|
ESP_NOW_Serial_Class::onSent(true); // always report success to avoid resends
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
// Rate limiter
|
// Rate limiter
|
||||||
class Rate {
|
class Rate {
|
||||||
public:
|
public:
|
||||||
@@ -58,6 +72,9 @@ public:
|
|||||||
Rate(float rate) : rate(rate) {}
|
Rate(float rate) : rate(rate) {}
|
||||||
|
|
||||||
operator bool() {
|
operator bool() {
|
||||||
|
if (t == last) {
|
||||||
|
return true; // the same step
|
||||||
|
}
|
||||||
if (t - last >= 1 / rate) {
|
if (t - last >= 1 / rate) {
|
||||||
last = t;
|
last = t;
|
||||||
return true;
|
return true;
|
||||||
|
|||||||
@@ -105,10 +105,23 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
static Vector rotationVectorBetween(const Vector& a, const Vector& b) {
|
static Vector rotationVectorBetween(const Vector& a, const Vector& b) {
|
||||||
|
float an = a.norm();
|
||||||
|
float bn = b.norm();
|
||||||
|
if (an < 1e-6 || bn < 1e-6) {
|
||||||
|
return Vector(0, 0, 0);
|
||||||
|
}
|
||||||
Vector direction = cross(a, b);
|
Vector direction = cross(a, b);
|
||||||
if (direction.zero()) {
|
if (direction.norm() < 1e-6) { // vectors are parallel
|
||||||
// vectors are opposite, return any perpendicular vector
|
if (dot(a, b) > 0) { // same direction
|
||||||
return cross(a, Vector(1, 0, 0));
|
return Vector(0, 0, 0);
|
||||||
|
}
|
||||||
|
// opposite direction
|
||||||
|
Vector perp = cross(a, Vector(1, 0, 0));
|
||||||
|
if (perp.norm() < 1e-6) {
|
||||||
|
perp = cross(a, Vector(0, 1, 0));
|
||||||
|
}
|
||||||
|
perp.normalize();
|
||||||
|
return perp * PI;
|
||||||
}
|
}
|
||||||
direction.normalize();
|
direction.normalize();
|
||||||
float angle = angleBetween(a, b);
|
float angle = angleBetween(a, b);
|
||||||
|
|||||||
@@ -1,49 +1,152 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
// Wi-Fi support
|
// Wi-Fi and ESP-NOW communication
|
||||||
|
|
||||||
#if WIFI_ENABLED
|
|
||||||
|
|
||||||
#include <WiFi.h>
|
#include <WiFi.h>
|
||||||
#include <WiFiAP.h>
|
#include <WiFiAP.h>
|
||||||
#include <WiFiUdp.h>
|
#include <WiFiUdp.h>
|
||||||
|
#include <MacAddress.h>
|
||||||
|
#include <ESP32_NOW_Serial.h>
|
||||||
|
#include <Preferences.h>
|
||||||
|
#include "util.h"
|
||||||
|
|
||||||
#define WIFI_SSID "flix"
|
extern Preferences storage; // use the main preferences storage
|
||||||
#define WIFI_PASSWORD "flixwifi"
|
|
||||||
#define WIFI_UDP_PORT 14550
|
|
||||||
#define WIFI_UDP_REMOTE_PORT 14550
|
|
||||||
#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
|
|
||||||
|
|
||||||
|
const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
|
||||||
|
int wifiMode = W_AP;
|
||||||
|
|
||||||
|
int wifiLongRange = 0;
|
||||||
|
int udpLocalPort = 14550;
|
||||||
|
int udpRemotePort = 14550;
|
||||||
|
IPAddress udpRemoteIP = "255.255.255.255";
|
||||||
WiFiUDP udp;
|
WiFiUDP udp;
|
||||||
|
|
||||||
|
ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
|
||||||
|
ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
|
||||||
|
int espnowChannel = 6;
|
||||||
|
|
||||||
void setupWiFi() {
|
void setupWiFi() {
|
||||||
print("Setup Wi-Fi\n");
|
print("Setup Wi-Fi\n");
|
||||||
WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
|
WiFi.enableLongRange(wifiLongRange);
|
||||||
udp.begin(WIFI_UDP_PORT);
|
|
||||||
|
if (wifiMode == W_AP) {
|
||||||
|
WiFi.softAP(storage.getString("WIFI_AP_SSID", "flix").c_str(), storage.getString("WIFI_AP_PASS", "flixwifi").c_str());
|
||||||
|
udp.begin(udpLocalPort);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wifiMode == W_STA) {
|
||||||
|
WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str());
|
||||||
|
udp.begin(udpLocalPort);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wifiMode == W_ESPNOW) {
|
||||||
|
WiFi.mode(WIFI_AP);
|
||||||
|
WiFi.setChannel(espnowChannel);
|
||||||
|
espnow.addr(MacAddress(storage.getString("ESPNOW_PEER_MAC", "FF:FF:FF:FF:FF:FF").c_str()));
|
||||||
|
String key = storage.getString("ESPNOW_PEER_KEY", "");
|
||||||
|
espnow.setKey(key.isEmpty() ? nullptr : (const uint8_t *)key.c_str());
|
||||||
|
espnow.begin();
|
||||||
|
espnowBroadcast.begin();
|
||||||
|
}
|
||||||
|
|
||||||
|
WiFi.setSleep(false); // disable power save
|
||||||
}
|
}
|
||||||
|
|
||||||
void sendWiFi(const uint8_t *buf, int len) {
|
void sendWiFi(const uint8_t *buf, int len) {
|
||||||
if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
|
if (espnow) {
|
||||||
udp.beginPacket(udp.remoteIP() ? udp.remoteIP() : WIFI_UDP_REMOTE_ADDR, WIFI_UDP_REMOTE_PORT);
|
espnow.write(buf, len);
|
||||||
|
|
||||||
|
static Rate discovery(2);
|
||||||
|
if (discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return;
|
||||||
|
|
||||||
|
udp.beginPacket(udpRemoteIP, udpRemotePort);
|
||||||
udp.write(buf, len);
|
udp.write(buf, len);
|
||||||
udp.endPacket();
|
udp.endPacket();
|
||||||
}
|
}
|
||||||
|
|
||||||
int receiveWiFi(uint8_t *buf, int len) {
|
int receiveWiFi(uint8_t *buf, int len) {
|
||||||
|
if (espnow) {
|
||||||
|
return espnow.read(buf, len);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return 0;
|
||||||
|
|
||||||
udp.parsePacket();
|
udp.parsePacket();
|
||||||
|
if (udp.remoteIP()) udpRemoteIP = udp.remoteIP();
|
||||||
return udp.read(buf, len);
|
return udp.read(buf, len);
|
||||||
}
|
}
|
||||||
|
|
||||||
void printWiFiInfo() {
|
void printWiFiInfo() {
|
||||||
|
if (espnow) {
|
||||||
|
print("Mode: ESP-NOW\n");
|
||||||
|
print("ESP-NOW version: %d\n", ESP_NOW.getVersion());
|
||||||
|
print("Max packet size: %d\n", ESP_NOW.getMaxDataLen());
|
||||||
|
print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
|
||||||
|
print("Peer MAC: %s\n", MacAddress(espnow.addr()).toString().c_str());
|
||||||
|
print("Encrypted: %d\n", espnow.isEncrypted());
|
||||||
|
print("Channel: %d\n", espnow.getChannel());
|
||||||
|
print("Lost packets: %d\n", espnow.lost);
|
||||||
|
} else if (WiFi.getMode() == WIFI_MODE_AP) {
|
||||||
|
print("Mode: Access Point (AP)\n");
|
||||||
print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
|
print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
|
||||||
print("SSID: %s\n", WiFi.softAPSSID().c_str());
|
print("SSID: %s\n", WiFi.softAPSSID().c_str());
|
||||||
print("Password: %s\n", WIFI_PASSWORD);
|
print("Password: ***\n");
|
||||||
|
print("Channel: %d\n", WiFi.channel());
|
||||||
print("Clients: %d\n", WiFi.softAPgetStationNum());
|
print("Clients: %d\n", WiFi.softAPgetStationNum());
|
||||||
print("Status: %d\n", WiFi.status());
|
|
||||||
print("IP: %s\n", WiFi.softAPIP().toString().c_str());
|
print("IP: %s\n", WiFi.softAPIP().toString().c_str());
|
||||||
print("Remote IP: %s\n", udp.remoteIP().toString().c_str());
|
print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
|
||||||
|
} else if (WiFi.getMode() == WIFI_MODE_STA) {
|
||||||
|
print("Mode: Client (STA)\n");
|
||||||
|
print("Connected: %d\n", WiFi.isConnected());
|
||||||
|
print("MAC: %s\n", WiFi.macAddress().c_str());
|
||||||
|
print("SSID: %s\n", WiFi.SSID().c_str());
|
||||||
|
print("Password: ***\n");
|
||||||
|
print("Channel: %d\n", WiFi.channel());
|
||||||
|
print("RSSI: %d dBm\n", WiFi.RSSI());
|
||||||
|
print("IP: %s\n", WiFi.localIP().toString().c_str());
|
||||||
|
print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
|
||||||
|
} else {
|
||||||
|
print("Mode: Disabled\n");
|
||||||
|
}
|
||||||
print("MAVLink connected: %d\n", mavlinkConnected);
|
print("MAVLink connected: %d\n", mavlinkConnected);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
void configWiFi(int mode, const char *first, const char *second) {
|
||||||
|
MacAddress mac;
|
||||||
|
if (mode == W_AP && strlen(first) > 0 && strlen(second) >= 8) {
|
||||||
|
storage.putString("WIFI_AP_SSID", first);
|
||||||
|
storage.putString("WIFI_AP_PASS", second);
|
||||||
|
} else if (mode == W_STA && strlen(first) > 0 && strlen(second) >= 8) {
|
||||||
|
storage.putString("WIFI_STA_SSID", first);
|
||||||
|
storage.putString("WIFI_STA_PASS", second);
|
||||||
|
} else if (mode == W_ESPNOW && mac.fromString(first)) {
|
||||||
|
storage.putString("ESPNOW_PEER_MAC", first);
|
||||||
|
storage.putString("ESPNOW_PEER_KEY", strlen(second) == ESP_NOW_KEY_LEN ? second : "");
|
||||||
|
} else {
|
||||||
|
print("Invalid configuration\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
print("✓ Reboot to apply new settings\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
void setWiFiMode(const String& mode) {
|
||||||
|
if (mode == "ap") {
|
||||||
|
wifiMode = W_AP;
|
||||||
|
} else if (mode == "sta") {
|
||||||
|
wifiMode = W_STA;
|
||||||
|
} else if (mode == "espnow") {
|
||||||
|
wifiMode = W_ESPNOW;
|
||||||
|
} else if (mode == "off") {
|
||||||
|
wifiMode = W_DISABLED;
|
||||||
|
} else {
|
||||||
|
print("Invalid Wi-Fi mode\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
static const char *modes[] = {"Disabled", "Access Point (AP)", "Client (STA)", "ESP-NOW"};
|
||||||
|
print("✓ Wi-Fi mode set to %s, reboot to apply\n", modes[wifiMode]);
|
||||||
|
}
|
||||||
|
|||||||
@@ -21,6 +21,8 @@
|
|||||||
#define degrees(rad) ((rad)*RAD_TO_DEG)
|
#define degrees(rad) ((rad)*RAD_TO_DEG)
|
||||||
|
|
||||||
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
||||||
|
template<typename T> T max(T a, T b) { return a > b ? a : b; }
|
||||||
|
template<typename T> T min(T a, T b) { return a < b ? a : b; }
|
||||||
|
|
||||||
long map(long x, long in_min, long in_max, long out_min, long out_max) {
|
long map(long x, long in_min, long in_max, long out_min, long out_max) {
|
||||||
const long run = in_max - in_min;
|
const long run = in_max - in_min;
|
||||||
@@ -149,7 +151,7 @@ public:
|
|||||||
void setRxInvert(bool invert) {};
|
void setRxInvert(bool invert) {};
|
||||||
};
|
};
|
||||||
|
|
||||||
HardwareSerial Serial, Serial2;
|
HardwareSerial Serial, Serial1, Serial2;
|
||||||
|
|
||||||
class EspClass {
|
class EspClass {
|
||||||
public:
|
public:
|
||||||
@@ -165,6 +167,9 @@ void delay(uint32_t ms) {
|
|||||||
|
|
||||||
bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
|
bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
|
||||||
bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
|
bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
|
||||||
|
uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution) { return freq; }
|
||||||
|
int8_t digitalPinToAnalogChannel(uint8_t pin) { return -1; }
|
||||||
|
uint32_t analogReadMilliVolts(uint8_t pin) { return 0; }
|
||||||
|
|
||||||
unsigned long __micros;
|
unsigned long __micros;
|
||||||
unsigned long __resetTime = 0;
|
unsigned long __resetTime = 0;
|
||||||
|
|||||||
@@ -0,0 +1,12 @@
|
|||||||
|
// Dummy file for the simulator
|
||||||
|
|
||||||
|
class ESP_NOW_Peer {
|
||||||
|
protected:
|
||||||
|
size_t send(const uint8_t *data, int len) { return 0; }
|
||||||
|
};
|
||||||
|
|
||||||
|
class ESP_NOW_Serial_Class : public ESP_NOW_Peer {
|
||||||
|
public:
|
||||||
|
virtual void onSent(bool success) {};
|
||||||
|
virtual size_t write(const uint8_t *data, size_t len) { return 0; };
|
||||||
|
};
|
||||||
@@ -13,14 +13,13 @@ class SBUS {
|
|||||||
public:
|
public:
|
||||||
SBUS(HardwareSerial& bus, const bool inv = true) {};
|
SBUS(HardwareSerial& bus, const bool inv = true) {};
|
||||||
SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
|
SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
|
||||||
void begin() {};
|
void begin(int rxpin = -1, int txpin = -1, bool inv = true, bool fast = false) {};
|
||||||
bool read() { return joystickInit(); };
|
bool read() { return joystickInit(); };
|
||||||
SBUSData data() {
|
void getChannels(uint16_t (&channels)[16]) const {
|
||||||
SBUSData data;
|
int16_t ch[16];
|
||||||
joystickGet(data.ch);
|
joystickGet(ch);
|
||||||
for (int i = 0; i < 16; i++) {
|
for (int i = 0; i < 16; i++) {
|
||||||
data.ch[i] = map(data.ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
|
channels[i] = map(ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
|
||||||
}
|
}
|
||||||
return data;
|
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -9,8 +9,7 @@
|
|||||||
#include "quaternion.h"
|
#include "quaternion.h"
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#include "wifi.h"
|
#include "wifi.h"
|
||||||
|
#include "lpf.h"
|
||||||
#define WIFI_ENABLED 1
|
|
||||||
|
|
||||||
extern float t, dt;
|
extern float t, dt;
|
||||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||||
@@ -21,28 +20,33 @@ extern float motors[4];
|
|||||||
|
|
||||||
Vector gyro, acc, imuRotation;
|
Vector gyro, acc, imuRotation;
|
||||||
Vector accBias, gyroBias, accScale(1, 1, 1);
|
Vector accBias, gyroBias, accScale(1, 1, 1);
|
||||||
|
LowPassFilter<Vector> gyroBiasFilter(0);
|
||||||
|
|
||||||
// declarations
|
// declarations
|
||||||
void step();
|
void step();
|
||||||
void computeLoopRate();
|
void computeLoopRate();
|
||||||
void applyGyro();
|
void applyGyro();
|
||||||
void applyAcc();
|
void applyAcc();
|
||||||
|
void applyLevel();
|
||||||
void control();
|
void control();
|
||||||
void interpretControls();
|
void interpretControls();
|
||||||
void controlAttitude();
|
void controlAttitude();
|
||||||
void controlRates();
|
void controlRates();
|
||||||
void controlTorque();
|
void controlTorque();
|
||||||
|
void desaturate(float& a, float& b, float& c, float& d);
|
||||||
const char* getModeName();
|
const char* getModeName();
|
||||||
void sendMotors();
|
void sendMotors();
|
||||||
|
int getDutyCycle(float value);
|
||||||
bool motorsActive();
|
bool motorsActive();
|
||||||
void testMotor(int n);
|
void testMotor(int n);
|
||||||
void print(const char* format, ...);
|
void print(const char* format, ...);
|
||||||
void pause(float duration);
|
void pause(float duration);
|
||||||
void doCommand(String str, bool echo);
|
void doCommand(String str, bool echo);
|
||||||
void handleInput();
|
void handleInput();
|
||||||
|
void setupRC();
|
||||||
void normalizeRC();
|
void normalizeRC();
|
||||||
void calibrateRC();
|
void calibrateRC();
|
||||||
void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
|
void calibrateRCChannel(int*, uint16_t[16], uint16_t[16], const char*);
|
||||||
void printRCCalibration();
|
void printRCCalibration();
|
||||||
void printLogHeader();
|
void printLogHeader();
|
||||||
void printLogData();
|
void printLogData();
|
||||||
@@ -54,6 +58,7 @@ void handleMavlink(const void *_msg);
|
|||||||
void mavlinkPrint(const char* str);
|
void mavlinkPrint(const char* str);
|
||||||
void sendMavlinkPrint();
|
void sendMavlinkPrint();
|
||||||
inline Quaternion fluToFrd(const Quaternion &q);
|
inline Quaternion fluToFrd(const Quaternion &q);
|
||||||
|
void setupPower();
|
||||||
void failsafe();
|
void failsafe();
|
||||||
void rcLossFailsafe();
|
void rcLossFailsafe();
|
||||||
void descend();
|
void descend();
|
||||||
@@ -63,7 +68,7 @@ const char *getParameterName(int index);
|
|||||||
float getParameter(int index);
|
float getParameter(int index);
|
||||||
float getParameter(const char *name);
|
float getParameter(const char *name);
|
||||||
bool setParameter(const char *name, const float value);
|
bool setParameter(const char *name, const float value);
|
||||||
void printParameters();
|
void printParameters(const char *filter);
|
||||||
void resetParameters();
|
void resetParameters();
|
||||||
|
|
||||||
// mocks
|
// mocks
|
||||||
@@ -73,3 +78,5 @@ void calibrateAccel() { print("Skip accel calibrating\n"); };
|
|||||||
void printIMUCalibration() { print("cal: N/A\n"); };
|
void printIMUCalibration() { print("cal: N/A\n"); };
|
||||||
void printIMUInfo() {};
|
void printIMUInfo() {};
|
||||||
void printWiFiInfo() {};
|
void printWiFiInfo() {};
|
||||||
|
void configWiFi(bool, const char*, const char*) { print("Skip WiFi config\n"); };
|
||||||
|
void setWiFiMode(const String& mode) { print("Skip WiFi mode set\n"); };
|
||||||
|
|||||||
@@ -27,6 +27,7 @@
|
|||||||
#include "mavlink.ino"
|
#include "mavlink.ino"
|
||||||
#include "motors.ino"
|
#include "motors.ino"
|
||||||
#include "parameters.ino"
|
#include "parameters.ino"
|
||||||
|
#include "power.ino"
|
||||||
#include "rc.ino"
|
#include "rc.ino"
|
||||||
#include "time.ino"
|
#include "time.ino"
|
||||||
|
|
||||||
@@ -72,6 +73,8 @@ public:
|
|||||||
gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());
|
gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());
|
||||||
acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));
|
acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));
|
||||||
|
|
||||||
|
voltage = 4.2f; // dummy voltage value
|
||||||
|
|
||||||
readRC();
|
readRC();
|
||||||
estimate();
|
estimate();
|
||||||
|
|
||||||
|
|||||||
@@ -1,4 +1,3 @@
|
|||||||
// Dummy file to make it possible to compile simulator with Flix' util.h
|
// Dummy file to make it possible to compile simulator with Flix' util.h
|
||||||
|
|
||||||
#define WRITE_PERI_REG(addr, val) {}
|
|
||||||
#define REG_CLR_BIT(_r, _b) {}
|
#define REG_CLR_BIT(_r, _b) {}
|
||||||
|
|||||||
@@ -11,9 +11,15 @@
|
|||||||
#include <sys/poll.h>
|
#include <sys/poll.h>
|
||||||
#include <gazebo/gazebo.hh>
|
#include <gazebo/gazebo.hh>
|
||||||
|
|
||||||
#define WIFI_UDP_PORT 14580
|
// Mocks
|
||||||
#define WIFI_UDP_REMOTE_PORT 14550
|
int wifiMode = 1;
|
||||||
#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
|
int wifiLongRange = 0;
|
||||||
|
int espnowChannel = 6;
|
||||||
|
const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
|
||||||
|
|
||||||
|
int udpLocalPort = 14580;
|
||||||
|
int udpRemotePort = 14550;
|
||||||
|
const char *udpRemoteIP = "255.255.255.255";
|
||||||
|
|
||||||
int wifiSocket;
|
int wifiSocket;
|
||||||
|
|
||||||
@@ -22,22 +28,22 @@ void setupWiFi() {
|
|||||||
sockaddr_in addr; // local address
|
sockaddr_in addr; // local address
|
||||||
addr.sin_family = AF_INET;
|
addr.sin_family = AF_INET;
|
||||||
addr.sin_addr.s_addr = INADDR_ANY;
|
addr.sin_addr.s_addr = INADDR_ANY;
|
||||||
addr.sin_port = htons(WIFI_UDP_PORT);
|
addr.sin_port = htons(udpLocalPort);
|
||||||
if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
|
if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
|
||||||
gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT << std::endl;
|
gzerr << "Failed to bind WiFi UDP socket on port " << udpLocalPort << std::endl;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
int broadcast = 1;
|
int broadcast = 1;
|
||||||
setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
|
setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
|
||||||
gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
|
gzmsg << "WiFi UDP socket initialized on port " << udpLocalPort << " (remote port " << udpRemotePort << ")" << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
void sendWiFi(const uint8_t *buf, int len) {
|
void sendWiFi(const uint8_t *buf, int len) {
|
||||||
if (wifiSocket == 0) setupWiFi();
|
if (wifiSocket == 0) setupWiFi();
|
||||||
sockaddr_in addr; // remote address
|
sockaddr_in addr; // remote address
|
||||||
addr.sin_family = AF_INET;
|
addr.sin_family = AF_INET;
|
||||||
addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
|
addr.sin_addr.s_addr = inet_addr(udpRemoteIP);
|
||||||
addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
|
addr.sin_port = htons(udpRemotePort);
|
||||||
sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
|
sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,3 @@
|
|||||||
|
# ESPNOW-proxy
|
||||||
|
|
||||||
|
Proxy sketch for using ESP-NOW connection with Flix drone.
|
||||||
@@ -0,0 +1,88 @@
|
|||||||
|
// Copyright (c) 2026 Oleg Kalachev <okalachev@gmail.com>
|
||||||
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
|
// Proxy for ESP-NOW connection
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
#include <WiFi.h>
|
||||||
|
#include <ESP32_NOW_Serial.h>
|
||||||
|
#include <MacAddress.h>
|
||||||
|
#include <MAVLink.h>
|
||||||
|
#include <Preferences.h>
|
||||||
|
#include "../../flix/util.h"
|
||||||
|
|
||||||
|
const int CHANNEL = 6;
|
||||||
|
char key[ESP_NOW_KEY_LEN + 1] = {0}; // with trailing null
|
||||||
|
|
||||||
|
Preferences storage;
|
||||||
|
|
||||||
|
std::vector<ESPNOWSerial *> peers;
|
||||||
|
|
||||||
|
void onNewPeer(const esp_now_recv_info_t *info, const uint8_t *data, int len, void *arg) {
|
||||||
|
if (len != 4 || memcmp(data, "flix", 4) != 0) return; // check if discovery message
|
||||||
|
|
||||||
|
Serial.printf("New peer: " MACSTR "\n", MAC2STR(info->src_addr));
|
||||||
|
ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, CHANNEL, WIFI_IF_AP);
|
||||||
|
link->begin();
|
||||||
|
link->setKey((const uint8_t *)key);
|
||||||
|
peers.push_back(link);
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
Serial.begin(115200);
|
||||||
|
WiFi.mode(WIFI_AP);
|
||||||
|
WiFi.setSleep(false);
|
||||||
|
WiFi.setChannel(CHANNEL);
|
||||||
|
|
||||||
|
ESP_NOW.onNewPeer(onNewPeer, NULL);
|
||||||
|
ESP_NOW.begin();
|
||||||
|
|
||||||
|
storage.begin("espnow-proxy");
|
||||||
|
if (!storage.isKey("key")) {
|
||||||
|
generateRandomKey();
|
||||||
|
storage.putString("key", key);
|
||||||
|
}
|
||||||
|
strcpy(key, storage.getString("key").c_str());
|
||||||
|
|
||||||
|
// Discover the first peer
|
||||||
|
while (peers.empty()) {
|
||||||
|
Serial.printf("espnow %s %s\n", WiFi.softAPmacAddress().c_str(), key);
|
||||||
|
delay(500);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void generateRandomKey() {
|
||||||
|
const char chars[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789!@#$%^&*-_+=";
|
||||||
|
for (int i = 0; i < ESP_NOW_KEY_LEN; i++) {
|
||||||
|
key[i] = chars[random(0, strlen(chars))];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
uint8_t buf[5000];
|
||||||
|
|
||||||
|
// Send from Serial to ESP-NOW
|
||||||
|
while (Serial.available() > 0) {
|
||||||
|
int b = Serial.read();
|
||||||
|
if (b < 0) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
mavlink_message_t msg;
|
||||||
|
mavlink_status_t status;
|
||||||
|
if (mavlink_parse_char(MAVLINK_COMM_0, (uint8_t)b, &msg, &status)) {
|
||||||
|
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||||
|
for (ESPNOWSerial *link : peers) {
|
||||||
|
link->write(buf, len);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Send from ESP-NOW to Serial
|
||||||
|
for (ESPNOWSerial *link : peers) {
|
||||||
|
int len = link->read(buf, sizeof(buf));
|
||||||
|
if (len > 0) {
|
||||||
|
Serial.write(buf, len);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -10,6 +10,7 @@ print('Connected:', flix.connected)
|
|||||||
print('Mode:', flix.mode)
|
print('Mode:', flix.mode)
|
||||||
print('Armed:', flix.armed)
|
print('Armed:', flix.armed)
|
||||||
print('Landed:', flix.landed)
|
print('Landed:', flix.landed)
|
||||||
|
print('Voltage:', flix.voltage, 'V')
|
||||||
print('Rates:', *[f'{math.degrees(r):.0f}°/s' for r in flix.rates])
|
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('Attitude:', *[f'{math.degrees(a):.0f}°' for a in flix.attitude_euler])
|
||||||
print('Motors:', flix.motors)
|
print('Motors:', flix.motors)
|
||||||
@@ -23,11 +24,11 @@ print('> imu')
|
|||||||
print(flix.cli('imu'))
|
print(flix.cli('imu'))
|
||||||
|
|
||||||
print('=== Get parameter...')
|
print('=== Get parameter...')
|
||||||
pitch_p = flix.get_param('PITCH_P')
|
pitch_p = flix.get_param('CTL_P_P')
|
||||||
print('PITCH_P = ', pitch_p)
|
print('CTL_P_P = ', pitch_p)
|
||||||
|
|
||||||
print('=== Set parameter...')
|
print('=== Set parameter...')
|
||||||
flix.set_param('PITCH_P', pitch_p)
|
flix.set_param('CTL_P_P', pitch_p)
|
||||||
|
|
||||||
print('=== Wait for gyro update...')
|
print('=== Wait for gyro update...')
|
||||||
print('Gyro: ', flix.wait('gyro'))
|
print('Gyro: ', flix.wait('gyro'))
|
||||||
|
|||||||
@@ -13,7 +13,7 @@ lines = []
|
|||||||
|
|
||||||
print('Downloading log...')
|
print('Downloading log...')
|
||||||
count = 0
|
count = 0
|
||||||
dev.write('log\n'.encode())
|
dev.write('log dump\n'.encode())
|
||||||
while True:
|
while True:
|
||||||
line = dev.readline()
|
line = dev.readline()
|
||||||
if not line:
|
if not line:
|
||||||
|
|||||||
@@ -43,6 +43,7 @@ records = [record for record in records if record[0] != 0]
|
|||||||
|
|
||||||
print(f'Received records: {len(records)}')
|
print(f'Received records: {len(records)}')
|
||||||
|
|
||||||
|
os.makedirs(f'{DIR}/log', exist_ok=True)
|
||||||
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
||||||
log.write(header.encode() + b'\n')
|
log.write(header.encode() + b'\n')
|
||||||
for record in records:
|
for record in records:
|
||||||
|
|||||||
@@ -24,19 +24,22 @@ pip install pyflix
|
|||||||
The API is accessed through the `Flix` class:
|
The API is accessed through the `Flix` class:
|
||||||
|
|
||||||
```python
|
```python
|
||||||
from flix import Flix
|
from pyflix import Flix
|
||||||
flix = Flix() # create a Flix object and wait for connection
|
flix = Flix() # create a Flix object and wait for connection
|
||||||
```
|
```
|
||||||
|
|
||||||
|
If using ESP-NOW connection, specify the proxy device name in `FLIX_DEVICE` environment variable or pass it to the constructor: `Flix(device='/dev/cu.usbserial-0001')`.
|
||||||
|
|
||||||
### Telemetry
|
### Telemetry
|
||||||
|
|
||||||
Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware itself:
|
Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware code:
|
||||||
|
|
||||||
```python
|
```python
|
||||||
print(flix.connected) # True if connected to the drone
|
print(flix.connected) # True if connected to the drone
|
||||||
print(flix.mode) # current flight mode (str)
|
print(flix.mode) # current flight mode (str)
|
||||||
print(flix.armed) # True if the drone is armed
|
print(flix.armed) # True if the drone is armed
|
||||||
print(flix.landed) # True if the drone is landed
|
print(flix.landed) # True if the drone is landed
|
||||||
|
print(flix.voltage) # battery voltage (NaN - unknown, ~0 - USB powered)
|
||||||
print(flix.attitude) # attitude quaternion [w, x, y, z]
|
print(flix.attitude) # attitude quaternion [w, x, y, z]
|
||||||
print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw]
|
print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw]
|
||||||
print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate]
|
print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate]
|
||||||
@@ -92,17 +95,18 @@ Full list of events:
|
|||||||
|-----|-----------|----------------|
|
|-----|-----------|----------------|
|
||||||
|`connected`|Connected to the drone||
|
|`connected`|Connected to the drone||
|
||||||
|`disconnected`|Connection is lost||
|
|`disconnected`|Connection is lost||
|
||||||
|`armed`|Armed state update|Armed state (*bool*)|
|
|`armed`|Armed state update|Armed state *(bool)*|
|
||||||
|`mode`|Flight mode update|Flight mode (*str*)|
|
|`mode`|Flight mode update|Flight mode *(str)*|
|
||||||
|`landed`|Landed state update|Landed state (*bool*)|
|
|`landed`|Landed state update|Landed state *(bool)*|
|
||||||
|
|`voltage`|Battery voltage update|Voltage *(float)*|
|
||||||
|`print`|The drone prints text to the console|Text|
|
|`print`|The drone prints text to the console|Text|
|
||||||
|`attitude`|Attitude update|Attitude quaternion (*list*)|
|
|`attitude`|Attitude update|Attitude quaternion *(list)*|
|
||||||
|`attitude_euler`|Attitude update|Euler angles (*list*)|
|
|`attitude_euler`|Attitude update|Euler angles *(list)*|
|
||||||
|`rates`|Angular rates update|Angular rates (*list*)|
|
|`rates`|Angular rates update|Angular rates *(list)*|
|
||||||
|`channels`|Raw RC channels update|Raw RC channels (*list*)|
|
|`channels`|Raw RC channels update|Raw RC channels *(list)*|
|
||||||
|`motors`|Motor outputs update|Motor outputs (*list*)|
|
|`motors`|Motor outputs update|Motor outputs *(list)*|
|
||||||
|`acc`|Accelerometer update|Accelerometer output (*list*)|
|
|`acc`|Accelerometer update|Accelerometer output *(list)*|
|
||||||
|`gyro`|Gyroscope update|Gyroscope output (*list*)|
|
|`gyro`|Gyroscope update|Gyroscope output *(list)*|
|
||||||
|`mavlink`|Received MAVLink message|Message object|
|
|`mavlink`|Received MAVLink message|Message object|
|
||||||
|`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|
|`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|
||||||
|`mavlink.<message_id>`|Received specific MAVLink message|Message object|
|
|`mavlink.<message_id>`|Received specific MAVLink message|Message object|
|
||||||
@@ -117,8 +121,8 @@ Full list of events:
|
|||||||
Get and set firmware parameters using `get_param` and `set_param` methods:
|
Get and set firmware parameters using `get_param` and `set_param` methods:
|
||||||
|
|
||||||
```python
|
```python
|
||||||
pitch_p = flix.get_param('PITCH_P') # get parameter value
|
pitch_p = flix.get_param('CTL_P_P') # get parameter value
|
||||||
flix.set_param('PITCH_P', 5) # set parameter value
|
flix.set_param('CTL_P_P', 5) # set parameter value
|
||||||
```
|
```
|
||||||
|
|
||||||
Execute console commands using `cli` method. This method returns the command response:
|
Execute console commands using `cli` method. This method returns the command response:
|
||||||
@@ -277,7 +281,3 @@ logger = logging.getLogger('flix')
|
|||||||
logger.setLevel(logging.DEBUG) # be more verbose
|
logger.setLevel(logging.DEBUG) # be more verbose
|
||||||
logger.setLevel(logging.WARNING) # be less verbose
|
logger.setLevel(logging.WARNING) # be less verbose
|
||||||
```
|
```
|
||||||
|
|
||||||
## Stability
|
|
||||||
|
|
||||||
The library is in development stage. The API is not stable.
|
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
|
|
||||||
import os
|
import os
|
||||||
import time
|
import time
|
||||||
|
import math
|
||||||
from queue import Queue, Empty
|
from queue import Queue, Empty
|
||||||
from typing import Optional, Callable, List, Dict, Any, Union, Sequence
|
from typing import Optional, Callable, List, Dict, Any, Union, Sequence
|
||||||
import logging
|
import logging
|
||||||
@@ -26,6 +27,7 @@ class Flix:
|
|||||||
mode: str = ''
|
mode: str = ''
|
||||||
armed: bool = False
|
armed: bool = False
|
||||||
landed: bool = False
|
landed: bool = False
|
||||||
|
voltage: float = math.nan
|
||||||
attitude: List[float]
|
attitude: List[float]
|
||||||
attitude_euler: List[float] # roll, pitch, yaw
|
attitude_euler: List[float] # roll, pitch, yaw
|
||||||
rates: List[float]
|
rates: List[float]
|
||||||
@@ -42,12 +44,17 @@ class Flix:
|
|||||||
_print_buffer: str = ''
|
_print_buffer: str = ''
|
||||||
_modes = ['RAW', 'ACRO', 'STAB', 'AUTO']
|
_modes = ['RAW', 'ACRO', 'STAB', 'AUTO']
|
||||||
|
|
||||||
def __init__(self, system_id: int=1, wait_connection: bool=True):
|
def __init__(self, system_id: int=1, wait_connection: bool=True, device=os.getenv('FLIX_DEVICE')):
|
||||||
if not (0 <= system_id < 256):
|
if not (0 <= system_id < 256):
|
||||||
raise ValueError('system_id must be in range [0, 255]')
|
raise ValueError('system_id must be in range [0, 255]')
|
||||||
self._setup_mavlink()
|
self._setup_mavlink()
|
||||||
self.system_id = system_id
|
self.system_id = system_id
|
||||||
self._init_state()
|
self._init_state()
|
||||||
|
if device is not None:
|
||||||
|
# User defined connection
|
||||||
|
logger.debug(f'Connecting to {device}')
|
||||||
|
self.connection: mavutil.mavfile = mavutil.mavlink_connection(device, source_system=255) # type: ignore
|
||||||
|
else:
|
||||||
try:
|
try:
|
||||||
# Direct connection
|
# Direct connection
|
||||||
logger.debug('Listening on port 14550')
|
logger.debug('Listening on port 14550')
|
||||||
@@ -68,7 +75,7 @@ class Flix:
|
|||||||
self._heartbeat_thread.start()
|
self._heartbeat_thread.start()
|
||||||
if wait_connection:
|
if wait_connection:
|
||||||
self.wait('mavlink.HEARTBEAT')
|
self.wait('mavlink.HEARTBEAT')
|
||||||
time.sleep(0.2) # give some time to receive initial state
|
time.sleep(0.6) # give some time to receive initial state
|
||||||
|
|
||||||
def _init_state(self):
|
def _init_state(self):
|
||||||
self.attitude = [1, 0, 0, 0]
|
self.attitude = [1, 0, 0, 0]
|
||||||
@@ -138,7 +145,7 @@ class Flix:
|
|||||||
while True:
|
while True:
|
||||||
try:
|
try:
|
||||||
msg: Optional[mavlink.MAVLink_message] = self.connection.recv_match(blocking=True)
|
msg: Optional[mavlink.MAVLink_message] = self.connection.recv_match(blocking=True)
|
||||||
if msg is None:
|
if msg is None or msg.get_srcSystem() != self.system_id:
|
||||||
continue
|
continue
|
||||||
self._connected()
|
self._connected()
|
||||||
msg_dict = msg.to_dict()
|
msg_dict = msg.to_dict()
|
||||||
@@ -185,11 +192,16 @@ class Flix:
|
|||||||
self._trigger('motors', self.motors)
|
self._trigger('motors', self.motors)
|
||||||
|
|
||||||
if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
|
if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
|
||||||
self.acc = self._mavlink_to_flu([msg.xacc / 1000, msg.yacc / 1000, msg.zacc / 1000])
|
ONE_G = 9.80665
|
||||||
|
self.acc = self._mavlink_to_flu([msg.xacc * ONE_G / 1000, msg.yacc * ONE_G / 1000, msg.zacc * ONE_G / 1000])
|
||||||
self.gyro = self._mavlink_to_flu([msg.xgyro / 1000, msg.ygyro / 1000, msg.zgyro / 1000])
|
self.gyro = self._mavlink_to_flu([msg.xgyro / 1000, msg.ygyro / 1000, msg.zgyro / 1000])
|
||||||
self._trigger('acc', self.acc)
|
self._trigger('acc', self.acc)
|
||||||
self._trigger('gyro', self.gyro)
|
self._trigger('gyro', self.gyro)
|
||||||
|
|
||||||
|
if isinstance(msg, mavlink.MAVLink_battery_status_message):
|
||||||
|
self.voltage = msg.voltages[0] / 1000
|
||||||
|
self._trigger('voltage', self.voltage)
|
||||||
|
|
||||||
if isinstance(msg, mavlink.MAVLink_serial_control_message):
|
if isinstance(msg, mavlink.MAVLink_serial_control_message):
|
||||||
# new chunk of data
|
# new chunk of data
|
||||||
text = bytes(msg.data)[:msg.count].decode('utf-8', errors='ignore')
|
text = bytes(msg.data)[:msg.count].decode('utf-8', errors='ignore')
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
[project]
|
[project]
|
||||||
name = "pyflix"
|
name = "pyflix"
|
||||||
version = "0.11"
|
version = "0.16"
|
||||||
description = "Python API for Flix drone"
|
description = "Python API for Flix drone"
|
||||||
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
||||||
license = "MIT"
|
license = "MIT"
|
||||||
|
|||||||