2 Commits
Author SHA1 Message Date
Oleg Kalachev 04122bcd3f Test failing simulator run 2026-05-27 13:22:35 +03:00
Oleg Kalachev 1f2f623f6a Test running simulation in ci 2026-05-27 12:31:37 +03:00
51 changed files with 271 additions and 1100 deletions
+5 -2
View File
@@ -27,8 +27,6 @@ jobs:
run: make BOARD=esp32:esp32:esp32c3
- name: Build firmware for ESP32-S3
run: make BOARD=esp32:esp32:esp32s3
- name: Build espnow-proxy
run: arduino-cli compile --fqbn esp32:esp32:esp32 tools/espnow-proxy
- name: Check c_cpp_properties.json
run: tools/check_c_cpp_properties.py
@@ -78,6 +76,11 @@ jobs:
run: sudo apt-get install -y libsdl2-dev
- name: Build simulator
run: make build_simulator
- name: Run simulator
env:
GAZEBO_MODEL_PATH: ${{ github.workspace }}/gazebo/models
GAZEBO_PLUGIN_PATH: ${{ github.workspace }}/gazebo/build
run: timeout --preserve-status 120 gzserver --verbose gazebo/flix.world || [ $? -eq 143 ]
- uses: actions/upload-artifact@v4
with:
name: gazebo-plugin-binary
+2 -3
View File
@@ -4,10 +4,9 @@ build/
tools/log/
tools/dist/
*.egg-info/
.core
.libs
.dependencies
.vscode/*
!.vscode/settings.default.json
!.vscode/settings.json
!.vscode/c_cpp_properties.json
!.vscode/tasks.json
!.vscode/launch.json
+21 -21
View File
@@ -6,18 +6,18 @@
"${workspaceFolder}/flix",
"${workspaceFolder}/gazebo",
"${workspaceFolder}/tools/**",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
"~/Arduino/libraries/**",
"/usr/include/gazebo-11/",
"/usr/include/ignition/math6/"
],
"forcedInclude": [
"${workspaceFolder}/.vscode/intellisense.h",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
"${workspaceFolder}/flix/cli.ino",
"${workspaceFolder}/flix/control.ino",
"${workspaceFolder}/flix/estimate.ino",
@@ -33,7 +33,7 @@
"${workspaceFolder}/flix/parameters.ino",
"${workspaceFolder}/flix/safety.ino"
],
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2601/bin/xtensa-esp32-elf-g++",
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
@@ -53,18 +53,18 @@
"name": "Mac",
"includePath": [
"${workspaceFolder}/flix",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
"~/Documents/Arduino/libraries/**",
"/opt/homebrew/include/gazebo-11/",
"/opt/homebrew/include/ignition/math6/"
],
"forcedInclude": [
"${workspaceFolder}/.vscode/intellisense.h",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
"${workspaceFolder}/flix/flix.ino",
"${workspaceFolder}/flix/cli.ino",
"${workspaceFolder}/flix/control.ino",
@@ -80,7 +80,7 @@
"${workspaceFolder}/flix/parameters.ino",
"${workspaceFolder}/flix/safety.ino"
],
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2601/bin/xtensa-esp32-elf-g++",
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
@@ -103,16 +103,16 @@
"${workspaceFolder}/flix",
"${workspaceFolder}/gazebo",
"${workspaceFolder}/tools/**",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
"~/Documents/Arduino/libraries/**"
],
"forcedInclude": [
"${workspaceFolder}/.vscode/intellisense.h",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
"${workspaceFolder}/flix/cli.ino",
"${workspaceFolder}/flix/control.ino",
"${workspaceFolder}/flix/estimate.ino",
@@ -128,7 +128,7 @@
"${workspaceFolder}/flix/parameters.ino",
"${workspaceFolder}/flix/safety.ino"
],
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2601/bin/xtensa-esp32-elf-g++.exe",
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++.exe",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
-1
View File
@@ -1,7 +1,6 @@
{
// See https://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
"recommendations": [
"dangmai.workspace-default-settings",
"ms-vscode.cpptools",
"ms-vscode.cmake-tools",
"ms-python.python"
+10 -18
View File
@@ -1,32 +1,24 @@
BOARD = esp32:esp32:d1_mini32
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*))
export ARDUINO_NETWORK_CONNECTION_TIMEOUT := 1h
build: .core .libs
arduino-cli compile --fqbn $(BOARD) --build-property "build.core_debug_level=1" flix
build: .dependencies
arduino-cli compile --fqbn $(BOARD) flix
upload: build
arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" flix
erase:
arduino-cli burn-bootloader --fqbn $(BOARD) -p "$(PORT)" -P esptool
monitor:
arduino-cli monitor -p "$(PORT)" -c baudrate=115200
core .core:
arduino-cli core update-index --additional-urls https://espressif.github.io/arduino-esp32/package_esp32_index.json
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:
dependencies .dependencies:
arduino-cli core update-index --config-file arduino-cli.yaml
arduino-cli core install esp32:esp32@3.3.6 --config-file arduino-cli.yaml
arduino-cli lib update-index
arduino-cli lib install "FlixPeriph"
arduino-cli lib install "MAVLink"@2.0.25
touch .libs
touch .dependencies
upload_proxy: .core .libs
upload_proxy: .dependencies
arduino-cli compile --fqbn $(BOARD) tools/espnow-proxy
arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" tools/espnow-proxy
@@ -34,7 +26,7 @@ gazebo/build cmake: gazebo/CMakeLists.txt
mkdir -p gazebo/build
cd gazebo/build && cmake ..
build_simulator: .libs gazebo/build
build_simulator: .dependencies gazebo/build
make -C gazebo/build
simulator: build_simulator
@@ -49,6 +41,6 @@ plot:
plotjuggler -d $(shell ls -t tools/log/*.csv | head -n1)
clean:
rm -rf gazebo/build flix/build flix/cache .core .libs
rm -rf gazebo/build flix/build flix/cache .dependencies
.PHONY: build upload monitor core libs cmake build_simulator simulator log clean
.PHONY: build upload monitor dependencies cmake build_simulator simulator log clean
+1 -1
View File
@@ -84,7 +84,7 @@ Additional articles:
|*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|
|Propeller|55 mm or 65 mm|<img src="docs/img/prop.jpg" width=100>|4|
|MOSFET (transistor)|UMW 100N03A or [analog](https://t.me/opensourcequadcopter/33).<br>Warning: don't use KIA 100N03A or other manufacturers, they might not work!|<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<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).<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|
+5
View File
@@ -0,0 +1,5 @@
board_manager:
additional_urls:
- https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json
network:
connection_timeout: 1h
-3
View File
@@ -79,9 +79,6 @@ To add a new parameter:
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:
Binary file not shown.

Before

Width:  |  Height:  |  Size: 60 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 52 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 56 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 62 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 49 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 41 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 56 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 60 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 54 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 69 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 58 KiB

+12 -42
View File
@@ -20,14 +20,13 @@ 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).
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.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.
3. Install ESP32 core, version 3.3.6. 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):
* `FlixPeriph`, the latest version.
* `MAVLink`, version 2.0.25.
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, *ESP32S3 Dev Module* for ESP32-S3 Super Mini) and the port.
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.
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.
### Command line (Windows, Linux, macOS)
@@ -58,12 +57,6 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
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:FlashSize=4M,CDCOnBoot=cdc upload
```
See other available Make commands in [Makefile](../Makefile).
> [!TIP]
@@ -76,25 +69,8 @@ See other available Make commands in [Makefile](../Makefile).
In case if using different IMU model than MPU9250, change `imu` variable declaration in the `imu.ino`:
```cpp
ICM20948 imu(SPI); // For ICM-20948 via SPI
// or
MPU6050 imu(Wire); // For MPU-6050 via I2C
```
If using non-default SPI pins, pass SCK, MISO, and MOSI pin numbers to `SPI.begin` call and SS pin to `imu` constructor like that:
```cpp
ICM20948(SPI, <SS>);
// ...
SPI.begin(<SCK>, <MISO>, <MOSI>);
imu.begin();
```
If using non-default I2C pins, pass SDA and SCL pin numbers to `Wire.setPins` call like that:
```cpp
Wire.begin(<SDA>, <SCL>);
imu.begin();
ICM20948 imu(SPI); // For ICM-20948
MPU6050 imu(Wire); // For MPU-6050
```
### Connect using QGroundControl
@@ -106,9 +82,6 @@ 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`).
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
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)**.
@@ -122,7 +95,7 @@ To access the console using serial port:
To access the console using QGroundControl:
1. Connect to the drone using QGroundControl app.
2. Go to the QGroundControl menu ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
2. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
<img src="img/cli.png" width="400">
@@ -215,7 +188,7 @@ After this setup, you should see the battery voltage in QGroundControl top panel
## Setup remote control
There are several ways to control the drone's flight: using **smartphone** (Wi-Fi), using **SBUS remote control**, or using **USB remote control** (Wi-Fi/ESP-NOW).
There are several ways to control the drone's flight: using **smartphone** (Wi-Fi), using **SBUS remote control**, or using **USB remote control** (Wi-Fi).
### Control with a smartphone
@@ -260,7 +233,7 @@ If your drone doesn't have RC receiver installed, you can use USB remote control
3. Power up the drone.
4. Connect your computer to the appeared `flix` Wi-Fi network (password: `flixwifi`).
5. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
6. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Joystick*. Calibrate your USB remote control there.
6. Go the the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Joystick*. Calibrate you USB remote control there.
7. Use the USB remote control to fly the drone!
## Flight
@@ -350,13 +323,13 @@ 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 in Arduino IDE or use `make monitor` command. The ESP32 will print its MAC address and generated encryption key, for example:
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) or 6 (encrypted).
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:
@@ -369,14 +342,11 @@ To setup ESP-NOW communication:
* Type: Serial.
* Serial Port: choose the port of the proxy ESP32 board, e. g. `/dev/cu.usbserial-0001`.
* Baud Rate: 115200.
5. Click *Save*, click *Connect*. QGroundControl should connect to the drone using ESP-NOW and begin showing the telemetry.
> [!TIP]
> Make sure Arduino IDE is not running when using ESP-NOW proxy board, as it may block it.
5. Click *Save*. QGroundControl should connect to the drone using ESP-NOW and begin showing the telemetry.
## Flight log
After the flight, you can download the flight log wirelessly for analysis. Use the following command on your computer for that:
After the flight, you can download the flight log for analysis wirelessly. Use the following command on your computer for that:
```bash
make log
-51
View File
@@ -4,46 +4,6 @@ This page contains user-built drones based on the Flix project. Publish your pro
---
Author: Alican Erüst.<br>
Description: QX95 mm frame, 55 mm propellers, 3.7 V 25C 1050 mAh LiPo battery, MPU6050 IMU, Logitech F310 gamepad controller, with a total quadcopter weight of 66 g.
<img src="img/user/alicanerus/1.jpg" height=200> <img src="img/user/alicanerus/2.jpg" height=200> <img src="img/user/alicanerus/3.jpg" height=200>
[Flight video](https://drive.google.com/file/d/1k0WeWTKnCAfaugkX7LcmNxsUuq79RL8Z/view?usp=sharing).
---
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).<br>
Sources and materials: [link](https://drive.google.com/drive/folders/1uWiDcuorLrtVs_IIR7Y13omij-7Q1nx8).
<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).
@@ -97,17 +57,6 @@ 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
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.
+31 -51
View File
@@ -6,10 +6,10 @@
#include "pid.h"
#include "vector.h"
#include "util.h"
#include "filter.h"
#include "lpf.h"
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern const int RAW, ACRO, STAB, AUTO, POS;
extern const int RAW, ACRO, STAB, AUTO;
extern const int W_AP, W_STA, W_ESPNOW;
extern float t, dt, loopRate;
extern uint16_t channels[16];
@@ -31,39 +31,33 @@ const char* motd =
"Commands:\n\n"
"help - show help\n"
"p - show all parameters\n"
"p <str> - show parameters starting with str\n"
"p <name> - show parameter\n"
"p <name> <value> - set parameter\n"
"preset - reset parameters\n"
"time - show time info\n"
"ps - show pitch/roll/yaw\n"
"psq - show attitude quaternion\n"
"imu - show IMU data\n"
"ca - calibrate accel\n"
"st - show state estimation\n"
"arm - arm the drone\n"
"disarm - disarm the drone\n"
"raw/stab/acro/auto - set mode\n"
"rc - show RC data\n"
"cr - calibrate RC\n"
"pw - show power 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"
"ap <ssid> <password> - setup Wi-Fi access point\n"
"sta <ssid> <password> - setup Wi-Fi client mode\n"
"espnow <mac> [<key>] - setup ESP-NOW peer\n"
"mot - show motor output\n"
"mfr/mfl/mrr/mrl [<thrust>] - test motor (remove props)\n"
"log [dump] - print log header [and data]\n"
"log - show log info\n"
"log header - show log header\n"
"log reset - reset log\n"
"log <name> <rate> - setup log topic rate\n"
"l <str> - show log values starting with str\n"
"l expose <name> - expose log value to telemetry\n"
"cr - calibrate RC\n"
"ca - calibrate accel\n"
"mfr, mfl, mrr, mrl - test motor (remove props)\n"
"sys - show system info\n"
"reset - reset drone's state\n"
"reboot - reboot the drone\n";
void print(const char* format, ...) {
char buf[3000];
char buf[1000];
va_list args;
va_start(args, format);
vsnprintf(buf, sizeof(buf), format, args);
@@ -98,8 +92,10 @@ void doCommand(String str, bool echo = false) {
// execute command
if (command == "help" || command == "motd") {
print("%s\n", motd);
} else if (command == "p" && arg1 == "") {
printParameters(arg0.c_str());
} else if (command == "p" && arg0 == "") {
printParameters();
} else if (command == "p" && arg0 != "" && arg1 == "") {
print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
} else if (command == "p") {
bool success = setParameter(arg0.c_str(), arg1.toFloat());
if (success) {
@@ -113,15 +109,15 @@ void doCommand(String str, bool echo = false) {
print("Time: %f\n", t);
print("Loop rate: %.0f\n", loopRate);
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") {
printIMUInfo();
printIMUCalibration();
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") {
armed = true;
} else if (command == "disarm") {
@@ -134,8 +130,6 @@ void doCommand(String str, bool echo = false) {
mode = ACRO;
} else if (command == "auto") {
mode = AUTO;
} else if (command == "pos") {
mode = POS;
} else if (command == "rc") {
print("channels: ");
for (int i = 0; i < 16; i++) {
@@ -148,10 +142,8 @@ void doCommand(String str, bool echo = false) {
print("armed: %d\n", armed);
} else if (command == "pw") {
print("Voltage: %.1f V\n", voltage);
} else if (command == "wifi" && arg0 == "") {
printWiFiInfo();
} else if (command == "wifi") {
setWiFiMode(arg0);
printWiFiInfo();
} else if (command == "ap") {
configWiFi(W_AP, arg0.c_str(), arg1.c_str());
} else if (command == "sta") {
@@ -161,41 +153,29 @@ void doCommand(String str, bool echo = false) {
} else if (command == "mot") {
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]);
} else if (command == "log" && arg0 == "") {
printLogInfo();
} else if (command == "log" && arg1 != "") {
configLogThrottle(arg0.c_str(), arg1.toFloat());
} else if (command == "log" && arg0 == "header") {
} else if (command == "log") {
printLogHeader();
} else if (command == "log" && arg0 == "reset") {
resetLog();
} else if (command == "l" && arg0 == "expose" && arg1 != "") {
exposeLogValue(arg1.c_str());
} else if (command == "l") {
printLogValues(arg0.c_str());
if (arg0 == "dump") printLogData();
} else if (command == "cr") {
calibrateRC();
} else if (command == "ca") {
calibrateAccel();
} else if (command == "mfr") {
testMotor(MOTOR_FRONT_RIGHT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
testMotor(MOTOR_FRONT_RIGHT);
} else if (command == "mfl") {
testMotor(MOTOR_FRONT_LEFT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
testMotor(MOTOR_FRONT_LEFT);
} else if (command == "mrr") {
testMotor(MOTOR_REAR_RIGHT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
testMotor(MOTOR_REAR_RIGHT);
} else if (command == "mrl") {
testMotor(MOTOR_REAR_LEFT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
testMotor(MOTOR_REAR_LEFT);
} else if (command == "sys") {
#ifdef ESP32
print("Chip: %s\n", ESP.getChipModel());
print("Temperature: %.1f °C\n", temperatureRead());
print("Total RAM: %d KB\n", ESP.getHeapSize() / 1024);
print("Free heap: %d KB\n", ESP.getFreeHeap() / 1024);
print("PSRAM: %d KB\n", ESP.getPsramSize() / 1024);
print("Free PSRAM: %d KB\n", ESP.getFreePsram() / 1024);
print("Free heap: %d\n", ESP.getFreeHeap());
print("Firmware: " __DATE__ " " __TIME__ "\n");
// Print tasks table
print("Num Task MinSt Prio Core CPU%%\n");
print("Num Task Stack Prio Core CPU%%\n");
int taskCount = uxTaskGetNumberOfTasks();
TaskStatus_t *systemState = new TaskStatus_t[taskCount];
uint32_t totalRunTime;
@@ -229,7 +209,7 @@ void handleInput() {
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (c == '\n') {
doCommand(input);
input.clear();
} else {
+3 -16
View File
@@ -6,7 +6,7 @@
#include "vector.h"
#include "quaternion.h"
#include "pid.h"
#include "filter.h"
#include "lpf.h"
#include "util.h"
#define PITCHRATE_P 0.05
@@ -34,7 +34,7 @@
#define TILT_MAX radians(30)
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3, POS = 4;
const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
int mode = STAB;
bool armed = false;
@@ -56,7 +56,6 @@ 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 float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
extern Vector position, positionTarget;
void control() {
interpretControls();
@@ -80,18 +79,7 @@ void interpretControls() {
thrustTarget = controlThrottle;
if (mode == POS) {
if (controlRoll != 0 || controlPitch != 0) {
positionTarget.x = NAN;
positionTarget.y = NAN;
} else if (invalid(positionTarget.x) || invalid(positionTarget.y)) {
// reset position target
positionTarget.x = position.x;
positionTarget.y = position.y;
}
}
if (mode == STAB || (mode == POS && invalid(positionTarget.x))) {
if (mode == STAB) {
float yawTarget = attitudeTarget.getYaw();
if (!armed || invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target
attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
@@ -161,7 +149,6 @@ void controlTorque() {
motors[MOTOR_REAR_LEFT] = 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);
+3 -11
View File
@@ -5,7 +5,7 @@
#include "quaternion.h"
#include "vector.h"
#include "filter.h"
#include "lpf.h"
#include "util.h"
Vector rates; // estimated angular rates, rad/s
@@ -15,24 +15,16 @@ bool landed;
float accWeight = 0.003;
float levelWeight = 0.0002;
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
NotchFilter<Vector> ratesNotch(382, 0);
void setupEstimate() {
print("Setup estimation\n");
ratesNotch.reset();
}
void estimate() {
applyGyro();
applyAcc();
applyLevel();
estimatePosition();
}
void applyGyro() {
// filter gyro to get angular rates
rates = ratesFilter.update(gyro);
rates = ratesNotch.update(rates);
// apply rates to attitude
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));
@@ -40,7 +32,8 @@ void applyGyro() {
void applyAcc() {
// test should we apply accelerometer gravity correction
landed = !motorsActive() && abs(acc.norm() - ONE_G) < ONE_G * 0.1f;
float accNorm = acc.norm();
landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
if (!landed) return;
@@ -54,7 +47,6 @@ void applyAcc() {
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);
-98
View File
@@ -1,98 +0,0 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Low pass and notch filters
#pragma once
template <typename T> // Using template to make the filter usable for scalar and vector values
class LowPassFilter {
public:
float alpha; // smoothing constant, 1 means filter disabled
T output;
LowPassFilter(float alpha): alpha(alpha) {};
T update(const T input) {
if (!init) {
init = true;
return output = input;
}
return output += alpha * (input - output);
}
void setCutOffFrequency(float cutOffFreq, float dt) {
alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
}
void reset() {
init = false;
}
private:
bool init = false;
};
template <typename T>
class NotchFilter {
public:
float frequency;
float bandwidth;
T output;
NotchFilter(float frequency, float bandwidth): frequency(frequency), bandwidth(bandwidth) {
reset();
};
T update(const T input) {
if (frequency <= 0 || bandwidth <= 0) return input;
if (!init) {
init = true;
x1 = x2 = input;
y1 = y2 = input;
return output = input;
}
output = b0 * input + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
x2 = x1;
x1 = input;
y2 = y1;
y1 = output;
return output;
}
void reset() {
const float dt = 0.001f;
float f = frequency;
float bw = bandwidth;
if (f < 0) f = 0;
if (bw < 1e-6f) bw = 1e-6f;
float q = f / bw;
if (q < 1e-3f) q = 1e-3f;
const float w0 = 2.0f * PI * f * dt;
const float c = cos(w0);
const float s = sin(w0);
const float alpha = s / (2.0f * q);
const float a0 = 1.0f + alpha;
const float invA0 = 1.0f / a0;
b0 = 1.0f * invA0;
b1 = -2.0f * c * invA0;
b2 = 1.0f * invA0;
a1 = -2.0f * c * invA0;
a2 = (1.0f - alpha) * invA0;
init = false;
}
private:
float b0, b1, b2, a1, a2;
T x1, x2, y1, y2;
bool init = false;
};
+2 -4
View File
@@ -17,7 +17,7 @@ extern float motors[4];
void setup() {
Serial.begin(115200);
print("Initializing Flix\n");
print("Initializing flix\n");
setupParameters();
setupPower();
setupLED();
@@ -26,8 +26,6 @@ void setup() {
setupWiFi();
setupIMU();
setupRC();
setupEstimate();
setupLog();
setLED(false);
print("Initializing complete\n");
}
@@ -42,6 +40,6 @@ void loop() {
handleInput();
processMavlink();
readVoltage();
loopLog();
logData();
syncParameters();
}
+3 -8
View File
@@ -6,7 +6,7 @@
#include <SPI.h>
#include <FlixPeriph.h>
#include "vector.h"
#include "filter.h"
#include "lpf.h"
#include "util.h"
MPU9250 imu(SPI);
@@ -40,12 +40,10 @@ void readIMU() {
imu.getGyro(gyro.x, gyro.y, gyro.z);
imu.getAccel(acc.x, acc.y, acc.z);
calibrateGyroOnce();
// Apply scale and bias
// apply scale and bias
acc = (acc - accBias) / accScale;
gyro = gyro - gyroBias;
// Rotate to body frame
// rotate to body frame
Quaternion rotation = Quaternion::fromEuler(imuRotation);
acc = Quaternion::rotateVector(acc, rotation.inversed());
gyro = Quaternion::rotateVector(gyro, rotation.inversed());
@@ -54,7 +52,6 @@ void readIMU() {
void calibrateGyroOnce() {
static Delay landedDelay(2);
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
gyroBias = gyroBiasFilter.update(gyro);
}
@@ -108,7 +105,6 @@ void calibrateAccelOnce() {
if (acc.x < accMin.x) accMin.x = acc.x;
if (acc.y < accMin.y) accMin.y = acc.y;
if (acc.z < accMin.z) accMin.z = acc.z;
// Compute scale and bias
accScale = (accMax - accMin) / 2 / ONE_G;
accBias = (accMax + accMin) / 2;
@@ -125,7 +121,6 @@ void printIMUInfo() {
print("model: %s\n", imu.getModel());
print("who am I: 0x%02X\n", imu.whoAmI());
print("rate: %.0f\n", loopRate);
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);
imu.waitForData();
+41 -241
View File
@@ -1,277 +1,77 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Logging subsystem
// In-RAM logging
#include "vector.h"
#include "util.h"
int logMemory = -1; // 0 - RAM, 1 - PSRAM, -1 - disabled
float logUsage = 0.5; // fraction of free memory to use for log
#define LOG_RATE 100
#define LOG_DURATION 10
#define LOG_SIZE LOG_DURATION * LOG_RATE
struct LogValue {
Vector attitudeEuler;
Vector attitudeTargetEuler;
struct LogEntry {
const char *name;
Value value;
float lastValue = NAN;
bool logged = true; // if false, use only for triggering log update
LogValue() : name(nullptr), value() {}; // empty value constructor
template <typename T>
LogValue(const char *name, T value, bool logged = true) : name(name), value(value), logged(logged) {};
float *value;
};
struct LogTopic {
LogValue values[10];
int length = 0; // number of logged values
float throttle; // max update rate, Hz
float lastUpdate = -INFINITY;
LogTopic(float throttle, LogValue v0, LogValue v1 = {}, LogValue v2 = {}, LogValue v3 = {}, LogValue v4 = {}, LogValue v5 = {}, LogValue v6 = {}, LogValue v7 = {}, LogValue v8 = {}, LogValue v9 = {}) :
throttle(throttle), values{v0, v1, v2, v3, v4, v5, v6, v7, v8, v9} {
// Count logged values
for (auto& v : values) {
if (v.name == nullptr) break;
if (v.logged) length++;
}
};
LogTopic(LogValue v0, LogValue v1 = {}, LogValue v2 = {}, LogValue v3 = {}, LogValue v4 = {}, LogValue v5 = {}, LogValue v6 = {}, LogValue v7 = {}, LogValue v8 = {}, LogValue v9 = {}) :
LogTopic(INFINITY, v0, v1, v2, v3, v4, v5, v6, v7, v8, v9) {};
};
LogTopic logTopics[] = {
// time
LogTopic({"t", &t}), // must be the first topic
LogTopic(1, {"loopRate", &loopRate}),
// imu
LogTopic(
{"gyro.x", &gyro.x},
{"gyro.y", &gyro.y},
{"gyro.z", &gyro.z}),
LogTopic(50,
{"acc.x", &acc.x},
{"acc.y", &acc.y},
{"acc.z", &acc.z}),
LogTopic(10,
{"gyroBias.x", &gyroBias.x},
{"gyroBias.y", &gyroBias.y},
{"gyroBias.z", &gyroBias.z}),
// estimation
LogTopic(50,
LogEntry logEntries[] = {
{"t", &t},
{"rates.x", &rates.x},
{"rates.y", &rates.y},
{"rates.z", &rates.z},
{"attitude.roll", []() { return attitude.getRoll(); }},
{"attitude.pitch", []() { return attitude.getPitch(); }},
{"attitude.yaw", []() { return attitude.getYaw(); }}),
LogTopic(50,
{"position.x", &position.x},
{"position.y", &position.y},
{"position.z", &position.z}),
// rc
LogTopic(10,
{"controlTime", &controlTime, false}, // trigger value
{"controlRoll", &controlRoll},
{"controlPitch", &controlPitch},
{"controlYaw", &controlYaw},
{"controlThrottle", &controlThrottle}),
// control
LogTopic({"armed", &armed}),
LogTopic({"mode", &mode}),
LogTopic(10,
{"ratesTarget.x", &ratesTarget.x},
{"ratesTarget.y", &ratesTarget.y},
{"ratesTarget.z", &ratesTarget.z},
{"attitudeTarget.roll", []() { return attitudeTarget.getRoll(); }},
{"attitudeTarget.pitch", []() { return attitudeTarget.getPitch(); }},
{"attitudeTarget.yaw", []() { return attitudeTarget.getYaw(); }},
{"thrustTarget", &thrustTarget}),
// motors
LogTopic(
{"motors[0]", &motors[0]},
{"motors[1]", &motors[1]},
{"motors[2]", &motors[2]},
{"motors[3]", &motors[3]}),
// misc
LogTopic(5,
{"voltage", &voltage},
{"temp", &temperatureRead},
{"imuTemp", []() { return imu.getTemp(); }}),
{"attitude.x", &attitudeEuler.x},
{"attitude.y", &attitudeEuler.y},
{"attitude.z", &attitudeEuler.z},
{"attitudeTarget.x", &attitudeTargetEuler.x},
{"attitudeTarget.y", &attitudeTargetEuler.y},
{"attitudeTarget.z", &attitudeTargetEuler.z},
{"thrustTarget", &thrustTarget}
};
void *logBuffer; // buffer for log data
size_t logCapacity;
size_t logCursor = 0;
size_t logLength = 0;
LogValue *logExposed = nullptr; // log values exposed to telemetry
const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
float logBuffer[LOG_SIZE][logColumns];
void setupLog() {
print("Setup log\n");
free(logBuffer); // when reconfiguring
logBuffer = nullptr;
logCursor = 0;
logLength = 0;
if (logMemory == 0) {
logCapacity = ESP.getFreeHeap() * logUsage;
logBuffer = (uint8_t *)calloc(logCapacity, 1);
} else if (logMemory == 1) {
logCapacity = ESP.getFreePsram() * logUsage;
logBuffer = (uint8_t *)heap_caps_calloc(logCapacity, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
}
void prepareLogData() {
attitudeEuler = attitude.toEuler();
attitudeTargetEuler = attitudeTarget.toEuler();
}
void loopLog() {
if (logBuffer == nullptr || !armed) return;
void logData() {
if (!armed) return;
static int logPointer = 0;
static Rate period(LOG_RATE);
if (!period) return;
if (!logLength) resetLog(); // reset state on first log write
prepareLogData();
static Rate sync(2);
if (sync) {
const uint8_t marker[] = {0x1A, 0x91, 0x4F, 0xF6, 0x7F};
writeLog(&marker, sizeof(marker)); // write sync marker
for (int i = 0; i < logColumns; i++) {
logBuffer[logPointer][i] = *logEntries[i].value;
}
for (uint8_t i = 0; i < sizeof(logTopics) / sizeof(logTopics[0]); i++) {
LogTopic& topic = logTopics[i];
if (t - topic.lastUpdate < 1 / topic.throttle) continue; // throttle topic
if (!isTopicUpdated(i)) continue; // skip if topic was't updated
topic.lastUpdate = t;
writeLog(&i, sizeof(i)); // write topic index
for (auto& value : topic.values) {
if (value.name == nullptr) break;
if (!value.logged) continue;
value.lastValue = value.value.get();
writeLog(&value.lastValue, sizeof(float)); // write value
logPointer++;
if (logPointer >= LOG_SIZE) {
logPointer = 0;
}
}
}
void resetLog() {
for (auto& topic : logTopics) {
topic.lastUpdate = -INFINITY;
for (auto& value : topic.values) {
value.lastValue = NAN;
}
}
logCursor = 0;
logLength = 0;
}
void writeLog(const void *data, size_t size) {
size_t first = min(size, logCapacity - logCursor);
size_t second = size - first;
memcpy(logBuffer + logCursor, data, first);
logCursor = (logCursor + first) % logCapacity;
if (second > 0) {
memcpy(logBuffer + logCursor, data + first, second);
logCursor = (logCursor + second) % logCapacity;
}
logLength = min(logLength + size, logCapacity);
}
void readLog(void *data, size_t position, size_t size) {
if (logLength == logCapacity) {
position = (logCursor + position) % logCapacity;
}
size_t first = min(size, logCapacity - position);
size_t second = size - first;
memcpy(data, logBuffer + position, first);
if (second > 0) {
memcpy(data + first, logBuffer, second);
}
}
bool isTopicUpdated(const uint8_t topic) {
LogTopic& logTopic = logTopics[topic];
bool updated = false;
for (auto& value : logTopic.values) {
if (value.name == nullptr) break;
float v = value.value.get();
if (!floatEquals(value.lastValue, v)) {
value.lastValue = v;
updated = true;
}
}
return updated;
}
void printLogInfo() {
if (logMemory == -1) return print("Log: disabled\n");
print("Memory: %s\n", logMemory == 0 ? "RAM" : "PSRAM");
print("Usage: %.f%%\n", logUsage * 100);
print("Capacity: %u bytes\n", (unsigned)logCapacity);
print("Used: %u bytes\n", (unsigned)logLength);
print("Estimated duration: %d seconds\n", estimateLogDuration());
}
int estimateLogDuration() {
float bandwidth = 0;
for (LogTopic& topic : logTopics) {
float rate = isinf(topic.throttle) ? loopRate : topic.throttle;
bandwidth += rate * topic.length * sizeof(float);
}
return logCapacity / bandwidth;
}
void printLogHeader() {
int i = 0;
for (auto& topic : logTopics) {
print("Topic #%d (%g Hz):\n", i++, topic.throttle);
for (auto& value : topic.values) {
if (value.name == nullptr) break;
print(" %s%s\n", value.name, value.logged ?"" : " (not logged)");
}
for (int i = 0; i < logColumns; i++) {
print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n");
}
}
void printLogValues(const char *filter) {
for (LogTopic& topic : logTopics) {
for (LogValue& value : topic.values) {
if (value.name == nullptr) break;
if (strncasecmp(value.name, filter, strlen(filter))) continue;
print("%s = %g\n", value.name, value.value.get());
void printLogData() {
for (int i = 0; i < LOG_SIZE; i++) {
if (logBuffer[i][0] == 0) continue; // skip empty records
for (int j = 0; j < logColumns; j++) {
print("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n");
}
}
}
void configLogThrottle(const char *name, float throttle) {
for (LogTopic& topic : logTopics) {
for (LogValue& value : topic.values) {
if (value.name == nullptr) break;
if (strcasecmp(value.name, name) != 0) continue;
topic.throttle = throttle;
print("Log throttle for %s set to %.1f Hz\n", name, throttle);
return;
}
}
print("Log value not found: %s\n", name);
}
void exposeLogValue(const char *name) {
for (int i = 0; i < sizeof(logTopics) / sizeof(logTopics[0]); i++) {
LogTopic& topic = logTopics[i];
for (LogValue& value : topic.values) {
if (value.name == nullptr) break;
if (strcasecmp(value.name, name) != 0) continue;
logExposed = &value;
print("Log value %s exposed\n", name);
return;
}
}
print("Log value not found: %s\n", name);
}
+34
View File
@@ -0,0 +1,34 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Low pass filter implementation
#pragma once
template <typename T> // Using template to make the filter usable for scalar and vector values
class LowPassFilter {
public:
float alpha; // smoothing constant, 1 means filter disabled
T output;
LowPassFilter(float alpha): alpha(alpha) {};
T update(const T input) {
if (!init) {
init = true;
return output = input;
}
return output += alpha * (input - output);
}
void setCutOffFrequency(float cutOffFreq, float dt) {
alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
}
void reset() {
init = false;
}
private:
bool init = false;
};
+18 -83
View File
@@ -10,13 +10,8 @@ extern float controlTime;
extern float voltage;
int mavlinkSysId = 1;
Rate telemetryFast(10);
Rate telemetrySlow(2);
Rate telemetryAttitude(20);
Rate telemetryRC(10);
Rate telemetryMotors(10);
Rate telemetryIMU(15);
Rate telemetryTopic(10);
bool mavlinkConnected = false;
String mavlinkPrintBuffer;
@@ -39,58 +34,42 @@ void sendMavlink() {
((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
mode, MAV_STATE_STANDBY);
sendMessage(&msg);
}
if (!mavlinkConnected) return; // send only heartbeat until connected
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);
sendMessage(&msg);
}
if (telemetrySlow && valid(voltage)) {
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};
uint16_t voltagesExt[] = {0, 0, 0, 0};
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);
if (valid(voltage)) sendMessage(&msg);
}
if (telemetryAttitude) {
if (telemetryFast && mavlinkConnected) {
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);
sendMessage(&msg);
}
if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
if (telemetryMotors) {
float controls[8];
memcpy(controls, motors, sizeof(motors));
mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
sendMessage(&msg);
}
if (telemetryIMU) {
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,
0, 0, 0, 0);
sendMessage(&msg);
}
if (telemetryTopic && logExposed != nullptr) {
mavlink_msg_named_value_float_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
time, logExposed->name, logExposed->value.get());
sendMessage(&msg);
}
}
void sendMessage(const void *msg) {
@@ -99,36 +78,16 @@ void sendMessage(const void *msg) {
sendWiFi(buf, len);
}
static uint8_t mavlinkBatch[ESP_NOW_MAX_DATA_LEN_V2];
static int mavlinkBatchSize = 0;
void batchMessage(const void *msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
int len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
if (mavlinkBatchSize + len > sizeof(mavlinkBatch)) {
sendWiFi(mavlinkBatch, mavlinkBatchSize);
mavlinkBatchSize = 0;
}
memcpy(mavlinkBatch + mavlinkBatchSize, buf, len);
mavlinkBatchSize += len;
}
void flushBatchMessages() {
sendWiFi(mavlinkBatch, mavlinkBatchSize);
mavlinkBatchSize = 0;
}
void receiveMavlink() {
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);
if (len) mavlinkConnected = true;
// New packet, parse it
mavlink_message_t msg;
mavlink_status_t status;
for (int i = 0; i < len; i++) {
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
mavlinkConnected = true;
handleMavlink(&msg);
}
}
@@ -196,13 +155,6 @@ void handleMavlink(const void *_msg) {
sendMessage(&msg);
}
if (msg.msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
mavlink_vision_position_estimate_t m;
mavlink_msg_vision_position_estimate_decode(&msg, &m);
Vector pos(m.x, m.y, m.z);
correctPosition(pos);
}
if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
mavlink_mission_request_list_t m;
mavlink_msg_mission_request_list_decode(&msg, &m);
@@ -230,24 +182,18 @@ void handleMavlink(const void *_msg) {
mavlink_msg_set_attitude_target_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return;
if (!(m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE)) {
// Attitude control
// copy attitude, rates and thrust targets
ratesTarget.x = m.body_roll_rate;
ratesTarget.y = -m.body_pitch_rate; // convert to flu
ratesTarget.z = -m.body_yaw_rate;
attitudeTarget.w = m.q[0];
attitudeTarget.x = m.q[1];
attitudeTarget.y = -m.q[2];
attitudeTarget.z = -m.q[3];
ratesExtra.x = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE ? 0 : m.body_roll_rate;
ratesExtra.y = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE ? 0 : -m.body_pitch_rate; // convert to flu
ratesExtra.z = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE ? 0 : -m.body_yaw_rate;
} else {
// Rates control
attitudeTarget.invalidate();
ratesTarget.x = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE ? ratesTarget.x : m.body_roll_rate;
ratesTarget.y = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE ? ratesTarget.y : -m.body_pitch_rate;
ratesTarget.z = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE ? ratesTarget.z : -m.body_yaw_rate;
}
thrustTarget = m.thrust;
ratesExtra = Vector(0, 0, 0);
thrustTarget = valid(m.thrust) ? m.thrust : thrustTarget;
if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
armed = m.thrust > 0;
}
@@ -265,29 +211,18 @@ void handleMavlink(const void *_msg) {
armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
}
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_LIST) {
const uint32_t qgcEpoch = 1262304000; // qgc accepts only timestamps after 2010-01-01
mavlink_message_t response;
mavlink_msg_log_entry_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
0, 1, 0, qgcEpoch + t * 60, logLength); // put fake unique date to make qgc happy with saving logs
sendMessage(&response);
}
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
mavlink_log_request_data_t m;
mavlink_msg_log_request_data_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return;
for (int i = 0; i < m.count; i += MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
int chunkSize = min(MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN, (int)(m.count - i));
mavlink_message_t response;
uint8_t data[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN];
readLog(data, m.ofs + i, chunkSize);
mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
m.id, m.ofs + i, chunkSize, data);
batchMessage(&response);
// Send all log records
for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
mavlink_message_t msg;
mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
sendMessage(&msg);
}
flushBatchMessages();
}
// Handle commands
+6 -8
View File
@@ -7,7 +7,7 @@
float motors[4]; // normalized motor thrusts in range [0..1]
int motorPins[4] = {-1, -1, -1, -1}; // default pin numbers
int motorPins[4] = {12, 13, 14, 15}; // default pin numbers
int pwmFrequency = 78000;
int pwmResolution = 10;
int pwmStop = 0;
@@ -17,26 +17,24 @@ 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() {
print("Setup motors\n");
// Configure pins
print("Setup Motors\n");
// configure pins
for (int i = 0; i < 4; i++) {
if (motorPins[i] < 0) continue; // skip unassigned motors
ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // when reconfiguring
}
sendMotors();
print("Motors initialized\n");
}
void sendMotors() {
for (int i = 0; i < 4; i++) {
if (motorPins[i] < 0) continue; // skip unassigned motors
ledcWrite(motorPins[i], getDutyCycle(motors[i]));
}
}
int getDutyCycle(float value) {
value = constrain(value, 0, 1);
if (pwmMax >= 0) { // pwm mode
float pwm = mapf(value, 0, 1, pwmMin, pwmMax);
if (value == 0) pwm = pwmStop;
@@ -51,9 +49,9 @@ bool motorsActive() {
return motors[0] != 0 || motors[1] != 0 || motors[2] != 0 || motors[3] != 0;
}
void testMotor(int n, float thrust) {
void testMotor(int n) {
print("Testing motor %d\n", n);
motors[n] = thrust;
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
sendMotors();
pause(3);
+9 -46
View File
@@ -5,16 +5,14 @@
#include <Preferences.h>
#include "util.h"
#include "pid.h"
extern int channelZero[16], channelMax[16];
extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
extern int rcRxPin, voltagePin;
extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel;
extern float rcLossTimeout, descendTime, disarmTilt;
extern float rcLossTimeout, descendTime;
extern float voltageScale;
extern LowPassFilter<float> voltageFilter;
extern PID posX_PID, posY_PID;
Preferences storage;
@@ -22,7 +20,6 @@ struct Parameter {
const char *name; // max length is 15
bool integer;
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) {};
@@ -61,12 +58,6 @@ Parameter parameters[] = {
{"CTL_FLT_MODE_0", &flightModes[0]},
{"CTL_FLT_MODE_1", &flightModes[1]},
{"CTL_FLT_MODE_2", &flightModes[2]},
// position
{"POS_XY_P", &posX_PID.p, setupPosition},
{"POS_XY_I", &posX_PID.i, setupPosition},
{"POS_XY_D", &posX_PID.d, setupPosition},
{"POS_XY_WU", &posX_PID.windup, setupPosition},
{"POS_XY_D_A", &posX_PID.lpf.alpha, setupPosition},
// imu
{"IMU_ROT_ROLL", &imuRotation.x},
{"IMU_ROT_PITCH", &imuRotation.y},
@@ -82,8 +73,6 @@ Parameter parameters[] = {
{"EST_ACC_WEIGHT", &accWeight},
{"EST_LVL_WEIGHT", &levelWeight},
{"EST_RATES_LPF_A", &ratesFilter.alpha},
{"EST_RATES_NF_F", &ratesNotch.frequency, setupEstimate},
{"EST_RATES_NF_BW", &ratesNotch.bandwidth, setupEstimate},
// motors
{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},
{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},
@@ -127,26 +116,7 @@ Parameter parameters[] = {
// 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},
{"MAV_RATE_TOPIC", &telemetryTopic.rate},
// log
{"LOG_MEMORY", &logMemory, setupLog},
{"LOG_USAGE", &logUsage, setupLog},
{"LOG_RATE_000", &logTopics[0].throttle},
{"LOG_RATE_001", &logTopics[1].throttle},
{"LOG_RATE_002", &logTopics[2].throttle},
{"LOG_RATE_003", &logTopics[3].throttle},
{"LOG_RATE_004", &logTopics[4].throttle},
{"LOG_RATE_005", &logTopics[5].throttle},
{"LOG_RATE_006", &logTopics[6].throttle},
{"LOG_RATE_007", &logTopics[7].throttle},
{"LOG_RATE_008", &logTopics[8].throttle},
{"LOG_RATE_009", &logTopics[9].throttle},
{"LOG_RATE_010", &logTopics[10].throttle},
{"LOG_RATE_011", &logTopics[11].throttle},
{"MAV_RATE_FAST", &telemetryFast.rate},
// power
{"PWR_VOLT_PIN", &voltagePin, setupPower},
{"PWR_VOLT_SCALE", &voltageScale},
@@ -154,7 +124,6 @@ Parameter parameters[] = {
// safety
{"SF_RC_LOSS_TIME", &rcLossTimeout},
{"SF_DESCEND_TIME", &descendTime},
{"SF_DISARM_TILT", &disarmTilt}
};
void setupParameters() {
@@ -162,10 +131,10 @@ void setupParameters() {
storage.begin("flix");
// Read parameters from storage
for (auto &parameter : parameters) {
parameter.inital = parameter.getValue();
if (storage.isKey(parameter.name)) {
parameter.setValue(storage.getFloat(parameter.name));
if (!storage.isKey(parameter.name)) {
storage.putFloat(parameter.name, parameter.getValue()); // store default value
}
parameter.setValue(storage.getFloat(parameter.name, 0));
parameter.cache = parameter.getValue();
}
}
@@ -211,23 +180,17 @@ void syncParameters() {
if (motorsActive()) return; // don't use flash while flying, it may cause a delay
for (auto &parameter : parameters) {
if (floatEquals(parameter.getValue(), parameter.cache)) continue; // no change
if (parameter.getValue() == parameter.cache) continue; // no change
if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NAN
storage.putFloat(parameter.name, parameter.getValue());
parameter.cache = parameter.getValue(); // update cache
}
}
void printParameters(const char *filter) {
print("Name Value [Default]\n");
void printParameters() {
for (auto &parameter : parameters) {
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);
}
print("%s = %g\n", parameter.name, parameter.getValue());
}
}
+1 -1
View File
@@ -5,7 +5,7 @@
#pragma once
#include "filter.h"
#include "lpf.h"
class PID {
public:
-95
View File
@@ -1,95 +0,0 @@
#include "vector.h"
#include "quaternion.h"
#include "pid.h"
#include "util.h"
float mass = 0.065;
float motorThrust = 0.031 * ONE_G;
float positionWeight = 0.9;
float velocityWeight = 0.9;
PID velX_PID(1.0, 0.0, 0.02);
PID velY_PID(1.0, 0.0, 0.02);
PID velZ_PID(1.0, 0.0, 0.02);
// PID posX_PID(1.0, 0.0, 0.0);
// PID posY_PID(1.0, 0.0, 0.0);
// PID posZ_PID(1.0, 0.0, 0.0);
PID posX_PID(0.1, 0.0, 0.0);
PID posY_PID(0.1, 0.0, 0.0);
PID posZ_PID(0.1, 0.0, 0.0);
Vector acceleration;
Vector velocity;
Vector position;
Vector velocityTarget(NAN, NAN, NAN);
Vector positionTarget(NAN, NAN, NAN);
extern Quaternion attitudeTarget;
extern float thrustTarget;
void setupPosition() {
// TODO: posY_PID = posX_PID;
// posX_PID.copy(posY_PID);
posY_PID.p = posX_PID.p;
posY_PID.i = posX_PID.i;
posY_PID.d = posX_PID.d;
}
void estimatePosition() {
acceleration = Quaternion::rotateVector(acceleration, attitude) + Vector(0, 0, -ONE_G);
velocity += acceleration * dt;
position += velocity * dt;
}
void correctPosition(Vector& pos) {
// position += (pos - position) * positionWeight;
position = pos;
// positionUpdated = true;
if (invalid(positionTarget.x) || invalid(positionTarget.y)) return;
attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x));
attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y));
}
void correctVelocity(Vector& vel) {
velocity += (vel - velocity) * velocityWeight;
}
void controlVelocity() {
Vector accelerationTarget(
velX_PID.update(velocityTarget.x - velocity.x),
velY_PID.update(velocityTarget.y - velocity.y),
velZ_PID.update(velocityTarget.z - velocity.z)
);
Vector thrustVector = (accelerationTarget + Vector(0, 0, ONE_G)) * mass;
const Vector up(0, 0, 1);
attitudeTarget = Quaternion::fromBetweenVectors(up, thrustVector);
float maxThrust = motorThrust * 4; // 4 motors
thrustTarget = thrustVector.norm() / maxThrust;
}
void controlPosition() {
if (positionTarget.invalid()) return;
velocityTarget = Vector(
posX_PID.update(positionTarget.x - position.x),
posY_PID.update(positionTarget.y - position.y),
posZ_PID.update(positionTarget.z - position.z)
);
controlVelocity();
}
void controlPositionSimple() {
if (positionTarget.invalid()) return;
// straight attitude target control
attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x));
attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y));
}
+2 -3
View File
@@ -5,11 +5,11 @@
#include <soc/soc.h>
#include <soc/rtc_cntl_reg.h>
#include "filter.h"
#include "lpf.h"
#include "util.h"
float voltage = NAN;
LowPassFilter<float> voltageFilter(1);
LowPassFilter<float> voltageFilter(0.2);
int voltagePin = -1;
float voltageScale = 2;
@@ -20,7 +20,6 @@ void setupPower() {
void readVoltage() {
if (voltagePin < 0) return;
static Rate rate(10);
if (!rate) return;
+5 -4
View File
@@ -27,13 +27,15 @@ void setupRC() {
bool readRC() {
if (rcRxPin < 0) return false;
if (!rc.read()) return false;
rc.getChannels(channels);
if (rc.read()) {
SBUSData data = rc.data();
for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
normalizeRC();
controlTime = t;
return true;
}
return false;
}
void normalizeRC() {
float controls[16];
@@ -53,7 +55,6 @@ void calibrateRC() {
print("RC_RX_PIN = %d, set the RC pin!\n", rcRxPin);
return;
}
uint16_t zero[16]; // for zero positions
uint16_t center[16]; // for center positions
uint16_t _[16]; // for unused data
+1 -15
View File
@@ -8,12 +8,10 @@ extern float controlRoll, controlPitch, controlThrottle, controlYaw;
float rcLossTimeout = 1;
float descendTime = 10;
float disarmTilt = radians(120);
void failsafe() {
rcLossFailsafe();
autoFailsafe();
tiltFailsafe();
}
// RC loss failsafe
@@ -38,7 +36,7 @@ void descend() {
// Allow pilot to interrupt automatic flight
void autoFailsafe() {
static float roll, pitch, yaw, throttle;
if (abs(roll - controlRoll) > 0.05 || abs(pitch - controlPitch) > 0.05 || abs(yaw - controlYaw) > 0.05 || abs(throttle - controlThrottle) > 0.05) {
if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
// controls changed and mode switch is not configured
if (mode == AUTO && invalid(controlMode)) mode = STAB; // regain control by the pilot
}
@@ -47,15 +45,3 @@ void autoFailsafe() {
yaw = controlYaw;
throttle = controlThrottle;
}
// Disarm if tilted too much
void tiltFailsafe() {
if (!armed) return;
if (mode != STAB) return;
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
float tilt = acos(up.z);
if (disarmTilt && tilt > disarmTilt) {
armed = false;
}
}
+4 -57
View File
@@ -23,12 +23,6 @@ bool valid(float 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)
float wrapAngle(float angle) {
angle = fmodf(angle, 2 * PI);
@@ -53,60 +47,16 @@ void splitString(String& str, String& token0, String& token1, String& token2) {
if (token2.c_str() == NULL) token2 = "";
}
// Simplified ESP-NOW Serial without resends
// Simplified ESP-NOW Serial without tx buffering and 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
void onSent(bool success) override {} // disable resends
size_t write(const uint8_t *data, size_t len) override {
return ESP_NOW_Peer::send(data, len); // pure send without buffering
}
};
// Simple variant type for logging and parameters
struct Value {
enum { EMPTY, FLOAT, INT, BOOL, FLOAT_FN, INT_FN, BOOL_FN } type;
union {
void *pointer;
float *_float;
int *_int;
bool *_bool;
float (*floatFn)();
int (*intFn)();
bool (*boolFn)();
};
Value() : type(EMPTY), pointer(nullptr) {};
Value(float *pt) : type(FLOAT), _float(pt) {};
Value(int *pt) : type(INT), _int(pt) {};
Value(bool *pt) : type(BOOL), _bool(pt) {};
Value(float (*fn)()) : type(FLOAT_FN), floatFn(fn) {};
Value(int (*fn)()) : type(INT_FN), intFn(fn) {};
Value(bool (*fn)()) : type(BOOL_FN), boolFn(fn) {};
float get() const {
switch (type) {
case FLOAT: return *_float;
case INT: return *_int;
case BOOL: return *_bool ? 1 : 0;
case FLOAT_FN: return floatFn();
case INT_FN: return intFn();
case BOOL_FN: return boolFn() ? 1 : 0;
default: return NAN;
}
};
void set(float value) const {
switch (type) {
case FLOAT: *_float = value; break;
case INT: *_int = value; break;
case BOOL: *_bool = (value != 0); break;
default: break;
}
};
};
// Rate limiter
class Rate {
public:
@@ -115,9 +65,6 @@ public:
Rate(float rate) : rate(rate) {}
operator bool() {
if (t == last) {
return true; // the same step
}
if (t - last >= 1 / rate) {
last = t;
return true;
+2 -21
View File
@@ -56,9 +56,8 @@ void setupWiFi() {
void sendWiFi(const uint8_t *buf, int len) {
if (espnow) {
espnow.write(buf, len);
static Rate discovery(2);
if (espnow.isEncrypted() && discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
if (discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
return;
}
@@ -89,8 +88,7 @@ void printWiFiInfo() {
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", WiFi.channel());
print("Lost packets: %d\n", espnow.lost);
print("Channel: %d\n", espnow.getChannel());
} else if (WiFi.getMode() == WIFI_MODE_AP) {
print("Mode: Access Point (AP)\n");
print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
@@ -133,20 +131,3 @@ void configWiFi(int mode, const char *first, const char *second) {
}
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]);
}
-11
View File
@@ -20,10 +20,6 @@
#define radians(deg) ((deg)*DEG_TO_RAD)
#define degrees(rad) ((rad)*RAD_TO_DEG)
#define MALLOC_CAP_SPIRAM (1<<10)
#define MALLOC_CAP_8BIT (1<<2)
#define ESP_NOW_MAX_DATA_LEN_V2 1470
#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; }
@@ -160,8 +156,6 @@ HardwareSerial Serial, Serial1, Serial2;
class EspClass {
public:
void restart() { Serial.println("Ignore reboot in simulation"); }
uint32_t getFreeHeap() { return 300 * 1024; } // assume 300 KB free heap
uint32_t getFreePsram() { return 8 * 1024 * 1024; } // assume 8 MB free PSRAM
} ESP;
unsigned long __delayTime = 0;
@@ -171,16 +165,11 @@ void delay(uint32_t ms) {
__delayTime += ms * 1000;
}
void *heap_caps_calloc(size_t n, size_t size, uint32_t caps) {
return calloc(n, size);
}
bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { 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; }
float temperatureRead() { return 0; }
unsigned long __micros;
unsigned long __resetTime = 0;
+5 -4
View File
@@ -15,11 +15,12 @@ public:
SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
void begin(int rxpin = -1, int txpin = -1, bool inv = true, bool fast = false) {};
bool read() { return joystickInit(); };
void getChannels(uint16_t (&channels)[16]) const {
int16_t ch[16];
joystickGet(ch);
SBUSData data() {
SBUSData data;
joystickGet(data.ch);
for (int i = 0; i < 16; i++) {
channels[i] = map(ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
data.ch[i] = map(data.ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
}
return data;
};
};
+5 -20
View File
@@ -9,7 +9,7 @@
#include "quaternion.h"
#include "Arduino.h"
#include "wifi.h"
#include "filter.h"
#include "lpf.h"
extern float t, dt;
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -38,7 +38,7 @@ const char* getModeName();
void sendMotors();
int getDutyCycle(float value);
bool motorsActive();
void testMotor(int, float);
void testMotor(int n);
void print(const char* format, ...);
void pause(float duration);
void doCommand(String str, bool echo);
@@ -48,17 +48,8 @@ void normalizeRC();
void calibrateRC();
void calibrateRCChannel(int*, uint16_t[16], uint16_t[16], const char*);
void printRCCalibration();
void loopLog();
void resetLog();
void writeLog(const void *data, size_t size);
void readLog(void *data, size_t position, size_t size);
bool isTopicUpdated(const uint8_t topic);
void printLogInfo();
int estimateLogDuration();
void printLogHeader();
void printLogValues(const char *filter);
void configLogThrottle(const char *name, float throttle);
void exposeLogValue(const char *name);
void printLogData();
void processMavlink();
void sendMavlink();
void sendMessage(const void *msg);
@@ -72,25 +63,19 @@ void failsafe();
void rcLossFailsafe();
void descend();
void autoFailsafe();
void tiltFailsafe();
int parametersCount();
const char *getParameterName(int index);
float getParameter(int index);
float getParameter(const char *name);
bool setParameter(const char *name, const float value);
void printParameters(const char *filter);
void printParameters();
void resetParameters();
// mocks
void setLED(bool on) {};
void calibrateGyro() { print("Skip gyro calibrating\n"); };
void calibrateAccel() { print("Skip accel calibrating\n"); };
void printIMUCalibration() { print("cal: N/A\n"); };
void printIMUInfo() {};
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"); };
class IMU {
public:
float getTemp() { return 0; }
} imu;
+2 -4
View File
@@ -23,7 +23,7 @@
#include "estimate.ino"
#include "safety.ino"
#include "log.ino"
#include "filter.h"
#include "lpf.h"
#include "mavlink.ino"
#include "motors.ino"
#include "parameters.ino"
@@ -55,8 +55,6 @@ public:
initNode();
Serial.begin(0);
setupParameters();
setupLog();
rcRxPin = 1; // set rc pin to enable rc reading
gzmsg << "Flix plugin loaded" << endl;
}
@@ -89,7 +87,7 @@ public:
applyMotorForces();
publishTopics();
loopLog();
logData();
syncParameters();
}
+1 -1
View File
@@ -14,7 +14,7 @@
// Mocks
int wifiMode = 1;
int wifiLongRange = 0;
int espnowChannel = 6;
// int espnowChannel = 6;
const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
int udpLocalPort = 14580;
+1 -10
View File
@@ -9,7 +9,6 @@ Usage:
import csv
import json
import docopt
import math
from mcap.writer import Writer
args = docopt.docopt(__doc__)
@@ -40,15 +39,7 @@ channel_id = writer.register_channel(
)
for row in csv_reader:
if row[0] == '': continue
data = {}
for key, value in zip(header, row):
if value == '' or math.isnan(float(value)):
data[key] = None
else:
data[key] = float(value)
data = {key: float(value) if value != '' else None for key, value in zip(header, row)}
data = {key: float(value) for key, value in zip(header, row)}
timestamp = round(float(row[0]) * 1e9)
writer.add_message(channel_id=channel_id, log_time=timestamp, data=json.dumps(data).encode(), publish_time=timestamp,)
+10 -30
View File
@@ -11,22 +11,18 @@
#include <Preferences.h>
#include "../../flix/util.h"
const bool DISABLE_SWARM = true;
const int CHANNEL = -1; // -1 means auto search
const int CHANNEL = 6;
char key[ESP_NOW_KEY_LEN + 1] = {0}; // with trailing null
Preferences storage;
std::vector<ESPNOWSerial *> peers;
bool stop = false;
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
if (stop) return;
Serial.printf("New peer: " MACSTR "\n", MAC2STR(info->src_addr));
ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, WiFi.channel(), WIFI_IF_STA);
ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, CHANNEL, WIFI_IF_AP);
link->begin();
link->setKey((const uint8_t *)key);
peers.push_back(link);
@@ -34,8 +30,9 @@ void onNewPeer(const esp_now_recv_info_t *info, const uint8_t *data, int len, vo
void setup() {
Serial.begin(115200);
WiFi.mode(WIFI_STA);
WiFi.mode(WIFI_AP);
WiFi.setSleep(false);
WiFi.setChannel(CHANNEL);
ESP_NOW.onNewPeer(onNewPeer, NULL);
ESP_NOW.begin();
@@ -46,6 +43,12 @@ void setup() {
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() {
@@ -58,20 +61,6 @@ void generateRandomKey() {
void loop() {
uint8_t buf[5000];
static int channelIndex = 0;
static const int channels[] = {6, 11, 1, 2, 6, 11, 3, 4, 5, 6, 1, 11, 8, 6, 9, 10, 11, 6, 1, 12, 13}; // 6, 1 and 11 are most common
static unsigned long last = 0;
if (!stop && millis() - last > 500) {
// Change search channel
last = millis();
channelIndex = (channelIndex + 1) % (sizeof(channels) / sizeof(channels[0]));
int channel = CHANNEL < 0 ? channels[channelIndex] : CHANNEL;
Serial.printf("Run on Flix: espnow %s %s\n", WiFi.STA.macAddress().c_str(), key);
Serial.printf("Searching channel %d\n", channel);
WiFi.setChannel(channel);
}
// Send from Serial to ESP-NOW
while (Serial.available() > 0) {
int b = Serial.read();
@@ -92,15 +81,6 @@ void loop() {
// Send from ESP-NOW to Serial
for (ESPNOWSerial *link : peers) {
int len = link->read(buf, sizeof(buf));
if (!stop) {
for (int i = 0; i < len; i++) {
if (buf[i] == MAVLINK_STX) {
// Got MAVLink message, stop discovery
Serial.printf("Received MAVLink from " MACSTR "\n", MAC2STR(link->addr()));
if (DISABLE_SWARM) stop = true;
}
}
}
if (len > 0) {
Serial.write(buf, len);
}
-76
View File
@@ -1,76 +0,0 @@
#!/usr/bin/env python3
"""Convert flight from Flix format to CSV
Usage:
log_to_csv.py <input_file>
"""
import os
from pyflix import Flix
import docopt
import struct
import csv
DIR = os.path.dirname(os.path.realpath(__file__))
HEADER_FILE = os.path.join(DIR, 'log/log_header.txt')
# Read log header
try:
# read from file
header = open(HEADER_FILE, 'r').read()
except FileNotFoundError:
flix = Flix()
header = flix.cli('log header') # receive the log schema
open(HEADER_FILE, 'w').write(header) # save to file
# Parse log header
topics = []
for line in header.splitlines():
if not line.startswith(' '):
topics.append([])
elif 'not logged' not in line:
topics[-1].append(line.strip())
# Read log file
args = docopt.docopt(__doc__)
input_file = args['<input_file>']
outfile_file = input_file + '.csv'
data = open(input_file, 'rb').read()
# Search for sync marker
SYNC_MARKER = bytes.fromhex('1A 91 4F F6 7F')
sync_offset = data.find(SYNC_MARKER)
if sync_offset == -1:
raise ValueError('Sync marker not found in log file')
data = data[sync_offset + len(SYNC_MARKER):]
data = data.replace(SYNC_MARKER, b'') # remove all other sync markers
header_row = [f'{value}' for topic in topics for value in topic]
rows = []
offset = 0
while offset < len(data):
try:
topic = struct.unpack_from('B', data, offset)[0]
offset += 1
if topic >= len(topics):
raise ValueError(f'Invalid topic {topic} at offset {offset}')
if topic == 0 or not rows:
rows.append({})
for name in topics[topic]:
value = struct.unpack_from('<f', data, offset)[0]
offset += 4
rows[-1][name] = value
except struct.error as e:
break
# Write CSV file
with open(outfile_file, 'w', newline='') as f:
writer = csv.DictWriter(f, fieldnames=header_row, extrasaction='ignore')
writer.writeheader()
rows = filter(lambda row: float(row.get('t', 0)), rows)
writer.writerows(rows)
-9
View File
@@ -28,8 +28,6 @@ from pyflix import Flix
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
Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware code:
@@ -222,13 +220,6 @@ The following scripts demonstrate how to use the library:
* [`log.py`](../log.py) — download flight logs from the drone.
* [`example.py`](../example.py) — a simple example, prints telemetry data and waits for events.
> [!TIP]
> Set `FLIX_DEVICE` environment variable to use these tools with ESP-NOW connection, for example:
>
> ```bash
> FLIX_DEVICE=/dev/cu.usbserial-0001 tools/cli.py
> ```
## Advanced usage
### MAVLink
+1 -6
View File
@@ -44,17 +44,12 @@ class Flix:
_print_buffer: str = ''
_modes = ['RAW', 'ACRO', 'STAB', 'AUTO']
def __init__(self, system_id: int=1, wait_connection: bool=True, device=os.getenv('FLIX_DEVICE')):
def __init__(self, system_id: int=1, wait_connection: bool=True):
if not (0 <= system_id < 256):
raise ValueError('system_id must be in range [0, 255]')
self._setup_mavlink()
self.system_id = system_id
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:
# Direct connection
logger.debug('Listening on port 14550')
+1 -1
View File
@@ -1,6 +1,6 @@
[project]
name = "pyflix"
version = "0.16"
version = "0.15"
description = "Python API for Flix drone"
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
license = "MIT"