mirror of
https://github.com/okalachev/flix.git
synced 2026-08-16 00:38:56 +00:00
Compare commits
1
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Oleg Kalachev
|
b235825f3d |
@@ -23,12 +23,8 @@ jobs:
|
||||
with:
|
||||
name: firmware-binary
|
||||
path: flix/build
|
||||
- name: Build firmware for ESP32-C3
|
||||
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
|
||||
|
||||
|
||||
+2
-3
@@ -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
|
||||
|
||||
Vendored
+21
-21
@@ -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": [
|
||||
|
||||
Vendored
-1
@@ -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"
|
||||
|
||||
@@ -1,40 +1,29 @@
|
||||
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*))
|
||||
PORT := $(wildcard /dev/serial/by-id/usb-Silicon_Labs_CP21* /dev/serial/by-id/usb-1a86_USB_Single_Serial_* /dev/cu.usbserial-*)
|
||||
PORT := $(strip $(PORT))
|
||||
|
||||
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
|
||||
|
||||
upload_proxy: .core .libs
|
||||
arduino-cli compile --fqbn $(BOARD) tools/espnow-proxy
|
||||
arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" tools/espnow-proxy
|
||||
touch .dependencies
|
||||
|
||||
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 +38,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
|
||||
|
||||
@@ -21,8 +21,8 @@
|
||||
* Dedicated for education and research.
|
||||
* Made from general-purpose components.
|
||||
* Simple and clean source code in Arduino (<2k lines firmware).
|
||||
* Communication using MAVLink protocol over Wi-Fi or ESP-NOW.
|
||||
* Control with USB gamepad, remote control or smartphone.
|
||||
* Connectivity using Wi-Fi and MAVLink protocol.
|
||||
* Control using USB gamepad, remote control or smartphone.
|
||||
* Wireless command line interface and analyzing.
|
||||
* Precise simulation with Gazebo.
|
||||
* Python library for scripting and automatic flights.
|
||||
@@ -47,21 +47,13 @@ See the [user builds gallery](docs/user.md):
|
||||
|
||||
<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
|
||||
|
||||
The simulator is implemented using Gazebo and runs the original Arduino code:
|
||||
|
||||
<img src="docs/img/simulator1.png" width=500 alt="Flix simulator">
|
||||
|
||||
## Documentation articles
|
||||
## Documentation
|
||||
|
||||
1. [Assembly instructions](docs/assembly.md).
|
||||
2. [Usage: build, setup and flight](docs/usage.md).
|
||||
@@ -79,14 +71,14 @@ Additional articles:
|
||||
|
||||
|Type|Part|Image|Quantity|
|
||||
|-|-|:-:|:-:|
|
||||
|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|
|
||||
|*Boost converter (optional, for more stable power supply)*|*5V output*|<img src="docs/img/buck-boost.jpg" width=100>|1|
|
||||
|Microcontroller board|ESP32 Mini|<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|
|
||||
|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|
|
||||
|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|
|
||||
|Propeller|55 mm (alternatively 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|
|
||||
|Pull-down resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|4|
|
||||
|3.7V Li-Po battery|LW 952540 (or any compatible by the size)|<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|
|
||||
|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|
|
||||
@@ -160,16 +152,16 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir
|
||||
|-|-|
|
||||
|GND|GND|
|
||||
|VIN|VCC (or 3.3V depending on the receiver)|
|
||||
|Signal (TX)|GPIO4|
|
||||
|Signal (TX)|GPIO4¹|
|
||||
|
||||
* Optionally connect the battery voltage divider for voltage monitoring to any ADC1 pin (e. g. *GPIO32* on ESP32, *GPIO3* on ESP32-S3).
|
||||
*¹ — 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.*
|
||||
|
||||
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.
|
||||
* Optionally connect the battery voltage divider for voltage monitoring to any ADC1 pin (e. g. *GPIO32* on ESP32, *GPIO3* on ESP32S3).
|
||||
|
||||
## Resources
|
||||
|
||||
* Telegram channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter.
|
||||
* Official Telegram chat: https://t.me/opensourcequadcopterchat (English / Russian).
|
||||
* Official Telegram chat: https://t.me/opensourcequadcopterchat.
|
||||
* Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/.
|
||||
|
||||
## Disclaimer
|
||||
|
||||
@@ -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
|
||||
@@ -28,8 +28,6 @@ Soldered components ([schematics variant](https://miro.com/app/board/uXjVN-dTjoo
|
||||
|
||||
<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
|
||||
|
||||
> [!WARNING]
|
||||
|
||||
@@ -67,38 +67,6 @@ 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.
|
||||
|
||||
### 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
|
||||
|
||||
See build instructions in [usage.md](usage.md).
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 46 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 101 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 33 KiB After Width: | Height: | Size: 23 KiB |
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: 60 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 38 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 50 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 |
+8
-14
@@ -5,32 +5,27 @@
|
||||
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 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 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 the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
|
||||
|
||||
## The drone doesn't fly
|
||||
|
||||
Do the following:
|
||||
|
||||
* **Check the battery voltage**. Use a multimeter to measure the battery voltage. The fully charged battery should have about 4.2V.
|
||||
* **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 the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
|
||||
* **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.
|
||||
* **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)*.
|
||||
* **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.
|
||||
* **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:
|
||||
* The `status` field should be `OK`.
|
||||
* The `rate` field should be about 1000 (Hz).
|
||||
* The `accel` and `gyro` fields should change as you move the drone.
|
||||
* **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 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 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:
|
||||
* `mfr` — should rotate front right motor (counter-clockwise).
|
||||
@@ -38,10 +33,9 @@ Do the following:
|
||||
* `mrl` — should rotate rear left motor (counter-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:
|
||||
|
||||
<img src="img/user/peter_ukhov-2/1.jpg" width="200">
|
||||
|
||||
* **If using an SBUS receiver**:
|
||||
* **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**:
|
||||
* **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.
|
||||
* **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.
|
||||
|
||||
+23
-95
@@ -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]
|
||||
@@ -77,26 +70,9 @@ In case if using different IMU model than MPU9250, change `imu` variable declara
|
||||
|
||||
```cpp
|
||||
ICM20948 imu(SPI); // For ICM-20948
|
||||
// or
|
||||
MPU6050 imu(Wire); // For MPU-6050
|
||||
```
|
||||
|
||||
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();
|
||||
```
|
||||
|
||||
### Connect using QGroundControl
|
||||
|
||||
QGroundControl is a ground control station software that can be used to monitor and control the drone.
|
||||
@@ -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">
|
||||
|
||||
@@ -139,9 +112,9 @@ You can also work with parameters using `p` command in the console. Parameter na
|
||||
|
||||
### Define IMU orientation
|
||||
|
||||
The IMU orientation (relative to the drone's axes) is defined using the parameters: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
|
||||
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 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:
|
||||
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:
|
||||
|
||||
<img src="img/imu-axes.png" width="200">
|
||||
|
||||
@@ -149,10 +122,10 @@ Use the following table to set the parameters for common IMU orientations:
|
||||
|
||||
|Orientation|Parameters|Orientation|Parameters|
|
||||
|:-:|-|-|-|
|
||||
|<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-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-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-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-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
|
||||
|
||||
@@ -165,8 +138,6 @@ Before flight you need to calibrate the accelerometer:
|
||||
|
||||
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).
|
||||
@@ -197,7 +168,7 @@ After this setup, you should see the battery voltage in QGroundControl top panel
|
||||
|
||||
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. Use the following commands **— remove the propellers before running the tests!**
|
||||
|
||||
@@ -215,22 +186,10 @@ 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
|
||||
|
||||
#### 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.
|
||||
2. Power the drone using the battery.
|
||||
3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
||||
@@ -243,11 +202,11 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
|
||||
|
||||
### Control with a remote control
|
||||
|
||||
If using SBUS-connected remote control you need to enable SBUS and calibrate it:
|
||||
Before using SBUS-connected remote control you need to enable SBUS and calibrate it:
|
||||
|
||||
1. Connect to the drone using QGroundControl.
|
||||
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. Check if the receiver is working using `rc` command in the console.
|
||||
3. Reboot the drone to apply changes.
|
||||
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!
|
||||
|
||||
@@ -260,7 +219,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
|
||||
@@ -286,7 +245,7 @@ When finished flying, **disarm** the drone, moving the left stick to the bottom
|
||||
|
||||
### Flight modes
|
||||
|
||||
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*).
|
||||
Flight mode is changed using mode switch on the remote control (if configured) or using the console commands. The main flight mode is *STAB*.
|
||||
|
||||
#### STAB
|
||||
|
||||
@@ -307,7 +266,7 @@ In this mode, the pilot controls the angular rates. This control method is diffi
|
||||
|
||||
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 and mode switch is not configured, the drone will switch back to *STAB* mode.
|
||||
If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
|
||||
|
||||
## Wi-Fi configuration
|
||||
|
||||
@@ -317,8 +276,11 @@ The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the
|
||||
|
||||
* `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.
|
||||
* `2` — Client mode *(STA)* — the drone connects to an existing Wi-Fi network.
|
||||
* `3` — *ESP-NOW (not implemented yet)*.
|
||||
|
||||
> [!WARNING]
|
||||
> Tests showed that Client mode may cause **additional delays** in remote control (due to retranslations), so it's generally not recommended.
|
||||
|
||||
The SSID and password are configured using the `ap` and `sta` console commands:
|
||||
|
||||
@@ -340,43 +302,9 @@ 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 in Arduino IDE 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).
|
||||
|
||||
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*, 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.
|
||||
|
||||
## 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
|
||||
|
||||
@@ -4,62 +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).
|
||||
|
||||
<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.
|
||||
@@ -97,17 +41,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.
|
||||
|
||||
+33
-57
@@ -6,11 +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;
|
||||
extern const int W_AP, W_STA, W_ESPNOW;
|
||||
extern float t, dt, loopRate;
|
||||
extern uint16_t channels[16];
|
||||
extern float controlTime;
|
||||
@@ -20,50 +19,42 @@ extern LowPassFilter<Vector> gyroBiasFilter;
|
||||
extern float voltage;
|
||||
|
||||
const char* motd =
|
||||
"\nWelcome to\n"
|
||||
" _______ __ __ ___ ___\n"
|
||||
"| ____|| | | | \\ \\ / /\n"
|
||||
"| |__ | | | | \\ V /\n"
|
||||
"| __| | | | | > <\n"
|
||||
"| | | `----.| | / . \\\n"
|
||||
"|__| |_______||__| /__/ \\__\\\n\n"
|
||||
"(C) Oleg Kalachev\n"
|
||||
"https://github.com/okalachev/flix\n\n"
|
||||
"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"
|
||||
"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 +89,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 +106,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") {
|
||||
@@ -146,54 +139,37 @@ 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());
|
||||
configWiFi(true, 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());
|
||||
configWiFi(false, arg0.c_str(), arg1.c_str());
|
||||
} 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("Firmware: " __DATE__ " " __TIME__ "\n");
|
||||
print("Free heap: %d\n", ESP.getFreeHeap());
|
||||
// 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;
|
||||
@@ -202,7 +178,7 @@ void doCommand(String str, bool echo = false) {
|
||||
String core = systemState[i].xCoreID == tskNO_AFFINITY ? "*" : String(systemState[i].xCoreID);
|
||||
int cpuPercentage = systemState[i].ulRunTimeCounter / (totalRunTime / 100);
|
||||
print("%-5d%-20s%-7d%-6d%-6s%d\n",systemState[i].xTaskNumber, systemState[i].pcTaskName,
|
||||
systemState[i].usStackHighWaterMark, systemState[i].uxCurrentPriority, core.c_str(), cpuPercentage);
|
||||
systemState[i].usStackHighWaterMark, systemState[i].uxCurrentPriority, core, cpuPercentage);
|
||||
}
|
||||
delete[] systemState;
|
||||
#endif
|
||||
@@ -227,7 +203,7 @@ void handleInput() {
|
||||
|
||||
while (Serial.available()) {
|
||||
char c = Serial.read();
|
||||
if (c == '\n' || c == '\r') {
|
||||
if (c == '\n') {
|
||||
doCommand(input);
|
||||
input.clear();
|
||||
} else {
|
||||
|
||||
+2
-16
@@ -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
|
||||
@@ -67,7 +67,7 @@ void control() {
|
||||
|
||||
void interpretControls() {
|
||||
if (controlMode < 0.25) mode = flightModes[0];
|
||||
else if (controlMode <= 0.75) mode = flightModes[1];
|
||||
else if (controlMode < 0.75) mode = flightModes[1];
|
||||
else if (controlMode > 0.75) mode = flightModes[2];
|
||||
|
||||
if (mode == AUTO) return; // pilot is not effective in AUTO mode
|
||||
@@ -149,26 +149,12 @@ 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);
|
||||
motors[1] = constrain(motors[1], 0, 1);
|
||||
motors[2] = constrain(motors[2], 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() {
|
||||
switch (mode) {
|
||||
case RAW: return "RAW";
|
||||
|
||||
+3
-21
@@ -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
|
||||
@@ -13,25 +13,16 @@ Quaternion attitude; // estimated attitude
|
||||
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();
|
||||
}
|
||||
|
||||
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));
|
||||
@@ -39,7 +30,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;
|
||||
|
||||
@@ -50,13 +42,3 @@ void applyAcc() {
|
||||
// apply 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));
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
+3
-5
@@ -17,17 +17,15 @@ extern float motors[4];
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
print("Initializing Flix\n");
|
||||
print("Initializing flix\n");
|
||||
disableBrownOut();
|
||||
setupParameters();
|
||||
setupPower();
|
||||
setupLED();
|
||||
setLED(true);
|
||||
setupMotors();
|
||||
setupWiFi();
|
||||
setupIMU();
|
||||
setupRC();
|
||||
setupEstimate();
|
||||
setupLog();
|
||||
setLED(false);
|
||||
print("Initializing complete\n");
|
||||
}
|
||||
@@ -42,6 +40,6 @@ void loop() {
|
||||
handleInput();
|
||||
processMavlink();
|
||||
readVoltage();
|
||||
loopLog();
|
||||
logData();
|
||||
syncParameters();
|
||||
}
|
||||
|
||||
+5
-10
@@ -6,11 +6,11 @@
|
||||
#include <SPI.h>
|
||||
#include <FlixPeriph.h>
|
||||
#include "vector.h"
|
||||
#include "filter.h"
|
||||
#include "lpf.h"
|
||||
#include "util.h"
|
||||
|
||||
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 gyroBias;
|
||||
@@ -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,8 +121,7 @@ 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("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
|
||||
print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
|
||||
imu.waitForData();
|
||||
Vector rawGyro, rawAcc;
|
||||
|
||||
+47
-242
@@ -1,272 +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) {};
|
||||
LogEntry logEntries[] = {
|
||||
{"t", &t},
|
||||
{"rates.x", &rates.x},
|
||||
{"rates.y", &rates.y},
|
||||
{"rates.z", &rates.z},
|
||||
{"ratesTarget.x", &ratesTarget.x},
|
||||
{"ratesTarget.y", &ratesTarget.y},
|
||||
{"ratesTarget.z", &ratesTarget.z},
|
||||
{"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}
|
||||
};
|
||||
|
||||
LogTopic logTopics[] = {
|
||||
// time
|
||||
LogTopic({"t", &t}), // must be the first topic
|
||||
LogTopic(1, {"loopRate", &loopRate}),
|
||||
const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
|
||||
float logBuffer[LOG_SIZE][logColumns];
|
||||
|
||||
// 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,
|
||||
{"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(); }}),
|
||||
|
||||
// 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(); }}),
|
||||
};
|
||||
|
||||
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
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
+27
@@ -0,0 +1,27 @@
|
||||
// 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) {
|
||||
return output += alpha * (input - output);
|
||||
}
|
||||
|
||||
void setCutOffFrequency(float cutOffFreq, float dt) {
|
||||
alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
|
||||
}
|
||||
|
||||
void reset() {
|
||||
output = T(); // set to zero
|
||||
}
|
||||
};
|
||||
+25
-83
@@ -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,18 +34,14 @@ 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 (!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 voltages[] = {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,
|
||||
@@ -58,39 +49,27 @@ void sendMavlink() {
|
||||
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
|
||||
acc.x * 1000, -acc.y * 1000, -acc.z * 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);
|
||||
}
|
||||
}
|
||||
@@ -223,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
|
||||
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;
|
||||
}
|
||||
// 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];
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -258,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
|
||||
@@ -299,7 +241,7 @@ void handleMavlink(const void *_msg) {
|
||||
}
|
||||
|
||||
if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
|
||||
if (m.param1 == 1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
|
||||
if (m.param1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
|
||||
accepted = true;
|
||||
armed = m.param1 == 1;
|
||||
}
|
||||
|
||||
+10
-9
@@ -7,36 +7,37 @@
|
||||
|
||||
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;
|
||||
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;
|
||||
const int MOTOR_REAR_LEFT = 0;
|
||||
const int MOTOR_REAR_RIGHT = 1;
|
||||
const int MOTOR_FRONT_RIGHT = 2;
|
||||
const int 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 +52,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] = 1;
|
||||
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);
|
||||
|
||||
+18
-52
@@ -6,11 +6,13 @@
|
||||
#include <Preferences.h>
|
||||
#include "util.h"
|
||||
|
||||
extern int channelZero[16], channelMax[16];
|
||||
extern int channelZero[16];
|
||||
extern int 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 int rcRxPin;
|
||||
extern int wifiMode, udpLocalPort, udpRemotePort;
|
||||
extern float rcLossTimeout, descendTime;
|
||||
extern int voltagePin;
|
||||
extern float voltageScale;
|
||||
extern LowPassFilter<float> voltageFilter;
|
||||
|
||||
@@ -20,7 +22,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) {};
|
||||
@@ -35,16 +36,13 @@ Parameter parameters[] = {
|
||||
{"CTL_R_RATE_I", &rollRatePID.i},
|
||||
{"CTL_R_RATE_D", &rollRatePID.d},
|
||||
{"CTL_R_RATE_WU", &rollRatePID.windup},
|
||||
{"CTL_R_RATE_D_A", &rollRatePID.lpf.alpha},
|
||||
{"CTL_P_RATE_P", &pitchRatePID.p},
|
||||
{"CTL_P_RATE_I", &pitchRatePID.i},
|
||||
{"CTL_P_RATE_D", &pitchRatePID.d},
|
||||
{"CTL_P_RATE_WU", &pitchRatePID.windup},
|
||||
{"CTL_P_RATE_D_A", &pitchRatePID.lpf.alpha},
|
||||
{"CTL_Y_RATE_P", &yawRatePID.p},
|
||||
{"CTL_Y_RATE_I", &yawRatePID.i},
|
||||
{"CTL_Y_RATE_D", &yawRatePID.d},
|
||||
{"CTL_Y_RATE_D_A", &yawRatePID.lpf.alpha},
|
||||
{"CTL_R_P", &rollPID.p},
|
||||
{"CTL_R_I", &rollPID.i},
|
||||
{"CTL_R_D", &rollPID.d},
|
||||
@@ -72,10 +70,7 @@ Parameter parameters[] = {
|
||||
{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
|
||||
// estimate
|
||||
{"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},
|
||||
@@ -87,7 +82,7 @@ Parameter parameters[] = {
|
||||
{"MOT_PWM_MIN", &pwmMin},
|
||||
{"MOT_PWM_MAX", &pwmMax},
|
||||
// rc
|
||||
{"RC_RX_PIN", &rcRxPin, setupRC},
|
||||
{"RC_RX_PIN", &rcRxPin},
|
||||
{"RC_ZERO_0", &channelZero[0]},
|
||||
{"RC_ZERO_1", &channelZero[1]},
|
||||
{"RC_ZERO_2", &channelZero[2]},
|
||||
@@ -111,42 +106,19 @@ Parameter parameters[] = {
|
||||
{"RC_MODE", &modeChannel},
|
||||
// wifi
|
||||
{"WIFI_MODE", &wifiMode},
|
||||
{"WIFI_PORT_LOC", &udpLocalPort},
|
||||
{"WIFI_PORT_REM", &udpRemotePort},
|
||||
{"WIFI_LONG_RANGE", &wifiLongRange},
|
||||
// espnow
|
||||
{"ESPNOW_CHANNEL", &espnowChannel},
|
||||
{"WIFI_LOC_PORT", &udpLocalPort},
|
||||
{"WIFI_REM_PORT", &udpRemotePort},
|
||||
// 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_PIN", &voltagePin},
|
||||
{"PWR_VOLT_SCALE", &voltageScale},
|
||||
{"PWR_VOLT_LPF_A", &voltageFilter.alpha},
|
||||
// safety
|
||||
{"SF_RC_LOSS_TIME", &rcLossTimeout},
|
||||
{"SF_DESCEND_TIME", &descendTime},
|
||||
{"SF_DISARM_TILT", &disarmTilt}
|
||||
};
|
||||
|
||||
void setupParameters() {
|
||||
@@ -154,10 +126,10 @@ void setupParameters() {
|
||||
storage.begin("flix");
|
||||
// Read parameters from storage
|
||||
for (auto ¶meter : 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();
|
||||
}
|
||||
}
|
||||
@@ -203,23 +175,17 @@ void syncParameters() {
|
||||
if (motorsActive()) return; // don't use flash while flying, it may cause a delay
|
||||
|
||||
for (auto ¶meter : 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 ¶meter : 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
@@ -5,7 +5,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "filter.h"
|
||||
#include "lpf.h"
|
||||
|
||||
class PID {
|
||||
public:
|
||||
|
||||
+3
-11
@@ -3,24 +3,16 @@
|
||||
|
||||
// Power management
|
||||
|
||||
#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);
|
||||
float voltage;
|
||||
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;
|
||||
|
||||
|
||||
+19
-18
@@ -6,7 +6,7 @@
|
||||
#include <SBUS.h>
|
||||
#include "util.h"
|
||||
|
||||
SBUS rc(Serial1);
|
||||
SBUS rc(Serial2);
|
||||
int rcRxPin = -1; // -1 means disabled
|
||||
|
||||
uint16_t channels[16]; // raw rc channels
|
||||
@@ -27,12 +27,14 @@ void setupRC() {
|
||||
|
||||
bool readRC() {
|
||||
if (rcRxPin < 0) return false;
|
||||
if (!rc.read()) return false;
|
||||
|
||||
rc.getChannels(channels);
|
||||
normalizeRC();
|
||||
controlTime = t;
|
||||
return true;
|
||||
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() {
|
||||
@@ -53,19 +55,18 @@ 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
|
||||
uint16_t zero[16];
|
||||
uint16_t center[16];
|
||||
uint16_t max[16];
|
||||
print("1/8 Calibrating RC: put all switches to default positions [3 sec]\n");
|
||||
pause(3);
|
||||
calibrateRCChannel(NULL, _, zero, "2/8 Move sticks [3 sec]\n... ...\n... .o.\n.o. ...\n");
|
||||
calibrateRCChannel(&throttleChannel, zero, _, "3/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, _, "5/8 Move sticks [3 sec]\n... ...\n..o .o.\n... ...\n");
|
||||
calibrateRCChannel(&pitchChannel, zero, _, "6/8 Move sticks [3 sec]\n... .o.\n... ...\n.o. ...\n");
|
||||
calibrateRCChannel(&rollChannel, zero, _, "7/8 Move sticks [3 sec]\n... ...\n... ..o\n.o. ...\n");
|
||||
calibrateRCChannel(&modeChannel, zero, _, "8/8 Put mode switch to max [3 sec]\n");
|
||||
calibrateRCChannel(NULL, zero, 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, max, "4/8 Move sticks [3 sec]\n.o. ...\n... .o.\n... ...\n");
|
||||
calibrateRCChannel(&yawChannel, center, max, "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(&rollChannel, zero, max, "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");
|
||||
printRCCalibration();
|
||||
}
|
||||
|
||||
|
||||
+2
-16
@@ -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
|
||||
@@ -39,23 +37,11 @@ void descend() {
|
||||
void autoFailsafe() {
|
||||
static float roll, pitch, yaw, throttle;
|
||||
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
|
||||
// controls changed
|
||||
if (mode == AUTO) mode = STAB; // regain control by the pilot
|
||||
}
|
||||
roll = controlRoll;
|
||||
pitch = controlPitch;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
+8
-68
@@ -6,7 +6,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <ESP32_NOW_Serial.h>
|
||||
#include <soc/soc.h>
|
||||
#include <soc/rtc_cntl_reg.h>
|
||||
|
||||
const float ONE_G = 9.80665;
|
||||
extern float t;
|
||||
@@ -23,12 +24,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);
|
||||
@@ -40,73 +35,21 @@ float wrapAngle(float 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
|
||||
void splitString(String& str, String& token0, String& token1, String& token2) {
|
||||
str.trim();
|
||||
if (str.isEmpty()) return;
|
||||
char chars[str.length() + 1];
|
||||
str.toCharArray(chars, str.length() + 1);
|
||||
token0 = strtok(chars, " ");
|
||||
token1 = strtok(NULL, " ");
|
||||
token1 = strtok(NULL, " "); // String(NULL) creates empty string
|
||||
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
|
||||
}
|
||||
};
|
||||
|
||||
// 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 +58,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;
|
||||
|
||||
+3
-16
@@ -105,23 +105,10 @@ public:
|
||||
}
|
||||
|
||||
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);
|
||||
if (direction.norm() < 1e-6) { // vectors are parallel
|
||||
if (dot(a, b) > 0) { // same direction
|
||||
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;
|
||||
if (direction.zero()) {
|
||||
// vectors are opposite, return any perpendicular vector
|
||||
return cross(a, Vector(1, 0, 0));
|
||||
}
|
||||
direction.normalize();
|
||||
float angle = angleBetween(a, b);
|
||||
|
||||
+15
-91
@@ -1,152 +1,76 @@
|
||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||
// Repository: https://github.com/okalachev/flix
|
||||
|
||||
// Wi-Fi and ESP-NOW communication
|
||||
// Wi-Fi communication
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <WiFiAP.h>
|
||||
#include <WiFiUdp.h>
|
||||
#include <MacAddress.h>
|
||||
#include <ESP32_NOW_Serial.h>
|
||||
#include <Preferences.h>
|
||||
#include "util.h"
|
||||
#include "Preferences.h"
|
||||
|
||||
extern Preferences storage; // use the main preferences storage
|
||||
|
||||
const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
|
||||
const int W_DISABLED = 0, W_AP = 1, W_STA = 2;
|
||||
int wifiMode = W_AP;
|
||||
|
||||
int wifiLongRange = 0;
|
||||
int udpLocalPort = 14550;
|
||||
int udpRemotePort = 14550;
|
||||
IPAddress udpRemoteIP = "255.255.255.255";
|
||||
WiFiUDP udp;
|
||||
|
||||
ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
|
||||
ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
|
||||
int espnowChannel = 6;
|
||||
WiFiUDP udp;
|
||||
|
||||
void setupWiFi() {
|
||||
print("Setup Wi-Fi\n");
|
||||
WiFi.enableLongRange(wifiLongRange);
|
||||
|
||||
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) {
|
||||
} else 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
|
||||
udp.begin(udpLocalPort);
|
||||
}
|
||||
|
||||
void sendWiFi(const uint8_t *buf, int len) {
|
||||
if (espnow) {
|
||||
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.endPacket();
|
||||
}
|
||||
|
||||
int receiveWiFi(uint8_t *buf, int len) {
|
||||
if (espnow) {
|
||||
return espnow.read(buf, len);
|
||||
}
|
||||
|
||||
if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return 0;
|
||||
|
||||
udp.parsePacket();
|
||||
if (udp.remoteIP()) udpRemoteIP = udp.remoteIP();
|
||||
return udp.read(buf, len);
|
||||
}
|
||||
|
||||
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", WiFi.channel());
|
||||
print("Lost packets: %d\n", espnow.lost);
|
||||
} else if (WiFi.getMode() == WIFI_MODE_AP) {
|
||||
if (WiFi.getMode() == WIFI_MODE_AP) {
|
||||
print("Mode: Access Point (AP)\n");
|
||||
print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
|
||||
print("SSID: %s\n", WiFi.softAPSSID().c_str());
|
||||
print("Password: ***\n");
|
||||
print("Channel: %d\n", WiFi.channel());
|
||||
print("Clients: %d\n", WiFi.softAPgetStationNum());
|
||||
print("IP: %s\n", WiFi.softAPIP().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");
|
||||
return;
|
||||
}
|
||||
print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
|
||||
print("MAVLink connected: %d\n", mavlinkConnected);
|
||||
}
|
||||
|
||||
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 : "");
|
||||
void configWiFi(bool ap, const char *ssid, const char *password) {
|
||||
if (ap) {
|
||||
storage.putString("WIFI_AP_SSID", ssid);
|
||||
storage.putString("WIFI_AP_PASS", password);
|
||||
} else {
|
||||
print("Invalid configuration\n");
|
||||
return;
|
||||
storage.putString("WIFI_STA_SSID", ssid);
|
||||
storage.putString("WIFI_STA_PASS", password);
|
||||
}
|
||||
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]);
|
||||
}
|
||||
|
||||
+1
-15
@@ -20,13 +20,7 @@
|
||||
#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; }
|
||||
|
||||
long map(long x, long in_min, long in_max, long out_min, long out_max) {
|
||||
const long run = in_max - in_min;
|
||||
@@ -155,13 +149,11 @@ public:
|
||||
void setRxInvert(bool invert) {};
|
||||
};
|
||||
|
||||
HardwareSerial Serial, Serial1, Serial2;
|
||||
HardwareSerial Serial, 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 +163,10 @@ 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;
|
||||
|
||||
@@ -1,12 +0,0 @@
|
||||
// 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; };
|
||||
};
|
||||
+5
-4
@@ -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;
|
||||
};
|
||||
};
|
||||
|
||||
+6
-25
@@ -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;
|
||||
@@ -27,38 +27,26 @@ void step();
|
||||
void computeLoopRate();
|
||||
void applyGyro();
|
||||
void applyAcc();
|
||||
void applyLevel();
|
||||
void control();
|
||||
void interpretControls();
|
||||
void controlAttitude();
|
||||
void controlRates();
|
||||
void controlTorque();
|
||||
void desaturate(float& a, float& b, float& c, float& d);
|
||||
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);
|
||||
void handleInput();
|
||||
void setupRC();
|
||||
void normalizeRC();
|
||||
void calibrateRC();
|
||||
void calibrateRCChannel(int*, uint16_t[16], uint16_t[16], const char*);
|
||||
void calibrateRCChannel(int *channel, uint16_t zero[16], uint16_t max[16], const char *str);
|
||||
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);
|
||||
@@ -67,30 +55,23 @@ void handleMavlink(const void *_msg);
|
||||
void mavlinkPrint(const char* str);
|
||||
void sendMavlinkPrint();
|
||||
inline Quaternion fluToFrd(const Quaternion &q);
|
||||
void setupPower();
|
||||
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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -75,8 +73,6 @@ public:
|
||||
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()));
|
||||
|
||||
voltage = 4.2f; // dummy voltage value
|
||||
|
||||
readRC();
|
||||
estimate();
|
||||
|
||||
@@ -89,7 +85,7 @@ public:
|
||||
|
||||
applyMotorForces();
|
||||
publishTopics();
|
||||
loopLog();
|
||||
logData();
|
||||
syncParameters();
|
||||
}
|
||||
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
// 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) {}
|
||||
|
||||
+1
-6
@@ -11,12 +11,7 @@
|
||||
#include <sys/poll.h>
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
// Mocks
|
||||
int wifiMode = 1;
|
||||
int wifiLongRange = 0;
|
||||
int espnowChannel = 6;
|
||||
const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
|
||||
|
||||
int wifiMode = 1; // mock
|
||||
int udpLocalPort = 14580;
|
||||
int udpRemotePort = 14550;
|
||||
const char *udpRemoteIP = "255.255.255.255";
|
||||
|
||||
+1
-10
@@ -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,)
|
||||
|
||||
|
||||
@@ -1,3 +0,0 @@
|
||||
# ESPNOW-proxy
|
||||
|
||||
Proxy sketch for using ESP-NOW connection with Flix drone.
|
||||
@@ -1,108 +0,0 @@
|
||||
// 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 bool DISABLE_SWARM = true;
|
||||
const int CHANNEL = -1; // -1 means auto search
|
||||
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);
|
||||
link->begin();
|
||||
link->setKey((const uint8_t *)key);
|
||||
peers.push_back(link);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
WiFi.mode(WIFI_STA);
|
||||
WiFi.setSleep(false);
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
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();
|
||||
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 (!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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
+3
-12
@@ -24,22 +24,20 @@ pip install pyflix
|
||||
The API is accessed through the `Flix` class:
|
||||
|
||||
```python
|
||||
from pyflix import Flix
|
||||
from flix 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:
|
||||
Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware itself:
|
||||
|
||||
```python
|
||||
print(flix.connected) # True if connected to the drone
|
||||
print(flix.mode) # current flight mode (str)
|
||||
print(flix.armed) # True if the drone is armed
|
||||
print(flix.landed) # True if the drone is landed
|
||||
print(flix.voltage) # battery voltage (NaN - unknown, ~0 - USB powered)
|
||||
print(flix.voltage) # battery voltage
|
||||
print(flix.attitude) # attitude quaternion [w, x, y, z]
|
||||
print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw]
|
||||
print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate]
|
||||
@@ -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
|
||||
|
||||
+13
-20
@@ -5,7 +5,6 @@
|
||||
|
||||
import os
|
||||
import time
|
||||
import math
|
||||
from queue import Queue, Empty
|
||||
from typing import Optional, Callable, List, Dict, Any, Union, Sequence
|
||||
import logging
|
||||
@@ -27,7 +26,7 @@ class Flix:
|
||||
mode: str = ''
|
||||
armed: bool = False
|
||||
landed: bool = False
|
||||
voltage: float = math.nan
|
||||
voltage: float = 0
|
||||
attitude: List[float]
|
||||
attitude_euler: List[float] # roll, pitch, yaw
|
||||
rates: List[float]
|
||||
@@ -44,27 +43,22 @@ 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')
|
||||
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14550', source_system=255) # type: ignore
|
||||
except OSError as e:
|
||||
if e.errno != errno.EADDRINUSE:
|
||||
raise
|
||||
# Port busy - using proxy
|
||||
logger.debug('Listening on port 14555 (proxy)')
|
||||
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254) # type: ignore
|
||||
try:
|
||||
# Direct connection
|
||||
logger.debug('Listening on port 14550')
|
||||
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14550', source_system=255) # type: ignore
|
||||
except OSError as e:
|
||||
if e.errno != errno.EADDRINUSE:
|
||||
raise
|
||||
# Port busy - using proxy
|
||||
logger.debug('Listening on port 14555 (proxy)')
|
||||
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254) # type: ignore
|
||||
self.connection.target_system = system_id
|
||||
self.mavlink: mavlink.MAVLink = self.connection.mav
|
||||
self._event_listeners: Dict[str, List[Callable[..., Any]]] = {}
|
||||
@@ -192,8 +186,7 @@ class Flix:
|
||||
self._trigger('motors', self.motors)
|
||||
|
||||
if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
|
||||
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.acc = self._mavlink_to_flu([msg.xacc / 1000, msg.yacc / 1000, msg.zacc / 1000])
|
||||
self.gyro = self._mavlink_to_flu([msg.xgyro / 1000, msg.ygyro / 1000, msg.zgyro / 1000])
|
||||
self._trigger('acc', self.acc)
|
||||
self._trigger('gyro', self.gyro)
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
[project]
|
||||
name = "pyflix"
|
||||
version = "0.16"
|
||||
version = "0.11"
|
||||
description = "Python API for Flix drone"
|
||||
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
||||
license = "MIT"
|
||||
|
||||
Reference in New Issue
Block a user