Try using installable Preferences library

Two variants are redundant

.github/workflows/build.yml

@@ -23,10 +23,19 @@ jobs:
       with:
         name: firmware-binary
         path: flix/build
-    - name: Build firmware without Wi-Fi
+    - name: Build firmware for ESP32-S3
+      run: make BOARD=esp32:esp32:esp32s3
+    - name: Build firmware with WiFi disabled
       run: sed -i 's/^#define WIFI_ENABLED 1$/#define WIFI_ENABLED 0/' flix/flix.ino && make
     - name: Check c_cpp_properties.json
       run: tools/check_c_cpp_properties.py
+    - name: Build for Black Pill F411CE (STM32)
+      run: |
+        arduino-cli config set board_manager.additional_urls https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
+        arduino-cli core install STMicroelectronics:stm32
+        arduino-cli board listall STMicroelectronics:stm32
+        arduino-cli lib install "Preferences"
+        make BOARD=STMicroelectronics:stm32:GenF4:pnum=BLACKPILL_F411CE
 
   build_macos:
     runs-on: macos-latest
@@ -53,15 +62,25 @@ jobs:
       run: python3 tools/check_c_cpp_properties.py
 
   build_simulator:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-latest
+    container:
+      image: ubuntu:20.04
     steps:
+    - name: Install dependencies
+      run: |
+        apt-get update
+        DEBIAN_FRONTEND=noninteractive apt-get install -y curl wget build-essential cmake g++ pkg-config gnupg2 lsb-release sudo
     - name: Install Arduino CLI
       uses: arduino/setup-arduino-cli@v1.1.1
     - uses: actions/checkout@v4
     - name: Install Gazebo
-      run: curl -sSL http://get.gazebosim.org | sh
+      run: |
+        sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+        wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+        sudo apt-get update
+        sudo apt-get install -y gazebo11 libgazebo11-dev
     - name: Install SDL2
-      run: sudo apt-get install libsdl2-dev
+      run: sudo apt-get install -y libsdl2-dev
     - name: Build simulator
       run: make build_simulator
     - uses: actions/upload-artifact@v4

.vscode/c_cpp_properties.json

@@ -5,13 +5,15 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
+				"${workspaceFolder}/tools/**",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
 				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
 				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
 				"~/Arduino/libraries/**",
-				"/usr/include/**"
+				"/usr/include/gazebo-11/",
+				"/usr/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
@@ -51,14 +53,14 @@
 			"name": "Mac",
 			"includePath": [
 				"${workspaceFolder}/flix",
-				// "${workspaceFolder}/gazebo",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
 				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/include/**",
 				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**",
-				"/opt/homebrew/include/**"
+				"/opt/homebrew/include/gazebo-11/",
+				"/opt/homebrew/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
@@ -100,6 +102,7 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
+				"${workspaceFolder}/tools/**",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",

.vscode/settings.json

@@ -1,5 +1,6 @@
 {
 	"C_Cpp.intelliSenseEngineFallback": "enabled",
+	"C_Cpp.errorSquiggles": "disabled",
 	"files.associations": {
 		"*.sdf": "xml",
 		"*.ino": "cpp",

README.md

@@ -17,11 +17,11 @@
 
 * Dedicated for education and research.
 * Made from general-purpose components.
-* Simple and clean source code in Arduino.
-* Control using remote control or smartphone.
-* Precise simulation with Gazebo.
+* Simple and clean source code in Arduino (<2k lines firmware).
+* Control using USB gamepad, remote control or smartphone.
 * Wi-Fi and MAVLink support.
 * Wireless command line interface and analyzing.
+* Precise simulation with Gazebo.
 * Python library.
 * Textbook on flight control theory and practice ([in development](https://quadcopter.dev)).
 * *Position control (using external camera) and autonomous flights¹*.
@@ -38,7 +38,11 @@ Version 0 demo video: https://youtu.be/8GzzIQ3C6DQ.
 
 <a href="https://youtu.be/8GzzIQ3C6DQ"><img width=500 src="https://i3.ytimg.com/vi/8GzzIQ3C6DQ/maxresdefault.jpg"></a>
 
-See the [user builds gallery](docs/user.md).
+Usage in education (RoboCamp): https://youtu.be/Wd3yaorjTx0.
+
+<a href="https://youtu.be/Wd3yaorjTx0"><img width=500 src="https://i3.ytimg.com/vi/Wd3yaorjTx0/sddefault.jpg"></a>
+
+See the [user builds gallery](docs/user.md):
 
 <a href="docs/user.md"><img src="docs/img/user/user.jpg" width=500></a>
 
@@ -51,7 +55,7 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 ## Articles
 
 * [Assembly instructions](docs/assembly.md).
-* [Building and running the code](docs/build.md).
+* [Usage: build, setup and flight](docs/usage.md).
 * [Troubleshooting](docs/troubleshooting.md).
 * [Firmware architecture overview](docs/firmware.md).
 * [Python library tutorial](tools/pyflix/README.md).
@@ -63,8 +67,8 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 |Type|Part|Image|Quantity|
 |-|-|:-:|:-:|
 |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>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|
-|<span style="background:yellow">(Recommended) Buck-boost converter</span>|To be determined, output 5V or 3.3V, see [user-contributed schematics](https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612179508274&cot=14)|<img src="docs/img/buck-boost.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|
+|<span style="background:yellow">Buck-boost converter</span> (recommended)|To be determined, output 5V or 3.3V, see [user-contributed schematics](https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612179508274&cot=14)|<img src="docs/img/buck-boost.jpg" width=100>|1|
 |Motor|8520 3.7V brushed motor (shaft 0.8mm).<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).|<img src="docs/img/motor.jpeg" width=100>|4|
 |Propeller|Hubsan 55 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|
@@ -77,16 +81,16 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 |Frame main part|3D printed⁴:<br>[`flix-frame-1.1.stl`](docs/assets/flix-frame-1.1.stl) [`flix-frame-1.1.step`](docs/assets/flix-frame-1.1.step)<br>Recommended settings: layer 0.2 mm, line 0.4 mm, infill 100%.|<img src="docs/img/frame1.jpg" width=100>|1|
 |Frame top part|3D printed:<br>[`esp32-holder.stl`](docs/assets/esp32-holder.stl) [`esp32-holder.step`](docs/assets/esp32-holder.step)|<img src="docs/img/esp32-holder.jpg" width=100>|1|
 |Washer for IMU board mounting|3D printed:<br>[`washer-m3.stl`](docs/assets/washer-m3.stl) [`washer-m3.step`](docs/assets/washer-m3.step)|<img src="docs/img/washer-m3.jpg" width=100>|2|
-|*RC transmitter (optional)*|*KINGKONG TINY X8 (warning: lacks USB support) or other⁵*|<img src="docs/img/tx.jpg" width=100>|1|
+|Controller (recommended)|CC2500 transmitter, like BetaFPV LiteRadio CC2500 (RC receiver/Wi-Fi).<br>Two-sticks gamepad (Wi-Fi only) — see [recommended gamepads](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/setup_view/joystick.html#supported-joysticks).<br>Other⁵|<img src="docs/img/betafpv.jpg" width=100><img src="docs/img/logitech.jpg" width=80>|1|
 |*RC receiver (optional)*|*DF500 or other⁵*|<img src="docs/img/rx.jpg" width=100>|1|
 |Wires|28 AWG recommended|<img src="docs/img/wire-28awg.jpg" width=100>||
 |Tape, double-sided tape||||
 
 *² — barometer is not used for now.*<br>
-*³ — change `MPU9250` to `ICM20948` in `imu.ino` file if using ICM-20948 board.*<br>
+*³ — change `MPU9250` to `ICM20948` or `MPU6050` in `imu.ino` file for using the appropriate boards.*<br>
 *³⁻¹ — MPU-6050 supports I²C interface only (not recommended). To use it change IMU declaration to `MPU6050 IMU(Wire)`.*<br>
 *⁴ — this frame is optimized for GY-91 board, if using other, the board mount holes positions should be modified.*<br>
-*⁵ — you may use any transmitter-receiver pair with SBUS interface.*
+*⁵ — you also may use any transmitter-receiver pair with SBUS interface.*
 
 Tools required for assembly:
 
@@ -102,7 +106,9 @@ Feel free to modify the design and or code, and create your own improved version
 
 ### Simplified connection diagram
 
-<img src="docs/img/schematics1.svg" width=800 alt="Flix version 1 schematics">
+<img src="docs/img/schematics1.svg" width=700 alt="Flix version 1 schematics">
+
+*(Dashed is optional).*
 
 Motor connection scheme:
 

docs/book/firmware.md

@@ -1,8 +1,10 @@
 # Архитектура прошивки
 
-<img src="img/dataflow.svg" width=800 alt="Firmware dataflow diagram">
+Прошивка Flix это обычный скетч Arduino, реализованный в однопоточном стиле. Код инициализации находится в функции `setup()`, а главный цикл — в функции `loop()`. Скетч состоит из нескольких файлов, каждый из которых отвечает за определенную подсистему.
 
-Главный цикл работает на частоте 1000 Гц. Передача данных между подсистемами происходит через глобальные переменные:
+<img src="img/dataflow.svg" width=600 alt="Firmware dataflow diagram">
+
+Главный цикл `loop()` работает на частоте 1000 Гц. Передача данных между подсистемами происходит через глобальные переменные:
 
 * `t` *(float)* — текущее время шага, *с*.
 * `dt` *(float)* — дельта времени между текущим и предыдущим шагами, *с*.
@@ -15,18 +17,34 @@
 
 ## Исходные файлы
 
-Исходные файлы прошивки находятся в директории `flix`. Ключевые файлы:
+Исходные файлы прошивки находятся в директории `flix`. Основные файлы:
 
-* [`flix.ino`](https://github.com/okalachev/flix/blob/canonical/flix/flix.ino) — основной входной файл, скетч Arduino. Включает определение глобальных переменных и главный цикл.
-* [`imu.ino`](https://github.com/okalachev/flix/blob/canonical/flix/imu.ino) — чтение данных с датчика IMU (гироскоп и акселерометр), калибровка IMU.
-* [`rc.ino`](https://github.com/okalachev/flix/blob/canonical/flix/rc.ino) — чтение данных с RC-приемника, калибровка RC.
-* [`mavlink.ino`](https://github.com/okalachev/flix/blob/canonical/flix/mavlink.ino) — взаимодействие с QGroundControl через MAVLink.
-* [`estimate.ino`](https://github.com/okalachev/flix/blob/canonical/flix/estimate.ino) — оценка ориентации дрона, комплементарный фильтр.
-* [`control.ino`](https://github.com/okalachev/flix/blob/canonical/flix/control.ino) — управление ориентацией и угловыми скоростями дрона, трехмерный двухуровневый каскадный PID-регулятор.
-* [`motors.ino`](https://github.com/okalachev/flix/blob/canonical/flix/motors.ino) — управление выходными сигналами на моторы через ШИМ.
+* [`flix.ino`](https://github.com/okalachev/flix/blob/master/flix/flix.ino) — основной файл Arduino-скетча. Определяет некоторые глобальные переменные и главный цикл.
+* [`imu.ino`](https://github.com/okalachev/flix/blob/master/flix/imu.ino) — чтение данных с датчика IMU (гироскоп и акселерометр), калибровка IMU.
+* [`rc.ino`](https://github.com/okalachev/flix/blob/master/flix/rc.ino) — чтение данных с RC-приемника, калибровка RC.
+* [`estimate.ino`](https://github.com/okalachev/flix/blob/master/flix/estimate.ino) — оценка ориентации дрона, комплементарный фильтр.
+* [`control.ino`](https://github.com/okalachev/flix/blob/master/flix/control.ino) — подсистема управления, трехмерный двухуровневый каскадный ПИД-регулятор.
+* [`motors.ino`](https://github.com/okalachev/flix/blob/master/flix/motors.ino) — выход PWM на моторы.
+* [`mavlink.ino`](https://github.com/okalachev/flix/blob/master/flix/mavlink.ino) — взаимодействие с QGroundControl или [pyflix](https://github.com/okalachev/flix/tree/master/tools/pyflix) через протокол MAVLink.
 
-Вспомогательные файлы включают:
+Вспомогательные файлы:
 
-* [`vector.h`](https://github.com/okalachev/flix/blob/canonical/flix/vector.h), [`quaternion.h`](https://github.com/okalachev/flix/blob/canonical/flix/quaternion.h) — реализация библиотек векторов и кватернионов проекта.
-* [`pid.h`](https://github.com/okalachev/flix/blob/canonical/flix/pid.h) — реализация общего ПИД-регулятора.
-* [`lpf.h`](https://github.com/okalachev/flix/blob/canonical/flix/lpf.h) — реализация общего фильтра нижних частот.
+* [`vector.h`](https://github.com/okalachev/flix/blob/master/flix/vector.h), [`quaternion.h`](https://github.com/okalachev/flix/blob/master/flix/quaternion.h) — библиотеки векторов и кватернионов.
+* [`pid.h`](https://github.com/okalachev/flix/blob/master/flix/pid.h) — ПИД-регулятор.
+* [`lpf.h`](https://github.com/okalachev/flix/blob/master/flix/lpf.h) — фильтр нижних частот.
+
+### Подсистема управления
+
+Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в *команду управления*, которая включает следующее:
+
+* `attitudeTarget` *(Quaternion)* — целевая ориентация дрона.
+* `ratesTarget` *(Vector)* — целевые угловые скорости, *рад/с*.
+* `ratesExtra` *(Vector)* — дополнительные (feed-forward) угловые скорости, для управления рысканием в режиме STAB, *рад/с*.
+* `torqueTarget` *(Vector)* — целевой крутящий момент, диапазон [-1, 1].
+* `thrustTarget` *(float)* — целевая общая тяга, диапазон [0, 1].
+
+Команда управления обрабатывается в функциях `controlAttitude()`, `controlRates()`, `controlTorque()`. Если значение одной из переменных установлено в `NAN`, то соответствующая функция пропускается.
+
+<img src="img/control.svg" width=300 alt="Control subsystem diagram">
+
+Состояние *armed* хранится в переменной `armed`, а текущий режим — в переменной `mode`.

docs/build.md

@@ -1,205 +0,0 @@
-# Building and running
-
-To build the firmware or the simulator, you need to clone the repository using git:
-
-```bash
-git clone https://github.com/okalachev/flix.git
-cd flix
-```
-
-## Simulation
-
-### Ubuntu
-
-The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you have a newer version, consider using a virtual machine.
-
-1. Install Arduino CLI:
-
-   ```bash
-   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh
-   ```
-
-2. Install Gazebo 11:
-
-   ```bash
-   curl -sSL http://get.gazebosim.org | sh
-   ```
-
-   Set up your Gazebo environment variables:
-
-   ```bash
-   echo "source /usr/share/gazebo/setup.sh" >> ~/.bashrc
-   source ~/.bashrc
-   ```
-
-3. Install SDL2 and other dependencies:
-
-   ```bash
-   sudo apt-get update && sudo apt-get install build-essential libsdl2-dev
-   ```
-
-4. Add your user to the `input` group to enable joystick support (you need to re-login after this command):
-
-   ```bash
-   sudo usermod -a -G input $USER
-   ```
-
-5. Run the simulation:
-
-   ```bash
-   make simulator
-   ```
-
-### macOS
-
-1. Install Homebrew package manager, if you don't have it installed:
-
-   ```bash
-   /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
-   ```
-
-2. Install Arduino CLI, Gazebo 11 and SDL2:
-
-   ```bash
-   brew tap osrf/simulation
-   brew install arduino-cli
-   brew install gazebo11
-   brew install sdl2
-   ```
-
-   Set up your Gazebo environment variables:
-
-   ```bash
-   echo "source /opt/homebrew/share/gazebo/setup.sh" >> ~/.zshrc
-   source ~/.zshrc
-   ```
-
-3. Run the simulation:
-
-   ```bash
-   make simulator
-   ```
-
-### Setup and flight
-
-#### Control with smartphone
-
-1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone. For **iOS**, use [QGroundControl build from TAJISOFT](https://apps.apple.com/ru/app/qgc-from-tajisoft/id1618653051).
-2. Connect your smartphone to the same Wi-Fi network as the machine running the simulator.
-3. If you're using a virtual machine, make sure that its network is set to the **bridged** mode with Wi-Fi adapter selected.
-4. Run the simulation.
-5. Open QGroundControl app. It should connect and begin showing the virtual drone's telemetry automatically.
-6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
-7. Use the virtual joystick to fly the drone!
-
-#### Control with USB remote control
-
-1. Connect your USB remote control to the machine running the simulator.
-2. Run the simulation.
-3. Calibrate the RC using `cr` command in the command line interface.
-4. Run the simulation again.
-5. Use the USB remote control to fly the drone!
-
-## Firmware
-
-### Arduino IDE (Windows, Linux, macOS)
-
-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.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
-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.16.
-5. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
-6. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
-7. 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.
-8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
-
-### Command line (Windows, Linux, macOS)
-
-1. [Install Arduino CLI](https://arduino.github.io/arduino-cli/installation/).
-
-   On Linux, use:
-
-   ```bash
-   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh
-   ```
-
-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. Compile the firmware using `make`. Arduino dependencies will be installed automatically:
-
-   ```bash
-   make
-   ```
-
-   You can flash the firmware to the board using command:
-
-   ```bash
-   make upload
-   ```
-
-   You can also compile the firmware, upload it and start serial port monitoring using command:
-
-   ```bash
-   make upload monitor
-   ```
-
-See other available Make commands in the [Makefile](../Makefile).
-
-> [!TIP]
-> You can test the firmware on a bare ESP32 board without connecting IMU and other peripherals. The Wi-Fi network `flix` should appear and all the basic functionality including CLI and QGroundControl connection should work.
-
-### Setup and flight
-
-Before flight you need to calibrate the accelerometer:
-
-1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line).
-2. Type `ca` command there and follow the instructions.
-
-#### Control with smartphone
-
-1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
-2. Power the drone using the battery.
-3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
-4. Open QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
-5. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
-6. Use the virtual joystick to fly the drone!
-
-#### Control with remote control
-
-Before flight using remote control, you need to calibrate it:
-
-1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line).
-2. Type `cr` command there and follow the instructions.
-3. Use the remote control to fly the drone!
-
-#### Control with USB remote control
-
-If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it.
-
-1. Install [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html) app on your computer.
-2. Connect your USB remote control to the computer.
-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 the the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Joystick*. Calibrate you USB remote control there.
-7. Use the USB remote control to fly the drone!
-
-#### Adjusting parameters
-
-You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl app. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*.
-
-<img src="img/parameters.png" width="400">
-
-#### CLI access
-
-In addition to accessing the drone's command line interface (CLI) using the serial port, you can also access it with QGroundControl using Wi-Fi connection. To do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
-
-<img src="img/cli.png" width="400">
-
-> [!NOTE]
-> If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article.
-
-### Firmware code structure
-
-See [firmware overview](firmware.md) for more details.

docs/build.md

@@ -0,0 +1 @@
+usage.md

docs/firmware.md

@@ -1,39 +1,56 @@
 # Firmware overview
 
-The firmware is a regular Arduino sketch, and follows the classic Arduino one-threaded design. The initialization code is in the `setup()` function, and the main loop is in the `loop()` function. The sketch includes multiple files, each responsible for a specific part of the system.
+The firmware is a regular Arduino sketch, and it follows the classic Arduino one-threaded design. The initialization code is in the `setup()` function, and the main loop is in the `loop()` function. The sketch includes several files, each responsible for a specific subsystem.
 
 ## Dataflow
 
-<img src="img/dataflow.svg" width=800 alt="Firmware dataflow diagram">
+<img src="img/dataflow.svg" width=600 alt="Firmware dataflow diagram">
 
-The main loop is running at 1000 Hz. All the dataflow is happening through global variables (for simplicity):
+The main loop is running at 1000 Hz. All the dataflow goes through global variables (for simplicity):
 
-* `t` *(double)* — current step time, *s*.
+* `t` *(float)* — current step time, *s*.
 * `dt` *(float)* — time delta between the current and previous steps, *s*.
 * `gyro` *(Vector)* — data from the gyroscope, *rad/s*.
 * `acc` *(Vector)* — acceleration data from the accelerometer, *m/s<sup>2</sup>*.
 * `rates` *(Vector)* — filtered angular rates, *rad/s*.
 * `attitude` *(Quaternion)* — estimated attitude (orientation) of drone.
-* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].
-* `motors` *(float[])* — motor outputs, range [0, 1].
+* `controlRoll`, `controlPitch`, ... *(float[])* — pilot control inputs, range [-1, 1].
+* `motors` *(float[])* — motor outputs, range [0, 1].
 
 ## Source files
 
-Firmware source files are located in `flix` directory. The key files are:
+Firmware source files are located in `flix` directory. The core files are:
 
-* [`flix.ino`](../flix/flix.ino) — main entry point, Arduino sketch. Includes global variables definition and the main loop.
+* [`flix.ino`](../flix/flix.ino) — Arduino sketch main file, entry point.Includes some global variable definitions and the main loop.
 * [`imu.ino`](../flix/imu.ino) — reading data from the IMU sensor (gyroscope and accelerometer), IMU calibration.
 * [`rc.ino`](../flix/rc.ino) — reading data from the RC receiver, RC calibration.
-* [`estimate.ino`](../flix/estimate.ino) — drone's attitude estimation, complementary filter.
-* [`control.ino`](../flix/control.ino) — drone's attitude and rates control, three-dimensional two-level cascade PID controller.
-* [`motors.ino`](../flix/motors.ino) — PWM motor outputs control.
+* [`estimate.ino`](../flix/estimate.ino) — attitude estimation, complementary filter.
+* [`control.ino`](../flix/control.ino) — control subsystem, three-dimensional two-level cascade PID controller.
+* [`motors.ino`](../flix/motors.ino) — PWM motor output control.
+* [`mavlink.ino`](../flix/mavlink.ino) — interaction with QGroundControl or [pyflix](../tools/pyflix) via MAVLink protocol.
 
-Utility files include:
+Utility files:
 
-* [`vector.h`](../flix/vector.h), [`quaternion.h`](../flix/quaternion.h) — project's vector and quaternion libraries implementation.
-* [`pid.h`](../flix/pid.h) — generic PID controller implementation.
-* [`lpf.h`](../flix/lpf.h) — generic low-pass filter implementation.
+* [`vector.h`](../flix/vector.h), [`quaternion.h`](../flix/quaternion.h) — vector and quaternion libraries.
+* [`pid.h`](../flix/pid.h) — generic PID controller.
+* [`lpf.h`](../flix/lpf.h) — generic low-pass filter.
+
+### Control subsystem
+
+Pilot inputs are interpreted in `interpretControls()`, and then converted to the *control command*, which consists of the following:
+
+* `attitudeTarget` *(Quaternion)* — target attitude of the drone.
+* `ratesTarget` *(Vector)* — target angular rates, *rad/s*.
+* `ratesExtra` *(Vector)* — additional (feed-forward) angular rates , used for yaw rate control in STAB mode, *rad/s*.
+* `torqueTarget` *(Vector)* — target torque, range [-1, 1].
+* `thrustTarget` *(float)* — collective thrust target, range [0, 1].
+
+Control command is processed in `controlAttitude()`, `controlRates()`, `controlTorque()` functions. Each function may be skipped if the corresponding target is set to `NAN`.
+
+<img src="img/control.svg" width=300 alt="Control subsystem diagram">
+
+Armed state is stored in `armed` variable, and current mode is stored in `mode` variable.
 
 ## Building
 
-See build instructions in [build.md](build.md).
+See build instructions in [usage.md](usage.md).

docs/img/arming.svg

@@ -0,0 +1,22 @@
+<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 340.21 211.28">
+  <defs>
+    <style>
+      .a {
+        fill: #d5d5d5;
+      }
+
+      .b {
+        fill: #fff;
+      }
+
+      .c {
+        fill: #636363;
+      }
+    </style>
+  </defs>
+  <path class="a" d="M340,159.31c-4.74-86-35.9-128.7-35.9-128.7C288.3,9.53,269.17,2.91,251.87.39s-22.31,7.87-22.31,7.87C201.7,4,170.11,4.19,170.11,4.19S138.51,4,110.65,8.26c0,0-5-10.38-22.3-7.87S51.91,9.53,36.14,30.61c0,0-31.16,42.67-35.9,128.7-2.82,51.08,19.68,55.36,38.43,50.4a60.08,60.08,0,0,0,30.55-19.66c7.51-9,19.64-25.32,34-28,17.28-3.26,33.14-4.77,45.09-4.78l21.82,0,21.81,0c12,0,27.82,1.52,45.09,4.78,14.34,2.71,26.47,19,34,28a60.06,60.06,0,0,0,30.56,19.66C320.29,214.67,342.79,210.39,340,159.31Z"/>
+  <circle class="b" cx="88.54" cy="85.75" r="45.22"/>
+  <circle class="b" cx="251.67" cy="85.75" r="45.22"/>
+  <circle class="c" cx="251.67" cy="85.75" r="13.8"/>
+  <circle class="c" cx="103.8" cy="112.12" r="13.8"/>
+</svg>

docs/img/controls.svg

@@ -0,0 +1,123 @@
+<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 464.2 249.05">
+  <defs>
+    <style>
+      .a {
+        fill: #d5d5d5;
+      }
+
+      .b {
+        fill: #fff;
+      }
+
+      .c {
+        fill: #636363;
+      }
+
+      .d {
+        opacity: 0.7;
+      }
+
+      .e {
+        fill: none;
+        stroke: #0076ba;
+        stroke-linecap: round;
+        stroke-miterlimit: 10;
+        stroke-width: 13px;
+      }
+
+      .f {
+        fill: #0076ba;
+      }
+
+      .g {
+        font-size: 30px;
+        font-family: Tahoma;
+      }
+
+      .h {
+        letter-spacing: 0em;
+      }
+
+      .i {
+        letter-spacing: -0.01em;
+      }
+
+      .j {
+        letter-spacing: -0.06em;
+      }
+
+      .k {
+        letter-spacing: 0em;
+      }
+
+      .l {
+        letter-spacing: -0.02em;
+      }
+
+      .m {
+        letter-spacing: 0em;
+      }
+
+      .n {
+        opacity: 0.6;
+      }
+    </style>
+  </defs>
+  <path class="a" d="M408.84,197.08c-4.74-86-35.9-128.7-35.9-128.7C357.17,47.3,338,40.68,320.73,38.17S298.43,46,298.43,46C270.57,41.81,239,42,239,42s-31.59-.15-59.45,4.07c0,0-5-10.38-22.31-7.86S120.78,47.3,105,68.38c0,0-31.16,42.68-35.9,128.7-2.82,51.08,19.68,55.36,38.42,50.4a60.06,60.06,0,0,0,30.56-19.66c7.51-9,19.64-25.32,34-28,17.27-3.26,33.14-4.77,45.09-4.78L239,195l21.82,0c11.95,0,27.81,1.52,45.09,4.78,14.34,2.71,26.47,19,34,28a60.08,60.08,0,0,0,30.55,19.66C389.16,252.44,411.66,248.16,408.84,197.08Z"/>
+  <circle class="b" cx="157.41" cy="123.52" r="45.22"/>
+  <circle class="b" cx="320.54" cy="123.52" r="45.22"/>
+  <circle class="c" cx="320.54" cy="123.52" r="13.8"/>
+  <circle class="c" cx="157.41" cy="149.89" r="13.8"/>
+  <g class="d">
+    <g>
+      <line class="e" x1="157.41" y1="149.89" x2="157.41" y2="49.87"/>
+      <polygon class="f" points="180.74 56.7 157.41 16.29 134.07 56.7 180.74 56.7"/>
+    </g>
+  </g>
+  <text class="g" transform="translate(38.38 25.91)">Th<tspan class="h" x="34.25" y="0">r</tspan><tspan x="44.91" y="0">o</tspan><tspan class="i" x="61.2" y="0">t</tspan><tspan x="71" y="0">tle</tspan></text>
+  <g class="d">
+    <g>
+      <line class="e" x1="157.41" y1="149.89" x2="82.41" y2="149.89"/>
+      <polygon class="f" points="89.24 126.56 48.82 149.89 89.24 173.23 89.24 126.56"/>
+    </g>
+  </g>
+  <text class="g" transform="translate(0.18 176.36)"><tspan class="j">Y</tspan><tspan class="h" x="15.37" y="0">a</tspan><tspan x="30.97" y="0">w</tspan></text>
+  <g class="d">
+    <g>
+      <line class="e" x1="320.54" y1="123.52" x2="320.54" y2="50.32"/>
+      <polygon class="f" points="343.88 57.15 320.54 16.74 297.2 57.15 343.88 57.15"/>
+    </g>
+  </g>
+  <text class="g" transform="translate(336.56 26.36)">P<tspan class="k" x="16.54" y="0">i</tspan><tspan x="23.45" y="0">tch</tspan></text>
+  <g class="d">
+    <g>
+      <line class="e" x1="320.54" y1="123.52" x2="395.54" y2="123.52"/>
+      <polygon class="f" points="388.71 146.86 429.12 123.52 388.71 100.19 388.71 146.86"/>
+    </g>
+  </g>
+  <text class="g" transform="translate(416.03 160.01)"><tspan class="l">R</tspan><tspan x="18.08" y="0">o</tspan><tspan class="m" x="34.37" y="0">l</tspan><tspan x="41.31" y="0">l</tspan></text>
+  <g class="d">
+    <g>
+      <line class="e" x1="157.41" y1="149.89" x2="213.73" y2="149.89"/>
+      <polygon class="f" points="206.9 173.23 247.32 149.89 206.9 126.56 206.9 173.23"/>
+    </g>
+  </g>
+  <g class="d">
+    <g>
+      <line class="e" x1="320.54" y1="124.52" x2="320.54" y2="197.73"/>
+      <polygon class="f" points="297.2 190.9 320.54 231.31 343.88 190.9 297.2 190.9"/>
+    </g>
+  </g>
+  <g class="n">
+    <g>
+      <line class="e" x1="318.03" y1="123.52" x2="262.32" y2="123.52"/>
+      <polygon class="f" points="269.14 100.19 228.73 123.52 269.14 146.86 269.14 100.19"/>
+    </g>
+  </g>
+  <g class="d">
+    <g>
+      <line class="e" x1="157.41" y1="151.66" x2="157.41" y2="197.73"/>
+      <polygon class="f" points="134.07 190.9 157.41 231.31 180.74 190.9 134.07 190.9"/>
+    </g>
+  </g>
+</svg>

docs/img/disarming.svg

@@ -0,0 +1,22 @@
+<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 340.21 211.28">
+  <defs>
+    <style>
+      .a {
+        fill: #d5d5d5;
+      }
+
+      .b {
+        fill: #fff;
+      }
+
+      .c {
+        fill: #636363;
+      }
+    </style>
+  </defs>
+  <path class="a" d="M340,159.31c-4.74-86-35.9-128.7-35.9-128.7C288.3,9.53,269.17,2.91,251.87.39s-22.31,7.87-22.31,7.87C201.7,4,170.11,4.19,170.11,4.19S138.51,4,110.65,8.26c0,0-5-10.38-22.3-7.87S51.91,9.53,36.14,30.61c0,0-31.16,42.67-35.9,128.7-2.82,51.08,19.68,55.36,38.43,50.4a60.08,60.08,0,0,0,30.55-19.66c7.51-9,19.64-25.32,34-28,17.28-3.26,33.14-4.77,45.09-4.78l21.82,0,21.81,0c12,0,27.82,1.52,45.09,4.78,14.34,2.71,26.47,19,34,28a60.06,60.06,0,0,0,30.56,19.66C320.29,214.67,342.79,210.39,340,159.31Z"/>
+  <circle class="b" cx="88.54" cy="85.75" r="45.22"/>
+  <circle class="b" cx="251.67" cy="85.75" r="45.22"/>
+  <circle class="c" cx="251.67" cy="85.75" r="13.8"/>
+  <circle class="c" cx="73.28" cy="112.12" r="13.8"/>
+</svg>

docs/troubleshooting.md

@@ -4,8 +4,9 @@
 
 Do the following:
 
-* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](build.md#firmware).
+* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#firmware).
 * **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
+* **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
 
 ## The drone doesn't fly
 
@@ -14,7 +15,7 @@ Do the following:
 * **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
 * **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
 * **Check the 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 board, change `MPU9250` to `ICM20948` everywhere in the `imu.ino` file.
+* **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 CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
 * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
 * **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.
@@ -31,7 +32,6 @@ Do the following:
   * `mfl` — should rotate front left motor (clockwise).
   * `mrl` — should rotate rear left motor (counter-clockwise).
   * `mrr` — should rotate rear right motor (clockwise).
-* **Calibrate the RC** if you use it. Type `cr` command in Serial Monitor and follow the instructions.
-* **Check the RC data** if you use it. Use `rc` command, `Control` should show correct values between -1 and 1, and between 0 and 1 for the throttle.
+* **Check the remote control**. Using `rc` command, check the control values reflect your sticks movement. All the controls should change between -1 and 1, and throttle between 0 and 1.
+* If using SBUS receiver, **calibrate the RC**. Type `cr` command in Serial Monitor and follow the instructions.
 * **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.
-* **Check the gyroscope only attitude estimation**. Comment out `applyAcc();` line in `estimate.ino` and check if the attitude estimation in QGroundControl. It should be stable, but only drift very slowly.

docs/usage.md

@@ -0,0 +1,252 @@
+# Usage: build, setup and flight
+
+To use Flix, you need to build the firmware and upload it to the ESP32 board. For simulation, you need to build and run the simulator.
+
+For the start, clone the repository using git:
+
+```bash
+git clone https://github.com/okalachev/flix.git
+cd flix
+```
+
+## Simulation
+
+### Ubuntu
+
+The latest version of Ubuntu supported by Gazebo 11 simulator is 20.04. If you have a newer version, consider using a virtual machine.
+
+1. Install Arduino CLI:
+
+   ```bash
+   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh
+   ```
+
+2. Install Gazebo 11:
+
+   ```bash
+   sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+   wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+   sudo apt-get update
+   sudo apt-get install -y gazebo11 libgazebo11-dev
+   ```
+
+   Set up your Gazebo environment variables:
+
+   ```bash
+   echo "source /usr/share/gazebo/setup.sh" >> ~/.bashrc
+   source ~/.bashrc
+   ```
+
+3. Install SDL2 and other dependencies:
+
+   ```bash
+   sudo apt-get update && sudo apt-get install build-essential libsdl2-dev
+   ```
+
+4. Add your user to the `input` group to enable joystick support (you need to re-login after this command):
+
+   ```bash
+   sudo usermod -a -G input $USER
+   ```
+
+5. Run the simulation:
+
+   ```bash
+   make simulator
+   ```
+
+### macOS
+
+1. Install Homebrew package manager, if you don't have it installed:
+
+   ```bash
+   /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
+   ```
+
+2. Install Arduino CLI, Gazebo 11 and SDL2:
+
+   ```bash
+   brew tap osrf/simulation
+   brew install arduino-cli
+   brew install gazebo11
+   brew install sdl2
+   ```
+
+   Set up your Gazebo environment variables:
+
+   ```bash
+   echo "source /opt/homebrew/share/gazebo/setup.sh" >> ~/.zshrc
+   source ~/.zshrc
+   ```
+
+3. Run the simulation:
+
+   ```bash
+   make simulator
+   ```
+
+### Setup
+
+#### Control with smartphone
+
+1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone. For **iOS**, use [QGroundControl build from TAJISOFT](https://apps.apple.com/ru/app/qgc-from-tajisoft/id1618653051).
+2. Connect your smartphone to the same Wi-Fi network as the machine running the simulator.
+3. If you're using a virtual machine, make sure that its network is set to the **bridged** mode with Wi-Fi adapter selected.
+4. Run the simulation.
+5. Open QGroundControl app. It should connect and begin showing the virtual drone's telemetry automatically.
+6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
+7. Use the virtual joystick to fly the drone!
+
+#### Control with USB remote control
+
+1. Connect your USB remote control to the machine running the simulator.
+2. Run the simulation.
+3. Calibrate the RC using `cr` command in the command line interface.
+4. Run the simulation again.
+5. Use the USB remote control to fly the drone!
+
+## Firmware
+
+### Arduino IDE (Windows, Linux, macOS)
+
+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.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
+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.16.
+5. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
+6. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
+7. 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.
+8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
+
+### Command line (Windows, Linux, macOS)
+
+1. [Install Arduino CLI](https://arduino.github.io/arduino-cli/installation/).
+
+   On Linux, use:
+
+   ```bash
+   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh
+   ```
+
+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. Compile the firmware using `make`. Arduino dependencies will be installed automatically:
+
+   ```bash
+   make
+   ```
+
+   You can flash the firmware to the board using command:
+
+   ```bash
+   make upload
+   ```
+
+   You can also compile the firmware, upload it and start serial port monitoring using command:
+
+   ```bash
+   make upload monitor
+   ```
+
+See other available Make commands in the [Makefile](../Makefile).
+
+> [!TIP]
+> You can test the firmware on a bare ESP32 board without connecting IMU and other peripherals. The Wi-Fi network `flix` should appear and all the basic functionality including CLI and QGroundControl connection should work.
+
+### Setup
+
+Before flight you need to calibrate the accelerometer:
+
+1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line).
+2. Type `ca` command there and follow the instructions.
+
+#### Control with smartphone
+
+1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
+2. Power the drone using the battery.
+3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
+4. Open QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
+5. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
+6. Use the virtual joystick to fly the drone!
+
+> [!TIP]
+> Decrease `TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
+
+#### Control with remote control
+
+Before flight using remote control, you need to calibrate it:
+
+1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line).
+2. Type `cr` command there and follow the instructions.
+3. Use the remote control to fly the drone!
+
+#### Control with USB remote control (Wi-Fi)
+
+If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it.
+
+1. Install [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html) app on your computer.
+2. Connect your USB remote control to the computer.
+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 the the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Joystick*. Calibrate you USB remote control there.
+7. Use the USB remote control to fly the drone!
+
+> [!NOTE]
+> If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article.
+
+## Flight
+
+For both virtual sticks and a physical joystick, the default control scheme is left stick for throttle and yaw and right stick for pitch and roll:
+
+<img src="img/controls.svg" width="300">
+
+### Arming and disarming
+
+To start the motors, you should **arm** the drone. To do that, move the left stick to the bottom right corner:
+
+<img src="img/arming.svg" width="150">
+
+After that, the motors **will start spinning** at low speed, indicating that the drone is armed and ready to fly.
+
+When finished flying, **disarm** the drone, moving the left stick to the bottom left corner:
+
+<img src="img/disarming.svg" width="150">
+
+### Flight modes
+
+Flight mode is changed using mode switch on the remote control or using the command line.
+
+#### STAB
+
+The default mode is *STAB*. In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
+
+> [!IMPORTANT]
+> The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually.
+
+#### ACRO
+
+In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing.
+
+#### MANUAL
+
+Manual mode disables all the stabilization, and the pilot inputs are passed directly to the motors. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**.
+
+#### AUTO
+
+In this mode, the pilot inputs are ignored (except the mode switch, if configured). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed autonomous behavior.
+
+If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
+
+## Adjusting parameters
+
+You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl app. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*.
+
+<img src="img/parameters.png" width="400">
+
+## CLI access
+
+In addition to accessing the drone's command line interface (CLI) using the serial port, you can also access it with QGroundControl using Wi-Fi connection. To do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
+
+<img src="img/cli.png" width="400">

docs/user.md

@@ -4,6 +4,25 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
+## RoboCamp
+
+Author: RoboCamp participants.<br>
+Description: 3D-printed and wooden frames, ESP32 Mini, DC-DC buck-boost converters. BetaFPV LiteRadio 3 to control the drones via Wi-Fi connection.<br>
+Features: altitude hold, obstacle avoidance, autonomous flight elements.<br>
+Some of the designed model files: https://drive.google.com/drive/folders/18YHWGquKeIevzrMH4-OUT-zKXMETTEUu?usp=share_link.
+
+RoboCamp took place in July 2025, Saint Petersburg, where 9 participants designed and built their own drones using the Flix project, and then modified the firmware to complete specific flight tasks.
+
+See the detailed video about the event:
+
+<a href="https://youtu.be/Wd3yaorjTx0"><img width=500 src="https://img.youtube.com/vi/Wd3yaorjTx0/sddefault.jpg"></a>
+
+Built drones:
+
+<img src="img/user/robocamp/1.jpg" width=500>
+
+---
+
 Author: chkroko.<br>
 Description: the first Flix drone built with **brushless motors** (DShot interface).<br>
 Features: SpeedyBee BLS 35A Mini V2 ESC, ESP32-S3 board, EMAX ECO 2 2207 1700kv motors, ICM20948V2 IMU, INA226 power monitor and Bluetooth gamepad for control.<br>

flix/cli.ino

@@ -8,10 +8,12 @@
 #include "util.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern float loopRate, dt;
-extern double t;
+extern const int ACRO, STAB, AUTO;
+extern float t, dt, loopRate;
 extern uint16_t channels[16];
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
+extern int mode;
+extern bool armed;
 
 const char* motd =
 "\nWelcome to\n"
@@ -31,6 +33,9 @@ const char* motd =
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
+"arm - arm the drone\n"
+"disarm - disarm the drone\n"
+"stab/acro/auto - set mode\n"
 "rc - show RC data\n"
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
@@ -54,7 +59,7 @@ void print(const char* format, ...) {
 }
 
 void pause(float duration) {
-	double start = t;
+	float start = t;
 	while (t - start < duration) {
 		step();
 		handleInput();
@@ -69,9 +74,10 @@ void doCommand(String str, bool echo = false) {
 	// parse command
 	String command, arg0, arg1;
 	splitString(str, command, arg0, arg1);
+	if (command.isEmpty()) return;
 
 	// echo command
-	if (echo && !command.isEmpty()) {
+	if (echo) {
 		print("> %s\n", str.c_str());
 	}
 
@@ -101,22 +107,30 @@ void doCommand(String str, bool echo = false) {
 		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("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
+		print("qw: %f qx: %f qy: %f qz: %f\n", attitude.w, attitude.x, attitude.y, attitude.z);
 	} else if (command == "imu") {
 		printIMUInfo();
-		print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
-		print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
 		printIMUCalibration();
-		print("rate: %.0f\n", loopRate);
 		print("landed: %d\n", landed);
+	} else if (command == "arm") {
+		armed = true;
+	} else if (command == "disarm") {
+		armed = false;
+	} else if (command == "stab") {
+		mode = STAB;
+	} else if (command == "acro") {
+		mode = ACRO;
+	} else if (command == "auto") {
+		mode = AUTO;
 	} else if (command == "rc") {
 		print("channels: ");
 		for (int i = 0; i < 16; i++) {
 			print("%u ", channels[i]);
 		}
-		print("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
-			controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
+		print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
+			controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
 		print("mode: %s\n", getModeName());
+		print("armed: %d\n", armed);
 	} 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]);
@@ -157,8 +171,6 @@ void doCommand(String str, bool echo = false) {
 		attitude = Quaternion();
 	} else if (command == "reboot") {
 		ESP.restart();
-	} else if (command == "") {
-		// do nothing
 	} else {
 		print("Invalid command: %s\n", command.c_str());
 	}

flix/control.ino

@@ -34,8 +34,8 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-enum { MANUAL, ACRO, STAB, USER } mode = STAB;
-enum { YAW, YAW_RATE } yawMode = YAW;
+const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
+int mode = STAB;
 bool armed = false;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -49,75 +49,57 @@ float tiltMax = TILT_MAX;
 
 Quaternion attitudeTarget;
 Vector ratesTarget;
+Vector ratesExtra; // feedforward rates
 Vector torqueTarget;
 float thrustTarget;
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
 
 void control() {
-	interpretRC();
+	interpretControls();
 	failsafe();
-	if (mode == STAB) {
-		controlAttitude();
-		controlRate();
-		controlTorque();
-	} else if (mode == ACRO) {
-		controlRate();
-		controlTorque();
-	} else if (mode == MANUAL) {
-		controlTorque();
-	}
+	controlAttitude();
+	controlRates();
+	controlTorque();
 }
 
-void interpretRC() {
-	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
-
+void interpretControls() {
 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controlMode < 0.25) {
-		mode = STAB;
-	} else if (controlMode < 0.75) {
-		mode = STAB;
-	} else {
-		mode = STAB;
-	}
+	if (controlMode < 0.25) mode = STAB;
+	if (controlMode < 0.75) mode = STAB;
+	if (controlMode > 0.75) mode = STAB;
+
+	if (mode == AUTO) return; // pilot is not effective in AUTO mode
+
+	if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture
+	if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture
 
 	thrustTarget = controlThrottle;
 
-	if (mode == ACRO) {
-		yawMode = YAW_RATE;
-		ratesTarget.x = controlRoll * maxRate.x;
-		ratesTarget.y = controlPitch* maxRate.y;
-		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
-
-	} else if (mode == STAB) {
-		yawMode = controlYaw == 0 ? YAW : YAW_RATE;
-
-		attitudeTarget = Quaternion::fromEuler(Vector(
-			controlRoll * tiltMax,
-			controlPitch * tiltMax,
-			attitudeTarget.getYaw()));
-		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
-
-	} else if (mode == MANUAL) {
-		// passthrough mode
-		yawMode = YAW_RATE;
-		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
+	if (mode == STAB) {
+		float yawTarget = attitudeTarget.getYaw();
+		if (!armed || invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target
+		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
+		ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
 	}
 
-	if (yawMode == YAW_RATE || !motorsActive()) {
-		// update yaw target as we don't have control over the yaw
-		attitudeTarget.setYaw(attitude.getYaw());
+	if (mode == ACRO) {
+		attitudeTarget.invalidate(); // skip attitude control
+		ratesTarget.x = controlRoll * maxRate.x;
+		ratesTarget.y = controlPitch * maxRate.y;
+		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
+	}
+
+	if (mode == MANUAL) { // passthrough mode
+		attitudeTarget.invalidate(); // skip attitude control
+		ratesTarget.invalidate(); // skip rate control
+		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 }
 
 void controlAttitude() {
-	if (!armed) {
-		rollPID.reset();
-		pitchPID.reset();
-		yawPID.reset();
-		return;
-	}
+	if (!armed || attitudeTarget.invalid() || thrustTarget < 0.1) return; // skip attitude control
 
 	const Vector up(0, 0, 1);
 	Vector upActual = Quaternion::rotateVector(up, attitude);
@@ -125,34 +107,38 @@ void controlAttitude() {
 
 	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
-	ratesTarget.x = rollPID.update(error.x, dt);
-	ratesTarget.y = pitchPID.update(error.y, dt);
+	ratesTarget.x = rollPID.update(error.x) + ratesExtra.x;
+	ratesTarget.y = pitchPID.update(error.y) + ratesExtra.y;
 
-	if (yawMode == YAW) {
-		float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
-		ratesTarget.z = yawPID.update(yawError, dt);
-	}
+	float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
+	ratesTarget.z = yawPID.update(yawError) + ratesExtra.z;
 }
 
-void controlRate() {
-	if (!armed) {
-		rollRatePID.reset();
-		pitchRatePID.reset();
-		yawRatePID.reset();
-		return;
-	}
+
+void controlRates() {
+	if (!armed || ratesTarget.invalid() || thrustTarget < 0.1) return; // skip rates control
 
 	Vector error = ratesTarget - rates;
 
 	// Calculate desired torque, where 0 - no torque, 1 - maximum possible torque
-	torqueTarget.x = rollRatePID.update(error.x, dt);
-	torqueTarget.y = pitchRatePID.update(error.y, dt);
-	torqueTarget.z = yawRatePID.update(error.z, dt);
+	torqueTarget.x = rollRatePID.update(error.x);
+	torqueTarget.y = pitchRatePID.update(error.y);
+	torqueTarget.z = yawRatePID.update(error.z);
 }
 
 void controlTorque() {
+	if (!torqueTarget.valid()) return; // skip torque control
+
 	if (!armed) {
-		memset(motors, 0, sizeof(motors));
+		memset(motors, 0, sizeof(motors)); // stop motors if disarmed
+		return;
+	}
+
+	if (thrustTarget < 0.1) {
+		motors[0] = 0.1; // idle thrust
+		motors[1] = 0.1;
+		motors[2] = 0.1;
+		motors[3] = 0.1;
 		return;
 	}
 
@@ -172,7 +158,7 @@ const char* getModeName() {
 		case MANUAL: return "MANUAL";
 		case ACRO: return "ACRO";
 		case STAB: return "STAB";
-		case USER: return "USER";
+		case AUTO: return "AUTO";
 		default: return "UNKNOWN";
 	}
 }

flix/estimate.ino

@@ -11,8 +11,6 @@
 #define WEIGHT_ACC 0.003
 #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
-
 void estimate() {
 	applyGyro();
 	applyAcc();
@@ -20,6 +18,7 @@ void estimate() {
 
 void applyGyro() {
 	// filter gyro to get angular rates
+	static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
 	rates = ratesFilter.update(gyro);
 
 	// apply rates to attitude

flix/failsafe.ino

@@ -1,41 +0,0 @@
-// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-// Fail-safe functions
-
-#define RC_LOSS_TIMEOUT 0.2
-#define DESCEND_TIME 3.0 // time to descend from full throttle to zero
-
-extern double controlTime;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw;
-
-void failsafe() {
-	armingFailsafe();
-	rcLossFailsafe();
-}
-
-// Prevent arming without zero throttle input
-void armingFailsafe() {
-	static double zeroThrottleTime;
-	static double armingTime;
-	if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time
-	if (controlTime > 0 && controlThrottle < 0.05) zeroThrottleTime = controlTime;
-	if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec
-}
-
-// RC loss failsafe
-void rcLossFailsafe() {
-	if (t - controlTime > RC_LOSS_TIMEOUT) {
-		descend();
-	}
-}
-
-// Smooth descend on RC lost
-void descend() {
-	mode = STAB;
-	controlRoll = 0;
-	controlPitch = 0;
-	controlYaw = 0;
-	controlThrottle -= dt / DESCEND_TIME;
-	if (controlThrottle < 0) controlThrottle = 0;
-}

flix/flix.ino

@@ -10,9 +10,10 @@
 #define SERIAL_BAUDRATE 115200
 #define WIFI_ENABLED 1
 
-double t = NAN; // current step time, s
+float t = NAN; // current step time, s
 float dt; // time delta from previous step, s
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]
+float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
+float controlMode = NAN;
 Vector gyro; // gyroscope data
 Vector acc; // accelerometer data, m/s/s
 Vector rates; // filtered angular rates, rad/s

flix/imu.ino

@@ -4,11 +4,12 @@
 // Work with the IMU sensor
 
 #include <SPI.h>
-#include <MPU9250.h>
+#include <FlixPeriph.h>
+#include "vector.h"
 #include "lpf.h"
 #include "util.h"
 
-MPU9250 IMU(SPI);
+MPU9250 imu(SPI);
 
 Vector accBias;
 Vector accScale(1, 1, 1);
@@ -16,22 +17,22 @@ Vector gyroBias;
 
 void setupIMU() {
 	print("Setup IMU\n");
-	IMU.begin();
+	imu.begin();
 	configureIMU();
 }
 
 void configureIMU() {
-	IMU.setAccelRange(IMU.ACCEL_RANGE_4G);
-	IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
-	IMU.setDLPF(IMU.DLPF_MAX);
-	IMU.setRate(IMU.RATE_1KHZ_APPROX);
-	IMU.setupInterrupt();
+	imu.setAccelRange(imu.ACCEL_RANGE_4G);
+	imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
+	imu.setDLPF(imu.DLPF_MAX);
+	imu.setRate(imu.RATE_1KHZ_APPROX);
+	imu.setupInterrupt();
 }
 
 void readIMU() {
-	IMU.waitForData();
-	IMU.getGyro(gyro.x, gyro.y, gyro.z);
-	IMU.getAccel(acc.x, acc.y, acc.z);
+	imu.waitForData();
+	imu.getGyro(gyro.x, gyro.y, gyro.z);
+	imu.getAccel(acc.x, acc.y, acc.z);
 	calibrateGyroOnce();
 	// apply scale and bias
 	acc = (acc - accBias) / accScale;
@@ -49,9 +50,8 @@ void rotateIMU(Vector& data) {
 }
 
 void calibrateGyroOnce() {
-	static float landedTime = 0;
-	landedTime = landed ? landedTime + dt : 0;
-	if (landedTime < 2) return; // calibrate only if definitely stationary
+	static Delay landedDelay(2);
+	if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
 
 	static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
 	gyroBias = gyroCalibrationFilter.update(gyro);
@@ -59,7 +59,7 @@ void calibrateGyroOnce() {
 
 void calibrateAccel() {
 	print("Calibrating accelerometer\n");
-	IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode
+	imu.setAccelRange(imu.ACCEL_RANGE_2G); // the most sensitive mode
 
 	print("1/6 Place level [8 sec]\n");
 	pause(8);
@@ -93,9 +93,9 @@ void calibrateAccelOnce() {
 	// Compute the average of the accelerometer readings
 	acc = Vector(0, 0, 0);
 	for (int i = 0; i < samples; i++) {
-		IMU.waitForData();
+		imu.waitForData();
 		Vector sample;
-		IMU.getAccel(sample.x, sample.y, sample.z);
+		imu.getAccel(sample.x, sample.y, sample.z);
 		acc = acc + sample;
 	}
 	acc = acc / samples;
@@ -119,7 +119,16 @@ void printIMUCalibration() {
 }
 
 void printIMUInfo() {
-	IMU.status() ? print("status: ERROR %d\n", IMU.status()) : print("status: OK\n");
-	print("model: %s\n", IMU.getModel());
-	print("who am I: 0x%02X\n", IMU.whoAmI());
+	imu.status() ? print("status: ERROR %d\n", imu.status()) : print("status: OK\n");
+	print("model: %s\n", imu.getModel());
+	print("who am I: 0x%02X\n", imu.whoAmI());
+	print("rate: %.0f\n", loopRate);
+	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;
+	imu.getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
+	imu.getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
+	print("raw gyro: %f %f %f\n", rawGyro.x, rawGyro.y, rawGyro.z);
+	print("raw acc: %f %f %f\n", rawAcc.x, rawAcc.y, rawAcc.z);
 }

flix/log.ino

@@ -10,7 +10,6 @@
 #define LOG_PERIOD 1.0 / LOG_RATE
 #define LOG_SIZE LOG_DURATION * LOG_RATE
 
-float tFloat;
 Vector attitudeEuler;
 Vector attitudeTargetEuler;
 
@@ -20,7 +19,7 @@ struct LogEntry {
 };
 
 LogEntry logEntries[] = {
-	{"t", &tFloat},
+	{"t", &t},
 	{"rates.x", &rates.x},
 	{"rates.y", &rates.y},
 	{"rates.z", &rates.z},
@@ -40,7 +39,6 @@ const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
 float logBuffer[LOG_SIZE][logColumns];
 
 void prepareLogData() {
-	tFloat = t;
 	attitudeEuler = attitude.toEuler();
 	attitudeTargetEuler = attitudeTarget.toEuler();
 }
@@ -48,7 +46,7 @@ void prepareLogData() {
 void logData() {
 	if (!armed) return;
 	static int logPointer = 0;
-	static double logTime = 0;
+	static float logTime = 0;
 	if (t - logTime < LOG_PERIOD) return;
 	logTime = t;
 

flix/mavlink.ino

@@ -12,11 +12,11 @@
 #define PERIOD_FAST 0.1
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
 
-float mavlinkControlScale = 0.7;
+bool mavlinkConnected = false;
 String mavlinkPrintBuffer;
 
-extern double controlTime;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
+extern float controlTime;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
 
 void processMavlink() {
 	sendMavlink();
@@ -26,8 +26,8 @@ void processMavlink() {
 void sendMavlink() {
 	sendMavlinkPrint();
 
-	static double lastSlow = 0;
-	static double lastFast = 0;
+	static float lastSlow = 0;
+	static float lastFast = 0;
 
 	mavlink_message_t msg;
 	uint32_t time = t * 1000;
@@ -36,16 +36,20 @@ void sendMavlink() {
 		lastSlow = t;
 
 		mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
-			MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
+			(armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) |
+			((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
+			((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
+
 		mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
 			MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
 		sendMessage(&msg);
 	}
 
-	if (t - lastFast >= PERIOD_FAST) {
+	if (t - lastFast >= PERIOD_FAST && mavlinkConnected) {
 		lastFast = t;
 
 		const float zeroQuat[] = {0, 0, 0, 0};
@@ -57,9 +61,9 @@ void sendMavlink() {
 			channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
 		if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
 
-		float actuator[32];
-		memcpy(actuator, motors, sizeof(motors));
-		mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 4, actuator);
+		float controls[8];
+		memcpy(controls, motors, sizeof(motors));
+		mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
 		sendMessage(&msg);
 
 		mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
@@ -79,6 +83,7 @@ void sendMessage(const void *msg) {
 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;
@@ -99,11 +104,10 @@ void handleMavlink(const void *_msg) {
 		if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast
 
 		controlThrottle = m.z / 1000.0f;
-		controlPitch = m.x / 1000.0f * mavlinkControlScale;
-		controlRoll = m.y / 1000.0f * mavlinkControlScale;
-		controlYaw = m.r / 1000.0f * mavlinkControlScale;
-		controlMode = 1; // STAB mode
-		controlArmed = 1; // armed
+		controlPitch = m.x / 1000.0f;
+		controlRoll = m.y / 1000.0f;
+		controlYaw = m.r / 1000.0f;
+		controlMode = NAN;
 		controlTime = t;
 
 		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
@@ -174,24 +178,73 @@ void handleMavlink(const void *_msg) {
 		doCommand(data, true);
 	}
 
+	if (msg.msgid == MAVLINK_MSG_ID_SET_ATTITUDE_TARGET) {
+		if (mode != AUTO) return;
+
+		mavlink_set_attitude_target_t m;
+		mavlink_msg_set_attitude_target_decode(&msg, &m);
+		if (m.target_system && m.target_system != SYSTEM_ID) return;
+
+		// 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);
+
+		if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
+		armed = m.thrust > 0;
+	}
+
+	if (msg.msgid == MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET) {
+		if (mode != AUTO) return;
+
+		mavlink_set_actuator_control_target_t m;
+		mavlink_msg_set_actuator_control_target_decode(&msg, &m);
+		if (m.target_system && m.target_system != SYSTEM_ID) return;
+
+		attitudeTarget.invalidate();
+		ratesTarget.invalidate();
+		torqueTarget.invalidate();
+		memcpy(motors, m.controls, sizeof(motors)); // copy motor thrusts
+		armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
+	}
+
 	// Handle commands
 	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
 		mavlink_command_long_t m;
 		mavlink_msg_command_long_decode(&msg, &m);
 		if (m.target_system && m.target_system != SYSTEM_ID) return;
-		mavlink_message_t ack;
 		mavlink_message_t response;
+		bool accepted = false;
 
 		if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
-			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg.sysid, msg.compid);
-			sendMessage(&ack);
+			accepted = true;
 			mavlink_msg_autopilot_version_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &response,
 				MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
 			sendMessage(&response);
-		} else {
-			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
-			sendMessage(&ack);
 		}
+
+		if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
+			if (m.param1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
+			accepted = true;
+			armed = m.param1 == 1;
+		}
+
+		if (m.command == MAV_CMD_DO_SET_MODE) {
+			if (m.param2 < 0 || m.param2 > AUTO) return; // incorrect mode
+			accepted = true;
+			mode = m.param2;
+		}
+
+		// send command ack
+		mavlink_message_t ack;
+		mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
+		sendMessage(&ack);
 	}
 }
 

flix/motors.ino

@@ -25,30 +25,32 @@ const int MOTOR_FRONT_LEFT = 3;
 
 void setupMotors() {
 	print("Setup Motors\n");
-
-	// configure pins
+#ifdef ESP32
 	ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
 	ledcAttach(MOTOR_1_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
 	ledcAttach(MOTOR_2_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
 	ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
-
+#else
+	analogWriteResolution(PWM_RESOLUTION);
+	analogWriteFrequency(PWM_FREQUENCY);
+#endif
 	sendMotors();
 	print("Motors initialized\n");
 }
 
 int getDutyCycle(float value) {
 	value = constrain(value, 0, 1);
-	float pwm = mapff(value, 0, 1, PWM_MIN, PWM_MAX);
+	float pwm = mapf(value, 0, 1, PWM_MIN, PWM_MAX);
 	if (value == 0) pwm = PWM_STOP;
-	float duty = mapff(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
+	float duty = mapf(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
 	return round(duty);
 }
 
 void sendMotors() {
-	ledcWrite(MOTOR_0_PIN, getDutyCycle(motors[0]));
-	ledcWrite(MOTOR_1_PIN, getDutyCycle(motors[1]));
-	ledcWrite(MOTOR_2_PIN, getDutyCycle(motors[2]));
-	ledcWrite(MOTOR_3_PIN, getDutyCycle(motors[3]));
+	analogWrite(MOTOR_0_PIN, getDutyCycle(motors[0]));
+	analogWrite(MOTOR_1_PIN, getDutyCycle(motors[1]));
+	analogWrite(MOTOR_2_PIN, getDutyCycle(motors[2]));
+	analogWrite(MOTOR_3_PIN, getDutyCycle(motors[3]));
 }
 
 bool motorsActive() {

flix/parameters.ino

@@ -8,7 +8,6 @@
 extern float channelZero[16];
 extern float channelMax[16];
 extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
-extern float mavlinkControlScale;
 
 Preferences storage;
 
@@ -70,12 +69,7 @@ Parameter parameters[] = {
 	{"RC_PITCH", &pitchChannel},
 	{"RC_THROTTLE", &throttleChannel},
 	{"RC_YAW", &yawChannel},
-	{"RC_ARMED", &armedChannel},
 	{"RC_MODE", &modeChannel},
-#if WIFI_ENABLED
-	// MAVLink
-	{"MAV_CTRL_SCALE", &mavlinkControlScale},
-#endif
 };
 
 void setupParameters() {
@@ -124,7 +118,7 @@ bool setParameter(const char *name, const float value) {
 }
 
 void syncParameters() {
-	static double lastSync = 0;
+	static float lastSync = 0;
 	if (t - lastSync < 1) return; // sync once per second
 	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
 	lastSync = t;

flix/pid.h

@@ -9,40 +9,44 @@
 
 class PID {
 public:
-	float p = 0;
-	float i = 0;
-	float d = 0;
-	float windup = 0;
+	float p, i, d;
+	float windup;
+	float dtMax;
 
 	float derivative = 0;
 	float integral = 0;
 
 	LowPassFilter<float> lpf; // low pass filter for derivative term
 
-	PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
+	PID(float p, float i, float d, float windup = 0, float dAlpha = 1, float dtMax = 0.1) :
+		p(p), i(i), d(d), windup(windup), lpf(dAlpha), dtMax(dtMax) {}
 
-	float update(float error, float dt) {
-		integral += error * dt;
+	float update(float error) {
+		float dt = t - prevTime;
 
-		if (isfinite(prevError) && dt > 0) {
-			// calculate derivative if both dt and prevError are valid
-			derivative = (error - prevError) / dt;
-
-			// apply low pass filter to derivative
-			derivative = lpf.update(derivative);
+		if (dt > 0 && dt < dtMax) {
+			integral += error * dt;
+			derivative = lpf.update((error - prevError) / dt); // compute derivative and apply low-pass filter
+		} else {
+			integral = 0;
+			derivative = 0;
 		}
 
 		prevError = error;
+		prevTime = t;
 
 		return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
 	}
 
 	void reset() {
 		prevError = NAN;
+		prevTime = NAN;
 		integral = 0;
 		derivative = 0;
+		lpf.reset();
 	}
 
 private:
 	float prevError = NAN;
+	float prevTime = NAN;
 };

flix/quaternion.h

@@ -45,7 +45,7 @@ public:
 			cx * cy * sz - sx * sy * cz);
 	}
 
-	static Quaternion fromBetweenVectors(Vector u, Vector v) {
+	static Quaternion fromBetweenVectors(const Vector& u, const Vector& v) {
 		float dot = u.x * v.x + u.y * v.y + u.z * v.z;
 		float w1 = u.y * v.z - u.z * v.y;
 		float w2 = u.z * v.x - u.x * v.z;
@@ -64,6 +64,22 @@ public:
 		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
+	bool valid() const {
+		return finite();
+	}
+
+	bool invalid() const {
+		return !valid();
+	}
+
+	void invalidate() {
+		w = NAN;
+		x = NAN;
+		y = NAN;
+		z = NAN;
+	}
+
+
 	float norm() const {
 		return sqrt(w * w + x * x + y * y + z * z);
 	}
@@ -116,29 +132,31 @@ public:
 		return euler;
 	}
 
+	float getRoll() const {
+		return toEuler().x;
+	}
+
+	float getPitch() const {
+		return toEuler().y;
+	}
+
 	float getYaw() const {
-		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
-		float yaw;
-		float sqx = x * x;
-		float sqy = y * y;
-		float sqz = z * z;
-		float sqw = w * w;
-		double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
-		if (sarg <= -0.99999) {
-			yaw = -2 * atan2(y, x);
-		} else if (sarg >= 0.99999) {
-			yaw = 2 * atan2(y, x);
-		} else {
-			yaw = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
-		}
-		return yaw;
+		return toEuler().z;
+	}
+
+	void setRoll(float roll) {
+		Vector euler = toEuler();
+		*this = Quaternion::fromEuler(Vector(roll, euler.y, euler.z));
+	}
+
+	void setPitch(float pitch) {
+		Vector euler = toEuler();
+		*this = Quaternion::fromEuler(Vector(euler.x, pitch, euler.z));
 	}
 
 	void setYaw(float yaw) {
-		// TODO: optimize?
 		Vector euler = toEuler();
-		euler.z = yaw;
-		(*this) = Quaternion::fromEuler(euler);
+		*this = Quaternion::fromEuler(Vector(euler.x, euler.y, yaw));
 	}
 
 	Quaternion operator * (const Quaternion& q) const {

flix/rc.ino

@@ -6,24 +6,24 @@
 #include <SBUS.h>
 #include "util.h"
 
-SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
+SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
 uint16_t channels[16]; // raw rc channels
-double controlTime; // time of the last controls update
+float controlTime; // time of the last controls update
 float channelZero[16]; // calibration zero values
 float channelMax[16]; // calibration max values
 
 // Channels mapping (using float to store in parameters):
-float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, armedChannel = NAN, modeChannel = NAN;
+float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
 
 void setupRC() {
 	print("Setup RC\n");
-	RC.begin();
+	rc.begin();
 }
 
 bool readRC() {
-	if (RC.read()) {
-		SBUSData data = RC.data();
+	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;
@@ -42,7 +42,6 @@ void normalizeRC() {
 	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
 	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
 	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
-	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default
 	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
 }
 
@@ -50,16 +49,15 @@ void calibrateRC() {
 	uint16_t zero[16];
 	uint16_t center[16];
 	uint16_t max[16];
-	print("1/9 Calibrating RC: put all switches to default positions [3 sec]\n");
+	print("1/8 Calibrating RC: put all switches to default positions [3 sec]\n");
 	pause(3);
-	calibrateRCChannel(NULL, zero, zero, "2/9 Move sticks [3 sec]\n...     ...\n...     .o.\n.o.     ...\n");
-	calibrateRCChannel(NULL, center, center, "3/9 Move sticks [3 sec]\n...     ...\n.o.     .o.\n...     ...\n");
-	calibrateRCChannel(&throttleChannel, zero, max, "4/9 Move sticks [3 sec]\n.o.     ...\n...     .o.\n...     ...\n");
-	calibrateRCChannel(&yawChannel, center, max, "5/9 Move sticks [3 sec]\n...     ...\n..o     .o.\n...     ...\n");
-	calibrateRCChannel(&pitchChannel, zero, max, "6/9 Move sticks [3 sec]\n...     .o.\n...     ...\n.o.     ...\n");
-	calibrateRCChannel(&rollChannel, zero, max, "7/9 Move sticks [3 sec]\n...     ...\n...     ..o\n.o.     ...\n");
-	calibrateRCChannel(&armedChannel, zero, max, "8/9 Switch to armed [3 sec]\n");
-	calibrateRCChannel(&modeChannel, zero, max, "9/9 Disarm and switch mode 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();
 }
 
@@ -94,6 +92,5 @@ void printRCCalibration() {
 	print("Pitch     %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
 	print("Yaw       %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
 	print("Throttle  %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
-	print("Armed     %-7g%-7g%-7g\n", armedChannel, armedChannel >= 0 ? channelZero[(int)armedChannel] : NAN, armedChannel >= 0 ? channelMax[(int)armedChannel] : NAN);
 	print("Mode      %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
 }

flix/safety.ino

@@ -0,0 +1,48 @@
+// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Fail-safe functions
+
+#define RC_LOSS_TIMEOUT 1
+#define DESCEND_TIME 10
+
+extern float controlTime;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw;
+
+void failsafe() {
+	rcLossFailsafe();
+	autoFailsafe();
+}
+
+// RC loss failsafe
+void rcLossFailsafe() {
+	if (controlTime == 0) return; // no RC at all
+	if (!armed) return;
+	if (t - controlTime > RC_LOSS_TIMEOUT) {
+		descend();
+	}
+}
+
+// Smooth descend on RC lost
+void descend() {
+	mode = AUTO;
+	attitudeTarget = Quaternion();
+	thrustTarget -= dt / DESCEND_TIME;
+	if (thrustTarget < 0) {
+		thrustTarget = 0;
+		armed = false;
+	}
+}
+
+// Allow pilot to interrupt automatic flight
+void autoFailsafe() {
+	static float roll, pitch, yaw, throttle;
+	if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
+		// controls changed
+		if (mode == AUTO) mode = STAB; // regain control by the pilot
+	}
+	roll = controlRoll;
+	pitch = controlPitch;
+	yaw = controlYaw;
+	throttle = controlThrottle;
+}

flix/time.ino

@@ -6,7 +6,7 @@
 float loopRate; // Hz
 
 void step() {
-	double now = micros() / 1000000.0;
+	float now = micros() / 1000000.0;
 	dt = now - t;
 	t = now;
 
@@ -18,7 +18,7 @@ void step() {
 }
 
 void computeLoopRate() {
-	static double windowStart = 0;
+	static float windowStart = 0;
 	static uint32_t rate = 0;
 	rate++;
 	if (t - windowStart >= 1) { // 1 second window

flix/util.h

@@ -6,17 +6,24 @@
 #pragma once
 
 #include <math.h>
+#ifdef ESP32
 #include <soc/soc.h>
 #include <soc/rtc_cntl_reg.h>
+#endif
 
 const float ONE_G = 9.80665;
+extern float t;
 
-float mapf(long x, long in_min, long in_max, float out_min, float out_max) {
-	return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
+float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
+	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-float mapff(float x, float in_min, float in_max, float out_min, float out_max) {
-	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+bool invalid(float x) {
+	return !isfinite(x);
+}
+
+bool valid(float x) {
+	return isfinite(x);
 }
 
 // Wrap angle to [-PI, PI)
@@ -32,7 +39,9 @@ float wrapAngle(float angle) {
 
 // Disable reset on low voltage
 void disableBrownOut() {
+#ifdef ESP32
 	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
+#endif
 }
 
 // Trim and split string by spaces
@@ -44,3 +53,23 @@ void splitString(String& str, String& token0, String& token1, String& token2) {
 	token1 = strtok(NULL, " "); // String(NULL) creates empty string
 	token2 = strtok(NULL, "");
 }
+
+// Delay filter for boolean signals - ensures the signal is on for at least 'delay' seconds
+class Delay {
+public:
+	float delay;
+	float start = NAN;
+
+	Delay(float delay) : delay(delay) {}
+
+	bool update(bool on) {
+		if (!on) {
+			start = NAN;
+			return false;
+		}
+		if (isnan(start)) {
+			start = t;
+		}
+		return t - start >= delay;
+	}
+};

flix/vector.h

@@ -21,6 +21,21 @@ public:
 		return isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
+	bool valid() const {
+		return finite();
+	}
+
+	bool invalid() const {
+		return !valid();
+	}
+
+	void invalidate() {
+		x = NAN;
+		y = NAN;
+		z = NAN;
+	}
+
+
 	float norm() const {
 		return sqrt(x * x + y * y + z * z);
 	}

flix/wifi.ino

@@ -13,6 +13,7 @@
 #define WIFI_PASSWORD "flixwifi"
 #define WIFI_UDP_PORT 14550
 #define WIFI_UDP_REMOTE_PORT 14550
+#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
 
 WiFiUDP udp;
 
@@ -24,7 +25,7 @@ void setupWiFi() {
 
 void sendWiFi(const uint8_t *buf, int len) {
 	if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
-	udp.beginPacket(WiFi.softAPBroadcastIP(), WIFI_UDP_REMOTE_PORT);
+	udp.beginPacket(udp.remoteIP() ? udp.remoteIP() : WIFI_UDP_REMOTE_ADDR, WIFI_UDP_REMOTE_PORT);
 	udp.write(buf, len);
 	udp.endPacket();
 }

gazebo/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 project(flix_gazebo)
 
-# === gazebo plugin
+# Gazebo plugin
 find_package(gazebo REQUIRED)
 find_package(SDL2 REQUIRED)
 include_directories(${GAZEBO_INCLUDE_DIRS})

gazebo/README.md

@@ -4,7 +4,7 @@
 
 ## Building and running
 
-See [building and running instructions](../docs/build.md#simulation).
+See [building and running instructions](../docs/usage.md#simulation).
 
 ## Code structure
 

gazebo/flix.h

@@ -12,10 +12,11 @@
 
 #define WIFI_ENABLED 1
 
-double t = NAN;
+float t = NAN;
 float dt;
 float motors[4];
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
+float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
+float controlMode = NAN;
 Vector acc;
 Vector gyro;
 Vector rates;
@@ -28,9 +29,9 @@ void computeLoopRate();
 void applyGyro();
 void applyAcc();
 void control();
-void interpretRC();
+void interpretControls();
 void controlAttitude();
-void controlRate();
+void controlRates();
 void controlTorque();
 const char* getModeName();
 void sendMotors();
@@ -54,9 +55,9 @@ void mavlinkPrint(const char* str);
 void sendMavlinkPrint();
 inline Quaternion fluToFrd(const Quaternion &q);
 void failsafe();
-void armingFailsafe();
 void rcLossFailsafe();
 void descend();
+void autoFailsafe();
 int parametersCount();
 const char *getParameterName(int index);
 float getParameter(int index);

gazebo/simulator.cpp

@@ -21,7 +21,7 @@
 #include "cli.ino"
 #include "control.ino"
 #include "estimate.ino"
-#include "failsafe.ino"
+#include "safety.ino"
 #include "log.ino"
 #include "lpf.h"
 #include "mavlink.ino"
@@ -59,6 +59,7 @@ public:
 
 	void OnReset() {
 		attitude = Quaternion(); // reset estimated attitude
+		armed = false;
 		__resetTime += __micros;
 		gzmsg << "Flix plugin reset" << endl;
 	}

gazebo/wifi.h

@@ -13,6 +13,7 @@
 
 #define WIFI_UDP_PORT 14580
 #define WIFI_UDP_REMOTE_PORT 14550
+#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
 
 int wifiSocket;
 
@@ -35,7 +36,7 @@ void sendWiFi(const uint8_t *buf, int len) {
 	if (wifiSocket == 0) setupWiFi();
 	sockaddr_in addr; // remote address
 	addr.sin_family = AF_INET;
-	addr.sin_addr.s_addr = INADDR_BROADCAST; // send UDP broadcast
+	addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
 	addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
 	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
 }

tools/check_c_cpp_properties.py

@@ -49,6 +49,8 @@ for configuration in props['configurations']:
     print('Check configuration', configuration['name'])
 
     for include_path in configuration.get('includePath', []):
+        if include_path.startswith('/opt/') or include_path.startswith('/usr/'): # don't check non-Arduino libs
+            continue
         check_path(include_path)
 
     for forced_include in configuration.get('forcedInclude', []):

tools/pyflix/README.md

@@ -59,6 +59,13 @@ flix.on('disconnected', lambda: print('Disconnected from Flix'))
 flix.on('print', lambda text: print(f'Flix says: {text}'))
 ```
 
+Unsubscribe from events using `off` method:
+
+```python
+flix.off('print')  # unsubscribe from print events
+flix.off(callback)  # unsubscribe specific callback
+```
+
 You can also wait for specific events using `wait` method. This method returns the data associated with the event:
 
 ```python
@@ -66,13 +73,14 @@ gyro = flix.wait('gyro')  # wait for gyroscope update
 attitude = flix.wait('attitude', timeout=3)  # wait for attitude update, raise TimeoutError after 3 seconds
 ```
 
-The `value` argument specifies a condition for filtering events. It can be either an expected value or a callable:
+The second argument (`value`) specifies a condition for filtering events. It can be either an expected value or a callable:
 
 ```python
-flix.wait('armed', value=True)  # wait until armed
-flix.wait('armed', value=False)  # wait until disarmed
-flix.wait('motors', value=lambda motors: not any(motors))  # wait until all motors stop
-flix.wait('attitude_euler', value=lambda att: att[0] > 0)  # wait until roll angle is positive
+flix.wait('armed', True)  # wait until armed
+flix.wait('armed', False)  # wait until disarmed
+flix.wait('mode', 'AUTO')  # wait until flight mode is switched to AUTO
+flix.wait('motors', lambda motors: not any(motors))  # wait until all motors stop
+flix.wait('attitude_euler', lambda att: att[0] > 0)  # wait until roll angle is positive
 ```
 
 Full list of events:
@@ -107,7 +115,7 @@ Get and set firmware parameters using `get_param` and `set_param` methods:
 
 ```python
 pitch_p = flix.get_param('PITCH_P')  # get parameter value
-flix.set_param('PITCH_P', 5)      # set parameter value
+flix.set_param('PITCH_P', 5)         # set parameter value
 ```
 
 Execute CLI commands using `cli` method. This method returns command response:
@@ -121,21 +129,65 @@ flix.cli('reboot')       # reboot the drone
 > [!TIP]
 > Use `help` command to get the list of available commands.
 
+You can arm and disarm the drone using `set_armed` method (warning: the drone will fall if disarmed in the air):
+
+```python
+flix.set_armed(True)   # arm the drone
+flix.set_armed(False)  # disarm the drone
+```
+
+You can imitate pilot's controls using `set_controls` method:
+
+```python
+flix.set_controls(roll=0, pitch=0, yaw=0, throttle=0.6)
+```
+
+> [!WARNING]
+> This method **is not intended for automatic flights**, only for adding support for a custom pilot input device.
+
 ### Automatic flight
 
-The flight control feature is in development. List of methods intended for automatic flight control:
+To perform automatic flight, switch the mode to *AUTO*, either from the remote control, or from the code:
 
-* `set_position`
-* `set_velocity`
-* `set_attitude`
-* `set_rates`
-* `set_motors`
-* `set_controls`
-* `set_mode`
+```python
+flix.set_mode('AUTO')
+```
+
+In this mode you can set flight control targets. Setting attitude target:
+
+```python
+flix.set_attitude([0.1, 0.2, 0.3], 0.6)  # set target roll, pitch, yaw and thrust
+flix.set_attitude([1, 0, 0, 0], 0.6)  # set target attitude quaternion and thrust
+```
+
+Setting angular rates target:
+
+```python
+flix.set_rates([0.1, 0.2, 0.3], 0.6)  # set target roll rate, pitch rate, yaw rate and thrust
+```
+
+You also can control raw motors outputs directly:
+
+```python
+flix.set_motors([0.5, 0.5, 0.5, 0.5])  # set motors outputs in range [0, 1]
+```
+
+In *AUTO* mode, the drone will arm automatically if the thrust is greater than zero, and disarm if thrust is zero. Therefore, to disarm the drone, set thrust to zero:
+
+```python
+flix.set_attitude([0, 0, 0], 0)  # disarm the drone
+```
+
+The following methods are in development and are not functional yet:
+
+* `set_position` — set target position.
+* `set_velocity` — set target velocity.
+
+To exit from *AUTO* mode move control sticks and the drone will switch to *STAB* mode.
 
 ## Usage alongside QGroundControl
 
-You can use the Flix library alongside the QGroundControl app, using a proxy mode. To do that:
+You can use the Flix library alongside the QGroundControl app, using proxy mode. To do that:
 
 1. Run proxy for `pyflix` and QGroundControl in background:
 
@@ -151,6 +203,8 @@ You can use the Flix library alongside the QGroundControl app, using a proxy mod
    * *Port*: 14560
 4. Restart QGroundControl.
 
+<img src="../../docs/img/qgc-proxy.png" width="300">
+
 Now you can run `pyflix` scripts and QGroundControl simultaneously.
 
 ## Tools
@@ -201,11 +255,11 @@ You can send values from the firmware like this (`mavlink.ino`):
 
 ```cpp
 // Send float named value
-mavlink_msg_named_value_float_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, t, "some_value", loopRate);
+mavlink_msg_named_value_float_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, t, "loop_rate", loopRate);
 sendMessage(&msg);
 
 // Send vector named value
-mavlink_msg_debug_vect_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "some_vector", t, gyroBias.x, gyroBias.y, gyroBias.z);
+mavlink_msg_debug_vect_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "gyro_bias", t, gyroBias.x, gyroBias.y, gyroBias.z);
 sendMessage(&msg);
 ```
 

tools/pyflix/flix.py

@@ -6,7 +6,7 @@
 import os
 import time
 from queue import Queue, Empty
-from typing import Literal, Optional, Callable, List, Dict, Any, Union
+from typing import Optional, Callable, List, Dict, Any, Union, Sequence
 import logging
 import errno
 from threading import Thread, Timer
@@ -36,10 +36,11 @@ class Flix:
 
     system_id: int
     messages: Dict[str, Dict[str, Any]]  # MAVLink messages storage
-    values: Dict[Union[str, int], Union[float, List[float]]] = {}  # named values
+    values: Dict[Union[str, int], Union[float, List[float]]]  # named values
 
     _connection_timeout = 3
     _print_buffer: str = ''
+    _modes = ['MANUAL', 'ACRO', 'STAB', 'AUTO']
 
     def __init__(self, system_id: int=1, wait_connection: bool=True):
         if not (0 <= system_id < 256):
@@ -55,12 +56,11 @@ class Flix:
             if e.errno != errno.EADDRINUSE:
                 raise
             # Port busy - using proxy
-            logger.debug('Listening on port 14560 (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]]] = {}
-        self.messages = {}
         self._disconnected_timer = Timer(0, self._disconnected)
         self._reader_thread = Thread(target=self._read_mavlink, daemon=True)
         self._reader_thread.start()
@@ -78,6 +78,8 @@ class Flix:
         self.motors = [0, 0, 0, 0]
         self.acc = [0, 0, 0]
         self.gyro = [0, 0, 0]
+        self.messages = {}
+        self.values = {}
 
     def on(self, event: str, callback: Callable):
         event = event.lower()
@@ -85,10 +87,15 @@ class Flix:
             self._event_listeners[event] = []
         self._event_listeners[event].append(callback)
 
-    def off(self, callback: Callable):
-        for event in self._event_listeners:
-            if callback in self._event_listeners[event]:
-                self._event_listeners[event].remove(callback)
+    def off(self, event_or_callback: Union[str, Callable]):
+        if isinstance(event_or_callback, str):
+            event = event_or_callback.lower()
+            if event in self._event_listeners:
+                del self._event_listeners[event]
+        else:
+            for event in self._event_listeners:
+                if event_or_callback in self._event_listeners[event]:
+                    self._event_listeners[event].remove(event_or_callback)
 
     def _trigger(self, event: str, *args):
         event = event.lower()
@@ -147,7 +154,7 @@ class Flix:
 
     def _handle_mavlink_message(self, msg: mavlink.MAVLink_message):
         if isinstance(msg, mavlink.MAVLink_heartbeat_message):
-            self.mode = ['MANUAL', 'ACRO', 'STAB', 'USER'][msg.custom_mode]
+            self.mode = self._modes[msg.custom_mode] if msg.custom_mode < len(self._modes) else f'UNKNOWN({msg.custom_mode})'
             self.armed = msg.base_mode & mavlink.MAV_MODE_FLAG_SAFETY_ARMED != 0
             self._trigger('mode', self.mode)
             self._trigger('armed', self.armed)
@@ -168,6 +175,11 @@ class Flix:
                              msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]
             self._trigger('channels', self.channels)
 
+        if isinstance(msg, mavlink.MAVLink_actuator_control_target_message):
+            self.motors = msg.controls[:4]  # type: ignore
+            self._trigger('motors', self.motors)
+
+        # TODO: to be removed: the old way of passing motor outputs
         if isinstance(msg, mavlink.MAVLink_actuator_output_status_message):
             self.motors = msg.actuator[:4]  # type: ignore
             self._trigger('motors', self.motors)
@@ -231,6 +243,19 @@ class Flix:
     def _flu_to_mavlink(v: List[float]) -> List[float]:
         return Flix._mavlink_to_flu(v)
 
+    def _command_send(self, command: int, params: Sequence[float]):
+        if len(params) != 7:
+            raise ValueError('Command must have 7 parameters')
+        for attempt in range(3):
+            try:
+                logger.debug(f'Send command {command} with params {params} (attempt #{attempt + 1})')
+                self.mavlink.command_long_send(self.system_id, 0, command, 0, *params)  # type: ignore
+                self.wait('mavlink.COMMAND_ACK', value=lambda msg: msg.command == command and msg.result == mavlink.MAV_RESULT_ACCEPTED, timeout=0.1)
+                return
+            except TimeoutError:
+                continue
+        raise RuntimeError(f'Failed to send command {command} after 3 attempts')
+
     def _connected(self):
         # Reset disconnection timer
         self._disconnected_timer.cancel()
@@ -275,6 +300,14 @@ class Flix:
                 continue
         raise RuntimeError(f'Failed to set parameter {name} to {value} after 3 attempts')
 
+    def set_mode(self, mode: Union[str, int]):
+        if isinstance(mode, str):
+            mode = self._modes.index(mode.upper())
+        self._command_send(mavlink.MAV_CMD_DO_SET_MODE, (0, mode, 0, 0, 0, 0, 0))
+
+    def set_armed(self, armed: bool):
+        self._command_send(mavlink.MAV_CMD_COMPONENT_ARM_DISARM, (1 if armed else 0, 0, 0, 0, 0, 0, 0))
+
     def set_position(self, position: List[float], yaw: Optional[float] = None, wait: bool = False, tolerance: float = 0.1):
         raise NotImplementedError('Position control is not implemented yet')
 
@@ -282,17 +315,37 @@ class Flix:
         raise NotImplementedError('Velocity control is not implemented yet')
 
     def set_attitude(self, attitude: List[float], thrust: float):
-        raise NotImplementedError('Automatic flight is not implemented yet')
+        if len(attitude) == 3:
+            attitude = Quaternion([attitude[0], attitude[1], attitude[2]]).q  # type: ignore
+        elif len(attitude) != 4:
+            raise ValueError('Attitude must be [roll, pitch, yaw] or [w, x, y, z] quaternion')
+        if not (0 <= thrust <= 1):
+            raise ValueError('Thrust must be in range [0, 1]')
+        attitude = self._flu_to_mavlink(attitude)
+        for _ in range(2): # duplicate to ensure delivery
+            self.mavlink.set_attitude_target_send(0, self.system_id, 0, 0,
+                [attitude[0], attitude[1], attitude[2], attitude[3]],
+                0, 0, 0, thrust)
 
     def set_rates(self, rates: List[float], thrust: float):
-        raise NotImplementedError('Automatic flight is not implemented yet')
+        if len(rates) != 3:
+            raise ValueError('Rates must be [roll_rate, pitch_rate, yaw_rate]')
+        if not (0 <= thrust <= 1):
+            raise ValueError('Thrust must be in range [0, 1]')
+        rates = self._flu_to_mavlink(rates)
+        for _ in range(2):  # duplicate to ensure delivery
+            self.mavlink.set_attitude_target_send(0, self.system_id, 0,
+                mavlink.ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE,
+                [1, 0, 0, 0],
+                rates[0], rates[1], rates[2], thrust)
 
     def set_motors(self, motors: List[float]):
         if len(motors) != 4:
             raise ValueError('motors must have 4 values')
         if not all(0 <= m <= 1 for m in motors):
             raise ValueError('motors must be in range [0, 1]')
-        raise NotImplementedError
+        for _ in range(2):  # duplicate to ensure delivery
+            self.mavlink.set_actuator_control_target_send(int(time.time() * 1000000), 0, self.system_id, 0, motors + [0] * 4)  # type: ignore
 
     def set_controls(self, roll: float, pitch: float, yaw: float, throttle: float):
         """Send pilot's controls. Warning: not intended for automatic control"""
@@ -300,10 +353,7 @@ class Flix:
             raise ValueError('roll, pitch, yaw must be in range [-1, 1]')
         if not 0 <= throttle <= 1:
             raise ValueError('throttle must be in range [0, 1]')
-        self.mavlink.manual_control_send(self.system_id, roll * 1000, pitch * 1000, yaw * 1000, throttle * 1000, 0)  # type: ignore
-
-    def set_mode(self, mode: Literal['MANUAL', 'ACRO', 'STAB', 'USER']):
-        raise NotImplementedError('Setting mode is not implemented yet')
+        self.mavlink.manual_control_send(self.system_id, int(pitch * 1000), int(roll * 1000), int(throttle * 1000), int(yaw * 1000), 0)  # type: ignore
 
     def cli(self, cmd: str, wait_response: bool = True) -> str:
         cmd = cmd.strip()
@@ -320,7 +370,9 @@ class Flix:
                 self.mavlink.serial_control_send(0, 0, 0, 0, len(cmd_bytes), cmd_bytes)
                 if not wait_response:
                     return ''
-                response = self.wait('print_full', timeout=0.1, value=lambda text: text.startswith(response_prefix))
+                timeout = 0.1
+                if cmd == 'log': timeout = 10 # log download may take more time
+                response = self.wait('print_full', timeout=timeout, value=lambda text: text.startswith(response_prefix))
                 return response[len(response_prefix):].strip()
             except TimeoutError:
                 continue

tools/pyflix/proxy.py

@@ -24,13 +24,16 @@ def main():
         if addr in TARGETS:  # packet from target
             if source_addr is None:
                 continue
-            sock.sendto(data, source_addr)
+            try:
+                sock.sendto(data, source_addr)
+                packets += 1
+            except: pass
         else:  # packet from source
             source_addr = addr
             for target in TARGETS:
                 sock.sendto(data, target)
+                packets += 1
 
-        packets += 1
         print(f'\rPackets: {packets}', end='')
 
 if __name__ == '__main__':

tools/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "pyflix"
-version = "0.5"
+version = "0.9"
 description = "Python API for Flix drone"
 authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
 license = "MIT"