Try to build the sim on the latest macos

Remove verbosity and debug
Merge branch 'master' into fix-macos-ci
2026-03-30 20:13:44 +00:00 · 2024-12-10 09:36:36 +03:00 · 2024-12-10 09:36:19 +03:00 · 2024-12-10 06:15:23 +03:00 · 2024-12-10 06:02:09 +03:00 · 2024-11-23 19:32:43 +03:00
38 changed files with 776 additions and 528 deletions
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -15,13 +15,11 @@ jobs:
      run: curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh
    - name: Build firmware
      run: make
-    - name: Build firmware without Wi-Fi
-      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

  build_macos:
-    runs-on: macos-latest
+    runs-on: macos-14
    steps:
    - uses: actions/checkout@v4
    - name: Install Arduino CLI
@@ -45,7 +43,7 @@ jobs:
      run: python3 tools/check_c_cpp_properties.py

  build_simulator:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-latest
    steps:
    - name: Install Arduino CLI
      uses: arduino/setup-arduino-cli@v1.1.1
@@ -56,28 +54,46 @@ jobs:
      run: sudo apt-get install libsdl2-dev
    - name: Build simulator
      run: make build_simulator
-    - uses: actions/upload-artifact@v4
+    - uses: actions/upload-artifact@v3
      with:
        name: gazebo-plugin-binary
        path: gazebo/build/*.so
        retention-days: 1

-#  build_simulator_macos:
-#    runs-on: macos-latest
-#    steps:
-#    - name: Install Arduino CLI
-#      run: brew install arduino-cli
-#    - uses: actions/checkout@v4
-#    - name: Clean up python binaries # Workaround for https://github.com/actions/setup-python/issues/577
-#      run: |
-#        rm -f /usr/local/bin/2to3*
-#        rm -f /usr/local/bin/idle3*
-#        rm -f /usr/local/bin/pydoc3*
-#        rm -f /usr/local/bin/python3*
-#        rm -f /usr/local/bin/python3*-config
-#    - name: Install Gazebo
-#      run: brew update && brew tap osrf/simulation && brew install gazebo11
-#    - name: Install SDL2
-#      run: brew install sdl2
-#    - name: Build simulator
-#      run: make build_simulator
+  build_simulator_macos:
+    runs-on: macos-15
+    steps:
+    - name: Install Arduino CLI
+      run: brew install arduino-cli
+    - uses: actions/checkout@v4
+    - name: Clean up python binaries # Workaround for https://github.com/actions/setup-python/issues/577
+      run: |
+        rm -f /usr/local/bin/2to3*
+        rm -f /usr/local/bin/idle3*
+        rm -f /usr/local/bin/pydoc3*
+        rm -f /usr/local/bin/python3*
+        rm -f /usr/local/bin/python3*-config
+    - name: Install Gazebo
+      run: brew update && brew tap osrf/simulation && brew install gazebo11
+    - name: Install SDL2
+      run: brew install sdl2
+    - name: Build simulator
+      run: make build_simulator
+
+  install_gz_ionic_macos:
+    runs-on: macos-15
+    steps:
+    - name: Install Gazebo
+      run: brew update && brew tap osrf/simulation && brew install gz-ionic
+
+  install_gz_jetty_macos:
+    runs-on: macos-15
+    steps:
+    - name: Install Gazebo
+      run: brew update && brew tap osrf/simulation && brew install gz-jetty
+
+  install_gz_harmonic_macos:
+    runs-on: macos-15
+    steps:
+    - name: Install Gazebo
+      run: brew update && brew tap osrf/simulation && brew install gz-harmonic
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -5,18 +5,18 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/.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",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/libraries/**",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32",
+				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/**",
+				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/dio_qspi/include",
 				"~/Arduino/libraries/**",
 				"/usr/include/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32/Arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -28,9 +28,10 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
+				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino"
 			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -50,19 +51,19 @@
 			"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",
+				"${workspaceFolder}/gazebo",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/libraries/**",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32",
+				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/include/**",
+				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**",
 				"/opt/homebrew/include/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32/Arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/flix.ino",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
@@ -74,9 +75,10 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
+				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino"
 			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -98,17 +100,17 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/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",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/libraries/**",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32",
+				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/**",
+				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32/Arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -120,9 +122,10 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
+				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino"
 			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
+			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
--- a/.vscode/extensions.json
+++ b/.vscode/extensions.json
@@ -2,6 +2,7 @@
 	// See https://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
 	"recommendations": [
 		"ms-vscode.cpptools",
+		"twxs.cmake",
 		"ms-vscode.cmake-tools",
 		"ms-python.python"
 	],
--- a/4
+++ b/4
@@ -13,10 +13,10 @@ monitor:

 dependencies .dependencies:
 	arduino-cli core update-index --config-file arduino-cli.yaml
-	arduino-cli core install esp32:esp32@3.2.0 --config-file arduino-cli.yaml
+	arduino-cli core install esp32:esp32@3.0.7 --config-file arduino-cli.yaml
 	arduino-cli lib update-index
 	arduino-cli lib install "FlixPeriph"
-	arduino-cli lib install "MAVLink"@2.0.16
+	arduino-cli lib install "MAVLink"@2.0.12
 	touch .dependencies

 gazebo/build cmake: gazebo/CMakeLists.txt
--- a/README.md
+++ b/README.md
@@ -1,7 +1,156 @@
 # Flix

-Minimal **Flix** quadcopter firmware implementation for the [book](https://quadcopter.dev).
+**Flix** (*flight + X*) — making an open source ESP32-based quadcopter from scratch.

-See the full code and documentation in the main branch: https://github.com/okalachev/flix.
+<table>
+  <tr>
+    <td align=center><strong>Version 1</strong> (3D-printed frame)</td>
+    <td align=center><strong>Version 0</strong></td>
+  </tr>
+  <tr>
+    <td><img src="docs/img/flix1.jpg" width=500 alt="Flix quadcopter"></td>
+    <td><img src="docs/img/flix.jpg" width=500 alt="Flix quadcopter"></td>
+  </tr>
+</table>

-<img src="docs/img/flix1.jpg" width=500 alt="Flix quadcopter">
+## Features
+
+* Simple and clean Arduino based source code.
+* Acro and Stabilized flight using remote control.
+* Precise simulation using Gazebo.
+* [In-RAM logging](docs/log.md).
+* Command line interface through USB port.
+* Wi-Fi support.
+* MAVLink support.
+* Control using mobile phone (with QGroundControl app).
+* Completely 3D-printed frame.
+* *Textbook and videos for students on writing a flight controller¹.*
+* *Position control and autonomous flights using external camera¹*.
+* [Building and running instructions](docs/build.md).
+
+*¹ — planned.*
+
+## It actually flies
+
+See detailed demo video (for version 0): https://youtu.be/8GzzIQ3C6DQ.
+
+<a href="https://youtu.be/8GzzIQ3C6DQ"><img width=500 src="https://i3.ytimg.com/vi/8GzzIQ3C6DQ/maxresdefault.jpg"></a>
+
+Version 1 test flight: https://t.me/opensourcequadcopter/42.
+
+<a href="https://t.me/opensourcequadcopter/42"><img width=500 src="docs/img/flight-video.jpg"></a>
+
+## Simulation
+
+The simulator is implemented using Gazebo and runs the original Arduino code:
+
+<img src="docs/img/simulator.png" width=500 alt="Flix simulator">
+
+See [instructions on running the simulation](docs/build.md).
+
+## Components (version 1)
+
+|Type|Part|Image|Quantity|
+|-|-|:-:|:-:|
+|Microcontroller board|ESP32 Mini|<img src="docs/img/esp32.jpg" width=100>|1|
+|IMU (and barometer²) board|GY‑91 (or other MPU‑9250/MPU‑6500 board), ICM‑20948³|<img src="docs/img/gy-91.jpg" width=90 align=center><img src="docs/img/icm-20948.jpg" width=100>|1|
+|Motor|8520 3.7V brushed motor (**shaft 0.8mm!**)|<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|
+|Pull-down resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|4|
+|3.7V Li-Po battery|LW 952540 (or any compatible by the size)|<img src="docs/img/battery.jpg" width=100>|1|
+|Li-Po Battery charger|Any|<img src="docs/img/charger.jpg" width=100>|1|
+|Screws for IMU board mounting|M3x5|<img src="docs/img/screw-m3.jpg" width=100>|2|
+|Screws for frame assembly|M1.4x5|<img src="docs/img/screw-m1.4.jpg" height=30 align=center>|4|
+|Frame bottom part|3D printed⁴:<br>[`flix-frame.stl`](docs/assets/flix-frame.stl) [`flix-frame.step`](docs/assets/flix-frame.step)|<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>|1|
+|*RC transmitter (optional)*|*KINGKONG TINY X8 or other⁵*|<img src="docs/img/tx.jpg" width=100>|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>
+*⁴ — 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.*
+
+Tools required for assembly:
+
+* 3D printer.
+* Soldering iron.
+* Solder wire (with flux).
+* Screwdrivers.
+* Multimeter.
+
+Feel free to modify the design and or code, and create your own improved versions of Flix! Send your results to the [official Telegram chat](https://t.me/opensourcequadcopterchat), or directly to the author ([E-mail](mailto:okalachev@gmail.com), [Telegram](https://t.me/okalachev)).
+
+## Schematics (version 1)
+
+### Simplified connection diagram
+
+<img src="docs/img/schematics1.svg" width=800 alt="Flix version 1 schematics">
+
+Motor connection scheme:
+
+<img src="docs/img/mosfet-connection.png" height=400 alt="MOSFET connection scheme">
+
+Complete diagram is Work-in-Progress.
+
+### Notes
+
+* Power ESP32 Mini with Li-Po battery using VCC (+) and GND (-) pins.
+* Connect the IMU board to the ESP32 Mini using VSPI, power it using 3.3V and GND pins:
+
+  |IMU pin|ESP32 pin|
+  |-|-|
+  |GND|GND|
+  |3.3V|3.3V|
+  |SCL *(SCK)*|SVP (GPIO18)|
+  |SDA *(MOSI)*|GPIO23|
+  |SAO *(MISO)*|GPIO19|
+  |NCS|GPIO5|
+
+* Solder pull-down resistors to the MOSFETs.
+* Connect the motors to the ESP32 Mini using MOSFETs, by following scheme:
+
+  |Motor|Position|Direction|Wires|GPIO|
+  |-|-|-|-|-|
+  |Motor 0|Rear left|Counter-clockwise|Black & White|GPIO12|
+  |Motor 1|Rear right|Clockwise|Blue & Red|GPIO13|
+  |Motor 2|Front right|Counter-clockwise|Black & White|GPIO14|
+  |Motor 3|Front left|Clockwise|Blue & Red|GPIO15|
+
+  Counter-clockwise motors have black and white wires and clockwise motors have blue and red wires.
+
+* Optionally connect the RC receiver to the ESP32's UART2:
+
+  |Receiver pin|ESP32 pin|
+  |-|-|
+  |GND|GND|
+  |VIN|VC (or 3.3V depending on the receiver)|
+  |Signal|GPIO4⁶|
+
+*⁶ — UART2 RX pin was [changed](https://docs.espressif.com/projects/arduino-esp32/en/latest/migration_guides/2.x_to_3.0.html#id14) to GPIO4 in Arduino ESP32 core 3.0.*
+
+### IMU placement
+
+Required IMU orientation on the drone is **FLU** (Forward, Left, Up)⁷:
+
+<img src="docs/img/flu.svg" width=400 alt="GY-91 axis">
+
+In case of using **FRD** orientation (Forward, Right, Down), use [the code for rotation](https://gist.github.com/okalachev/713db47e31bce643dbbc9539d166ce98).
+
+*⁷ — This X/Y/Z IMU axis orientation is used in the Flix IMU library, internal accel/gyro/mag axes differ.*
+
+## Version 0
+
+See the information on the obsolete version 0 in the [corresponding article](docs/version0.md).
+
+## Materials
+
+Subscribe to the Telegram channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter.
+
+Join the official Telegram chat: https://t.me/opensourcequadcopterchat.
+
+Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/.
--- a/docs/build.md
+++ b/docs/build.md
@@ -9,9 +9,9 @@ cd flix

 ## Simulation

-### Ubuntu
+### Ubuntu 20.04

-The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you have a newer version, consider using a virtual machine.
+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:

@@ -106,15 +106,13 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you h
 ### 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):
+2. Install ESP32 core, version 3.0.7 (version 2.x is not supported). See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
+3. Install 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.
+   * `MAVLink`, version 2.0.12.
+4. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
+5. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
+6. [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)

--- a/docs/firmware.md
+++ b/docs/firmware.md
@@ -12,8 +12,8 @@ The main loop is running at 1000 Hz. All the dataflow is happening through globa
 * `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, normalized to [0, 1] range; reverse rotation is possible.
+* `controls` *(float[])* — user control inputs from the RC, normalized to [-1, 1] range.
+* `motors` *(float[])* — motor outputs, normalized to [-1, 1] range; reverse rotation is possible.

 ## Source files

--- a/docs/img/flu.svg
+++ b/docs/img/flu.svg
--- a/docs/img/gy91-lfd.svg
+++ b/docs/img/gy91-lfd.svg
--- a/docs/img/mx.png
+++ b/docs/img/mx.png
--- a/docs/troubleshooting.md
+++ b/docs/troubleshooting.md
@@ -0,0 +1,32 @@
+# Troubleshooting
+
+## The sketch doesn't compile
+
+Do the following:
+
+* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](build.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.
+
+## The drone doesn't fly
+
+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.
+* **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere 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.
+* **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**.
+* **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while.
+* **Check the IMU sample rate**. Perform `imu` command. The `rate` field should be about 1000 (Hz).
+* **Check the IMU data**. Perform `imu` command, check raw accelerometer and gyro output. The output should change as you move the drone.
+* **Calibrate the accelerometer.** if is wasn't done before. Perform `ca` command and put the results to `imu.ino` file.
+* **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct.
+* **Check the motors**. Perform the following commands using Serial Monitor:
+  * `mfr` — should rotate front right motor (counter-clockwise).
+  * `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. Perform `rc` command and put the results to `rc.ino` file.
+* **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 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.
--- a/docs/version0.md
+++ b/docs/version0.md
@@ -0,0 +1,30 @@
+# Flix version 0
+
+Flix version 0 (obsolete):
+
+<img src="img/flix.jpg" width=500 alt="Flix quadcopter">
+
+## Components list
+
+|Type|Part|Image|Quantity|
+|-|-|-|-|
+|Microcontroller board|ESP32 Mini|<img src="img/esp32.jpg" width=100>|1|
+|IMU and barometer² board|GY-91 (or other MPU-9250 board)|<img src="img/gy-91.jpg" width=100>|1|
+|Quadcopter frame|K100|<img src="img/frame.jpg" width=100>|1|
+|Motor|8520 3.7V brushed motor (**shaft 0.8mm!**)|<img src="img/motor.jpeg" width=100>|4|
+|Propeller|Hubsan 55 mm|<img src="img/prop.jpg" width=100>|4|
+|Motor ESC|2.7A 1S Dual Way Micro Brush ESC|<img src="img/esc.jpg" width=100>|4|
+|RC transmitter|KINGKONG TINY X8|<img src="img/tx.jpg" width=100>|1|
+|RC receiver|DF500 (SBUS)|<img src="img/rx.jpg" width=100>|1|
+|~~SBUS inverter~~*||<img src="img/inv.jpg" width=100>|~~1~~|
+|Battery|3.7 Li-Po 850 MaH 60C|||
+|Battery charger||<img src="img/charger.jpg" width=100>|1|
+|Wires, connectors, tape, ...|||
+
+*\* — not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).*
+
+## Schematics
+
+<img src="img/schematics.svg" width=800 alt="Flix schematics">
+
+You can also check a user contributed [variant of complete circuit diagram](https://miro.com/app/board/uXjVN-dTjoo=/) of the drone.
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -6,9 +6,8 @@
 #include "pid.h"
 #include "vector.h"

-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern float loopRate;
-extern uint16_t channels[16];
+extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
+extern LowPassFilter<Vector> ratesFilter;

 const char* motd =
 "\nWelcome to\n"
@@ -20,23 +19,52 @@ const char* motd =
 "|__|     |_______||__| /__/ \\__\\\n\n"
 "Commands:\n\n"
 "help - show help\n"
+"show - show all parameters\n"
+"<name> <value> - set parameter\n"
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
 "rc - show RC data\n"
-"mot - show motor output\n"
+"mot - show motor data\n"
 "log - dump in-RAM log\n"
 "cr - calibrate RC\n"
 "cg - calibrate gyro\n"
 "ca - calibrate accel\n"
-"mfr, mfl, mrr, mrl - test motor (remove props)\n"
+"mfr, mfl, mrr, mrl - test motor\n"
 "reset - reset drone's state\n";

-void doCommand(const String& command) {
+const struct Param {
+	const char* name;
+	float* value;
+	float* value2;
+} params[] = {
+	{"rp", &rollRatePID.p, &pitchRatePID.p},
+	{"ri", &rollRatePID.i, &pitchRatePID.i},
+	{"rd", &rollRatePID.d, &pitchRatePID.d},
+
+	{"ap", &rollPID.p, &pitchPID.p},
+	{"ai", &rollPID.i, &pitchPID.i},
+	{"ad", &rollPID.d, &pitchPID.d},
+
+	{"yp", &yawRatePID.p, nullptr},
+	{"yi", &yawRatePID.i, nullptr},
+	{"yd", &yawRatePID.d, nullptr},
+
+	{"lpr", &ratesFilter.alpha, nullptr},
+	{"lpd", &rollRatePID.lpf.alpha, &pitchRatePID.lpf.alpha},
+
+	{"ss", &loopRate, nullptr},
+	{"dt", &dt, nullptr},
+	{"t", &t, nullptr},
+};
+
+void doCommand(String& command, String& value) {
 	if (command == "help" || command == "motd") {
 		Serial.println(motd);
+	} else if (command == "show") {
+		showTable();
 	} else if (command == "ps") {
-		Vector a = attitude.toEuler();
+		Vector a = attitude.toEulerZYX();
 		Serial.printf("roll: %f pitch: %f yaw: %f\n", a.x * RAD_TO_DEG, a.y * RAD_TO_DEG, a.z * RAD_TO_DEG);
 	} else if (command == "psq") {
 		Serial.printf("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
@@ -44,18 +72,18 @@ void doCommand(const String& command) {
 		printIMUInfo();
 		Serial.printf("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
 		Serial.printf("acc: %f %f %f\n", acc.x, acc.y, acc.z);
-		printIMUCalibration();
+		printIMUCal();
 		Serial.printf("rate: %f\n", loopRate);
 	} else if (command == "rc") {
-		Serial.printf("channels: ");
-		for (int i = 0; i < 16; i++) {
-			Serial.printf("%u ", channels[i]);
-		}
-		Serial.printf("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
-			controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
-		Serial.printf("mode: %s\n", getModeName());
+		Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
+			channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH],
+			channels[RC_CHANNEL_ROLL], channels[RC_CHANNEL_ARMED], channels[RC_CHANNEL_MODE]);
+		Serial.printf("Control: throttle %f yaw %f pitch %f roll %f armed %f mode %f\n",
+			controls[RC_CHANNEL_THROTTLE], controls[RC_CHANNEL_YAW], controls[RC_CHANNEL_PITCH],
+			controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_ARMED], controls[RC_CHANNEL_MODE]);
+		Serial.printf("Mode: %s\n", getModeName());
 	} else if (command == "mot") {
-		Serial.printf("front-right %f front-left %f rear-right %f rear-left %f\n",
+		Serial.printf("MOTOR front-right %f front-left %f rear-right %f rear-left %f\n",
 			motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
 	} else if (command == "log") {
 		dumpLog();
@@ -66,38 +94,76 @@ void doCommand(const String& command) {
 	} else if (command == "ca") {
 		calibrateAccel();
 	} else if (command == "mfr") {
-		testMotor(MOTOR_FRONT_RIGHT);
+		cliTestMotor(MOTOR_FRONT_RIGHT);
 	} else if (command == "mfl") {
-		testMotor(MOTOR_FRONT_LEFT);
+		cliTestMotor(MOTOR_FRONT_LEFT);
 	} else if (command == "mrr") {
-		testMotor(MOTOR_REAR_RIGHT);
+		cliTestMotor(MOTOR_REAR_RIGHT);
 	} else if (command == "mrl") {
-		testMotor(MOTOR_REAR_LEFT);
+		cliTestMotor(MOTOR_REAR_LEFT);
 	} else if (command == "reset") {
 		attitude = Quaternion();
-	} else if (command == "") {
-		// do nothing
 	} else {
+		float val = value.toFloat();
+		// TODO: on error returns 0, check invalid value
+
+		for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
+			if (command == params[i].name) {
+				*params[i].value = val;
+				if (params[i].value2 != nullptr) *params[i].value2 = val;
+				Serial.print(command);
+				Serial.print(" = ");
+				Serial.println(val, 4);
+				return;
+			}
+		}
 		Serial.println("Invalid command: " + command);
 	}
 }

-void handleInput() {
+void showTable() {
+	for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
+		Serial.print(params[i].name);
+		Serial.print(" ");
+		Serial.println(*params[i].value, 5);
+	}
+}
+
+void cliTestMotor(uint8_t n) {
+	Serial.printf("Testing motor %d\n", n);
+	motors[n] = 1;
+	delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306
+	sendMotors();
+	delay(3000);
+	motors[n] = 0;
+	sendMotors();
+	Serial.println("Done");
+}
+
+void parseInput() {
 	static bool showMotd = true;
-	static String input;
+	static String command;
+	static String value;
+	static bool parsingCommand = true;

 	if (showMotd) {
-		Serial.printf("%s\n", motd);
+		Serial.println(motd);
 		showMotd = false;
 	}

 	while (Serial.available()) {
 		char c = Serial.read();
 		if (c == '\n') {
-			doCommand(input);
-			input.clear();
+			parsingCommand = true;
+			if (!command.isEmpty()) {
+				doCommand(command, value);
+			}
+			command.clear();
+			value.clear();
+		} else if (c == ' ') {
+			parsingCommand = false;
 		} else {
-			input += c;
+			(parsingCommand ? command : value) += c;
 		}
 	}
 }
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -7,7 +7,6 @@
 #include "quaternion.h"
 #include "pid.h"
 #include "lpf.h"
-#include "util.h"

 #define PITCHRATE_P 0.05
 #define PITCHRATE_I 0.2
@@ -30,8 +29,8 @@
 #define YAW_P 3
 #define PITCHRATE_MAX radians(360)
 #define ROLLRATE_MAX radians(360)
-#define YAWRATE_MAX radians(300)
-#define TILT_MAX radians(30)
+#define YAWRATE_MAX radians(360)
+#define MAX_TILT radians(30)

 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz

@@ -51,8 +50,6 @@ Vector ratesTarget;
 Vector torqueTarget;
 float thrustTarget;

-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-
 void control() {
 	interpretRC();
 	failsafe();
@@ -69,39 +66,38 @@ void control() {
 }

 void interpretRC() {
-	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
-
+	armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5;

 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controlMode < 0.25) {
+	if (controls[RC_CHANNEL_MODE] < 0.25) {
 		mode = STAB;
-	} else if (controlMode < 0.75) {
+	} else if (controls[RC_CHANNEL_MODE] < 0.75) {
 		mode = STAB;
 	} else {
 		mode = STAB;
 	}

-	thrustTarget = controlThrottle;
+	thrustTarget = controls[RC_CHANNEL_THROTTLE];

 	if (mode == ACRO) {
 		yawMode = YAW_RATE;
-		ratesTarget.x = controlRoll * ROLLRATE_MAX;
-		ratesTarget.y = controlPitch * PITCHRATE_MAX;
-		ratesTarget.z = -controlYaw * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
+		ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
+		ratesTarget.y = controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX;
+		ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU

 	} else if (mode == STAB) {
-		yawMode = controlYaw == 0 ? YAW : YAW_RATE;
+		yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;

-		attitudeTarget = Quaternion::fromEuler(Vector(
-			controlRoll * TILT_MAX,
-			controlPitch * TILT_MAX,
+		attitudeTarget = Quaternion::fromEulerZYX(Vector(
+			controls[RC_CHANNEL_ROLL] * MAX_TILT,
+			controls[RC_CHANNEL_PITCH] * MAX_TILT,
 			attitudeTarget.getYaw()));
-		ratesTarget.z = -controlYaw * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
+		ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU

 	} else if (mode == MANUAL) {
 		// passthrough mode
 		yawMode = YAW_RATE;
-		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
+		torqueTarget = Vector(controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_PITCH], -controls[RC_CHANNEL_YAW]) * 0.01;
 	}

 	if (yawMode == YAW_RATE || !motorsActive()) {
@@ -119,10 +115,10 @@ void controlAttitude() {
 	}

 	const Vector up(0, 0, 1);
-	Vector upActual = Quaternion::rotateVector(up, attitude);
-	Vector upTarget = Quaternion::rotateVector(up, attitudeTarget);
+	Vector upActual = attitude.rotate(up);
+	Vector upTarget = attitudeTarget.rotate(up);

-	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
+	Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);

 	ratesTarget.x = rollPID.update(error.x, dt);
 	ratesTarget.y = pitchPID.update(error.y, dt);
@@ -166,6 +162,10 @@ void controlTorque() {
 	motors[3] = constrain(motors[3], 0, 1);
 }

+bool motorsActive() {
+	return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
+}
+
 const char* getModeName() {
 	switch (mode) {
 		case MANUAL: return "MANUAL";
--- a/flix/estimate.ino
+++ b/flix/estimate.ino
@@ -6,9 +6,9 @@
 #include "quaternion.h"
 #include "vector.h"
 #include "lpf.h"
-#include "util.h"

-#define WEIGHT_ACC 0.003
+#define ONE_G 9.807f
+#define WEIGHT_ACC 0.5f
 #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz

 LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
@@ -23,7 +23,8 @@ void applyGyro() {
 	rates = ratesFilter.update(gyro);

 	// apply rates to attitude
-	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));
+	attitude *= Quaternion::fromAngularRates(rates * dt);
+	attitude.normalize();
 }

 void applyAcc() {
@@ -34,9 +35,10 @@ void applyAcc() {
 	if (!landed) return;

 	// calculate accelerometer correction
-	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
-	Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
+	Vector up = attitude.rotate(Vector(0, 0, 1));
+	Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;

 	// apply correction
-	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
+	attitude *= Quaternion::fromAngularRates(correction);
+	attitude.normalize();
 }
--- a/flix/failsafe.ino
+++ b/flix/failsafe.ino
@@ -6,21 +6,17 @@
 #define RC_LOSS_TIMEOUT 0.2
 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero

-extern float controlTime;
-
-// RC loss failsafe
 void failsafe() {
-	if (t - controlTime > RC_LOSS_TIMEOUT) {
+	if (t - controlsTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }

-// Smooth descend on RC lost
 void descend() {
+	// Smooth descend on RC lost
 	mode = STAB;
-	controlRoll = 0;
-	controlPitch = 0;
-	controlYaw = 0;
-	controlThrottle -= dt / DESCEND_TIME;
-	if (controlThrottle < 0) controlThrottle = 0;
+	controls[RC_CHANNEL_ROLL] = 0;
+	controls[RC_CHANNEL_PITCH] = 0;
+	controls[RC_CHANNEL_YAW] = 0;
+	controls[RC_CHANNEL_THROTTLE] -= dt / DESCEND_TIME;
 }
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -5,34 +5,51 @@

 #include "vector.h"
 #include "quaternion.h"
-#include "util.h"

 #define SERIAL_BAUDRATE 115200
+
 #define WIFI_ENABLED 1

+#define RC_CHANNELS 16
+#define RC_CHANNEL_ROLL 0
+#define RC_CHANNEL_PITCH 1
+#define RC_CHANNEL_THROTTLE 2
+#define RC_CHANNEL_YAW 3
+#define RC_CHANNEL_ARMED 4
+#define RC_CHANNEL_MODE 5
+
+#define MOTOR_REAR_LEFT 0
+#define MOTOR_REAR_RIGHT 1
+#define MOTOR_FRONT_RIGHT 2
+#define MOTOR_FRONT_LEFT 3
+
 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 loopRate; // loop rate, Hz
+int16_t channels[RC_CHANNELS]; // raw rc channels
+float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
+float controlsTime; // time of the last controls update
 Vector gyro; // gyroscope data
 Vector acc; // accelerometer data, m/s/s
 Vector rates; // filtered angular rates, rad/s
 Quaternion attitude; // estimated attitude
-float motors[4]; // normalized motors thrust in range [0..1]
+float motors[4]; // normalized motors thrust in range [-1..1]

 void setup() {
 	Serial.begin(SERIAL_BAUDRATE);
-	Serial.println("Initializing flix\n");
+	Serial.println("Initializing flix");
 	disableBrownOut();
 	setupLED();
 	setupMotors();
 	setLED(true);
-#if WIFI_ENABLED
+#if WIFI_ENABLED == 1
 	setupWiFi();
 #endif
 	setupIMU();
 	setupRC();
+
 	setLED(false);
-	Serial.println("Initializing complete\n");
+	Serial.println("Initializing complete");
 }

 void loop() {
@@ -42,8 +59,8 @@ void loop() {
 	estimate();
 	control();
 	sendMotors();
-	handleInput();
-#if WIFI_ENABLED
+	parseInput();
+#if WIFI_ENABLED == 1
 	processMavlink();
 #endif
 	logData();
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -2,16 +2,20 @@
 // Repository: https://github.com/okalachev/flix

 // Work with the IMU sensor
+// IMU is oriented FLU (front-left-up) style.
+// In case of FRD (front-right-down) orientation of the IMU, use this code:
+// https://gist.github.com/okalachev/713db47e31bce643dbbc9539d166ce98.

 #include <SPI.h>
 #include <MPU9250.h>
-#include "util.h"

-MPU9250 IMU(SPI);
+#define ONE_G 9.80665

 // NOTE: use 'ca' command to calibrate the accelerometer and put the values here
-Vector accBias;
+Vector accBias(0, 0, 0);
 Vector accScale(1, 1, 1);
+
+MPU9250 IMU(SPI);
 Vector gyroBias;

 void setupIMU() {
@@ -41,16 +45,6 @@ void readIMU() {
 	// apply scale and bias
 	acc = (acc - accBias) / accScale;
 	gyro = gyro - gyroBias;
-	// rotate
-	rotateIMU(acc);
-	rotateIMU(gyro);
-}
-
-void rotateIMU(Vector& data) {
-	// Rotate from LFD to FLU
-	// NOTE: In case of using other IMU orientation, change this line:
-	data = Vector(data.y, data.x, -data.z);
-	// Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation
 }

 void calibrateGyro() {
@@ -66,7 +60,7 @@ void calibrateGyro() {
 	}
 	gyroBias = gyroBias / samples;

-	printIMUCalibration();
+	printIMUCal();
 	configureIMU();
 }

@@ -75,27 +69,20 @@ void calibrateAccel() {
 	IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode

 	Serial.setTimeout(60000);
-	Serial.print("1/6 Place level [enter] ");
-	Serial.readStringUntil('\n');
+	Serial.print("Place level [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("2/6 Place nose up [enter] ");
-	Serial.readStringUntil('\n');
+	Serial.print("Place nose up [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("3/6 Place nose down [enter] ");
-	Serial.readStringUntil('\n');
+	Serial.print("Place nose down [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("4/6 Place on right side [enter] ");
-	Serial.readStringUntil('\n');
+	Serial.print("Place on right side [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("5/6 Place on left side [enter] ");
-	Serial.readStringUntil('\n');
+	Serial.print("Place on left side [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("6/6 Place upside down [enter] ");
-	Serial.readStringUntil('\n');
+	Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();

-	printIMUCalibration();
-	Serial.print("✓ Calibration done!\n");
+	printIMUCal();
 	configureIMU();
 }

@@ -121,15 +108,18 @@ void calibrateAccelOnce() {
 	if (acc.x < accMin.x) accMin.x = acc.x;
 	if (acc.y < accMin.y) accMin.y = acc.y;
 	if (acc.z < accMin.z) accMin.z = acc.z;
+	Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z);
+	Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z);
+	Serial.printf("min %f %f %f\n", accMin.x, accMin.y, accMin.z);
 	// Compute scale and bias
 	accScale = (accMax - accMin) / 2 / ONE_G;
 	accBias = (accMax + accMin) / 2;
 }

-void printIMUCalibration() {
-	Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
-	Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
-	Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);
+void printIMUCal() {
+	Serial.printf("gyro bias: %f, %f, %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
+	Serial.printf("accel bias: %f, %f, %f\n", accBias.x, accBias.y, accBias.z);
+	Serial.printf("accel scale: %f, %f, %f\n", accScale.x, accScale.y, accScale.z);
 }

 void printIMUInfo() {
--- a/flix/led.ino
+++ b/flix/led.ino
@@ -21,3 +21,7 @@ void setLED(bool on) {
 	digitalWrite(LED_BUILTIN, on ? HIGH : LOW);
 	state = on;
 }
+
+void blinkLED() {
+	setLED(micros() / BLINK_PERIOD % 2);
+}
--- a/flix/log.ino
+++ b/flix/log.ino
@@ -26,12 +26,12 @@ void logData() {
 	logBuffer[logPointer][4] = ratesTarget.x;
 	logBuffer[logPointer][5] = ratesTarget.y;
 	logBuffer[logPointer][6] = ratesTarget.z;
-	logBuffer[logPointer][7] = attitude.toEuler().x;
-	logBuffer[logPointer][8] = attitude.toEuler().y;
-	logBuffer[logPointer][9] = attitude.toEuler().z;
-	logBuffer[logPointer][10] = attitudeTarget.toEuler().x;
-	logBuffer[logPointer][11] = attitudeTarget.toEuler().y;
-	logBuffer[logPointer][12] = attitudeTarget.toEuler().z;
+	logBuffer[logPointer][7] = attitude.toEulerZYX().x;
+	logBuffer[logPointer][8] = attitude.toEulerZYX().y;
+	logBuffer[logPointer][9] = attitude.toEulerZYX().z;
+	logBuffer[logPointer][10] = attitudeTarget.toEulerZYX().x;
+	logBuffer[logPointer][11] = attitudeTarget.toEulerZYX().y;
+	logBuffer[logPointer][12] = attitudeTarget.toEulerZYX().z;
 	logBuffer[logPointer][13] = thrustTarget;

 	logPointer++;
--- a/flix/lpf.h
+++ b/flix/lpf.h
@@ -22,8 +22,7 @@ public:
 			output = input;
 			initialized = true;
 		}
-
-		return output += alpha * (input - output);
+		return output = output * (1 - alpha) + input * alpha;
 	}

 	void setCutOffFrequency(float cutOffFreq, float dt) {
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -3,7 +3,7 @@

 // MAVLink communication

-#if WIFI_ENABLED
+#if WIFI_ENABLED == 1

 #include <MAVLink.h>

@@ -13,8 +13,6 @@
 #define MAVLINK_CONTROL_SCALE 0.7f
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f

-extern float controlTime;
-
 void processMavlink() {
 	sendMavlink();
 	receiveMavlink();
@@ -30,8 +28,8 @@ void sendMavlink() {
 	if (t - lastSlow >= PERIOD_SLOW) {
 		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),
+		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 : 0),
 			0, MAV_STATE_STANDBY);
 		sendMessage(&msg);
 	}
@@ -40,12 +38,15 @@ void sendMavlink() {
 		lastFast = t;

 		const float zeroQuat[] = {0, 0, 0, 0};
+		Quaternion attitudeFRD = FLU2FRD(attitude); // MAVLink uses FRD coordinate system
 		mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
-			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
+			time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat);
 		sendMessage(&msg);

-		mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
-			channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
+		mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0,
+			controls[0] * 10000, controls[1] * 10000, controls[2] * 10000,
+			controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,
+			INT16_MAX, INT16_MAX, UINT8_MAX);
 		sendMessage(&msg);

 		float actuator[32];
@@ -54,8 +55,8 @@ void sendMavlink() {
 		sendMessage(&msg);

 		mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
-			acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd
-			gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
+			acc.x * 1000, acc.y * 1000, acc.z * 1000,
+			gyro.x * 1000, gyro.y * 1000, gyro.z * 1000,
 			0, 0, 0, 0);
 		sendMessage(&msg);
 	}
@@ -82,21 +83,25 @@ void receiveMavlink() {
 }

 void handleMavlink(const void *_msg) {
-	const mavlink_message_t& msg = *(mavlink_message_t *)_msg;
+	mavlink_message_t *msg = (mavlink_message_t *)_msg;
+	if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
+		mavlink_manual_control_t manualControl;
+		mavlink_msg_manual_control_decode(msg, &manualControl);
+		controls[RC_CHANNEL_THROTTLE] = manualControl.z / 1000.0f;
+		controls[RC_CHANNEL_PITCH] = manualControl.x / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controls[RC_CHANNEL_ROLL] = manualControl.y / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controls[RC_CHANNEL_YAW] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controls[RC_CHANNEL_MODE] = 1; // STAB mode
+		controls[RC_CHANNEL_ARMED] = 1; // armed
+		controlsTime = t;

-	if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
-		mavlink_manual_control_t m;
-		mavlink_msg_manual_control_decode(&msg, &m);
-		controlThrottle = m.z / 1000.0f;
-		controlPitch = m.x / 1000.0f * MAVLINK_CONTROL_SCALE;
-		controlRoll = m.y / 1000.0f * MAVLINK_CONTROL_SCALE;
-		controlYaw = m.r / 1000.0f * MAVLINK_CONTROL_SCALE;
-		controlMode = 1; // STAB mode
-		controlArmed = 1; // armed
-		controlTime = t;
-
-		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
+		if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
 	}
 }

+// Convert Forward-Left-Up to Forward-Right-Down quaternion
+inline Quaternion FLU2FRD(const Quaternion &q) {
+	return Quaternion(q.w, q.x, -q.y, -q.z);
+}
+
 #endif
--- a/flix/motors.ino
+++ b/flix/motors.ino
@@ -2,22 +2,16 @@
 // Repository: https://github.com/okalachev/flix

 // Motors output control using MOSFETs
-
-#include "util.h"
+// In case of using ESC, use this version of the code: https://gist.github.com/okalachev/8871d3a94b6b6c0a298f41a4edd34c61.
+// Motor: 8520 3.7V

 #define MOTOR_0_PIN 12 // rear left
 #define MOTOR_1_PIN 13 // rear right
 #define MOTOR_2_PIN 14 // front right
 #define MOTOR_3_PIN 15 // front left

-#define PWM_FREQUENCY 78000
-#define PWM_RESOLUTION 10
-
-// Motors array indexes:
-const int MOTOR_REAR_LEFT = 0;
-const int MOTOR_REAR_RIGHT = 1;
-const int MOTOR_FRONT_RIGHT = 2;
-const int MOTOR_FRONT_LEFT = 3;
+#define PWM_FREQUENCY 200
+#define PWM_RESOLUTION 8

 void setupMotors() {
 	Serial.println("Setup Motors");
@@ -32,30 +26,14 @@ void setupMotors() {
 	Serial.println("Motors initialized");
 }

-int getDutyCycle(float value) {
-	value = constrain(value, 0, 1);
-	float duty = mapff(value, 0, 1, 0, (1 << PWM_RESOLUTION) - 1);
-	return round(duty);
+uint8_t signalToDutyCycle(float control) {
+	float duty = mapff(control, 0, 1, 0, (1 << PWM_RESOLUTION) - 1);
+	return round(constrain(duty, 0, (1 << PWM_RESOLUTION) - 1));
 }

 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]));
-}
-
-bool motorsActive() {
-	return motors[0] != 0 || motors[1] != 0 || motors[2] != 0 || motors[3] != 0;
-}
-
-void testMotor(int n) {
-	Serial.printf("Testing motor %d\n", n);
-	motors[n] = 1;
-	delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306
-	sendMotors();
-	delay(3000);
-	motors[n] = 0;
-	sendMotors();
-	Serial.printf("Done\n");
+	ledcWrite(MOTOR_0_PIN, signalToDutyCycle(motors[0]));
+	ledcWrite(MOTOR_1_PIN, signalToDutyCycle(motors[1]));
+	ledcWrite(MOTOR_2_PIN, signalToDutyCycle(motors[2]));
+	ledcWrite(MOTOR_3_PIN, signalToDutyCycle(motors[3]));
 }
--- a/flix/quaternion.h
+++ b/flix/quaternion.h
@@ -15,22 +15,22 @@ public:

 	Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {};

-	static Quaternion fromAxisAngle(const Vector& axis, float angle) {
+	static Quaternion fromAxisAngle(float a, float b, float c, float angle) {
 		float halfAngle = angle * 0.5;
 		float sin2 = sin(halfAngle);
 		float cos2 = cos(halfAngle);
-		float sinNorm = sin2 / axis.norm();
-		return Quaternion(cos2, axis.x * sinNorm, axis.y * sinNorm, axis.z * sinNorm);
+		float sinNorm = sin2 / sqrt(a * a + b * b + c * c);
+		return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);
 	}

-	static Quaternion fromRotationVector(const Vector& rotation) {
-		if (rotation.zero()) {
+	static Quaternion fromAngularRates(const Vector& rates) {
+		if (rates.zero()) {
 			return Quaternion();
 		}
-		return Quaternion::fromAxisAngle(rotation, rotation.norm());
+		return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
 	}

-	static Quaternion fromEuler(const Vector& euler) {
+	static Quaternion fromEulerZYX(const Vector& euler) {
 		float cx = cos(euler.x / 2);
 		float cy = cos(euler.y / 2);
 		float cz = cos(euler.z / 2);
@@ -60,38 +60,14 @@ public:
 		return ret;
 	}

-	bool finite() const {
-		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
-	}
-
-	float norm() const {
-		return sqrt(w * w + x * x + y * y + z * z);
-	}
-
-	void normalize() {
-		float n = norm();
-		w /= n;
-		x /= n;
-		y /= n;
-		z /= n;
-	}
-
-	void toAxisAngle(Vector& axis, float& angle) const {
+	void toAxisAngle(float& a, float& b, float& c, float& angle) {
 		angle = acos(w) * 2;
-		axis.x = x / sin(angle / 2);
-		axis.y = y / sin(angle / 2);
-		axis.z = z / sin(angle / 2);
+		a = x / sin(angle / 2);
+		b = y / sin(angle / 2);
+		c = z / sin(angle / 2);
 	}

-	Vector toRotationVector() const {
-		if (w == 1 && x == 0 && y == 0 && z == 0) return Vector(0, 0, 0); // neutral quaternion
-		float angle;
-		Vector axis;
-		toAxisAngle(axis, angle);
-		return angle * axis;
-	}
-
-	Vector toEuler() const {
+	Vector toEulerZYX() const {
 		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87
 		Vector euler;
 		float sqx = x * x;
@@ -136,12 +112,21 @@ public:

 	void setYaw(float yaw) {
 		// TODO: optimize?
-		Vector euler = toEuler();
+		Vector euler = toEulerZYX();
 		euler.z = yaw;
-		(*this) = Quaternion::fromEuler(euler);
+		(*this) = Quaternion::fromEulerZYX(euler);
 	}

-	Quaternion operator * (const Quaternion& q) const {
+	Quaternion& operator *= (const Quaternion& q) {
+		Quaternion ret(
+			w * q.w - x * q.x - y * q.y - z * q.z,
+			w * q.x + x * q.w + y * q.z - z * q.y,
+			w * q.y + y * q.w + z * q.x - x * q.z,
+			w * q.z + z * q.w + x * q.y - y * q.x);
+		return (*this = ret);
+	}
+
+	Quaternion operator * (const Quaternion& q) {
 		return Quaternion(
 			w * q.w - x * q.x - y * q.y - z * q.z,
 			w * q.x + x * q.w + y * q.z - z * q.y,
@@ -149,14 +134,6 @@ public:
 			w * q.z + z * q.w + x * q.y - y * q.x);
 	}

-	bool operator == (const Quaternion& q) const {
-		return w == q.w && x == q.x && y == q.y && z == q.z;
-	}
-
-	bool operator != (const Quaternion& q) const {
-		return !(*this == q);
-	}
-
 	Quaternion inversed() const {
 		float normSqInv = 1 / (w * w + x * x + y * y + z * z);
 		return Quaternion(
@@ -166,39 +143,37 @@ public:
 			-z * normSqInv);
 	}

-	Vector conjugate(const Vector& v) const {
+	float norm() const {
+		return sqrt(w * w + x * x + y * y + z * z);
+	}
+
+	void normalize() {
+		float n = norm();
+		w /= n;
+		x /= n;
+		y /= n;
+		z /= n;
+	}
+
+	Vector conjugate(const Vector& v) {
 		Quaternion qv(0, v.x, v.y, v.z);
 		Quaternion res = (*this) * qv * inversed();
 		return Vector(res.x, res.y, res.z);
 	}

-	Vector conjugateInversed(const Vector& v) const {
+	Vector conjugateInversed(const Vector& v) {
 		Quaternion qv(0, v.x, v.y, v.z);
 		Quaternion res = inversed() * qv * (*this);
 		return Vector(res.x, res.y, res.z);
 	}

-	// Rotate quaternion by quaternion
-	static Quaternion rotate(const Quaternion& a, const Quaternion& b, const bool normalize = true) {
-		Quaternion rotated = a * b;
-		if (normalize) {
-			rotated.normalize();
-		}
-		return rotated;
-	}
-
 	// Rotate vector by quaternion
-	static Vector rotateVector(const Vector& v, const Quaternion& q) {
-		return q.conjugateInversed(v);
+	inline Vector rotate(const Vector& v) {
+		return conjugateInversed(v);
 	}

-	// Quaternion between two quaternions a and b
-	static Quaternion between(const Quaternion& a, const Quaternion& b, const bool normalize = true) {
-		Quaternion q = a * b.inversed();
-		if (normalize) {
-			q.normalize();
-		}
-		return q;
+	inline bool finite() const {
+		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
 	}

 	size_t printTo(Print& p) const {
--- a/flix/rc.ino
+++ b/flix/rc.ino
@@ -4,77 +4,52 @@
 // Work with the RC receiver

 #include <SBUS.h>
-#include "util.h"
-
-SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
-
-uint16_t channels[16]; // raw rc channels
-float controlTime; // time of the last controls update
-

 // NOTE: use 'cr' command to calibrate the RC and put the values here
-int channelZero[] = {992, 992, 172, 992, 172, 172, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-int channelMax[] = {1811, 1811, 1811, 1811, 1811, 1811, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+int channelNeutral[] = {995, 883, 200, 972, 512, 512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

-// Channels mapping:
-int rollChannel = 0;
-int pitchChannel = 1;
-int throttleChannel = 2;
-int yawChannel = 3;
-int armedChannel = 4;
-int modeChannel = 5;
+SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins

 void setupRC() {
 	Serial.println("Setup RC");
 	RC.begin();
 }

-bool readRC() {
+void readRC() {
 	if (RC.read()) {
 		SBUSData data = RC.data();
-		for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
+		memcpy(channels, data.ch, sizeof(channels)); // copy channels data
 		normalizeRC();
-		controlTime = t;
-		return true;
+		controlsTime = t;
 	}
-	return false;
 }

 void normalizeRC() {
-	float controls[16];
-	for (int i = 0; i < 16; i++) {
-		controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
+	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
+		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
 	}
-	// Update control values
-	controlRoll = controls[rollChannel];
-	controlPitch = controls[pitchChannel];
-	controlYaw = controls[yawChannel];
-	controlThrottle = controls[throttleChannel];
-	controlArmed = controls[armedChannel];
-	controlMode = controls[modeChannel];
 }

 void calibrateRC() {
-	Serial.println("Calibrate RC: move all sticks to maximum positions [4 sec]");
+	Serial.println("Calibrate RC: move all sticks to maximum positions within 4 seconds");
 	Serial.println("··o     ··o\n···     ···\n···     ···");
 	delay(4000);
-	while (!readRC());
-	for (int i = 0; i < 16; i++) {
+	for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated
+	for (int i = 0; i < RC_CHANNELS; i++) {
 		channelMax[i] = channels[i];
 	}
-	Serial.println("Calibrate RC: move all sticks to neutral positions [4 sec]");
+	Serial.println("Calibrate RC: move all sticks to neutral positions within 4 seconds");
 	Serial.println("···     ···\n···     ·o·\n·o·     ···");
 	delay(4000);
-	while (!readRC());
-	for (int i = 0; i < 16; i++) {
-		channelZero[i] = channels[i];
+	for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated
+	for (int i = 0; i < RC_CHANNELS; i++) {
+		channelNeutral[i] = channels[i];
 	}
-	printRCCalibration();
+	printRCCal();
 }

-void printRCCalibration() {
-	for (int i = 0; i < sizeof(channelZero) / sizeof(channelZero[0]); i++) Serial.printf("%d ", channelZero[i]);
-	Serial.printf("\n");
-	for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) Serial.printf("%d ", channelMax[i]);
-	Serial.printf("\n");
+void printRCCal() {
+	printArray(channelNeutral, RC_CHANNELS);
+	printArray(channelMax, RC_CHANNELS);
 }
--- a/flix/time.ino
+++ b/flix/time.ino
@@ -3,8 +3,6 @@

 // Time related functions

-float loopRate; // Hz
-
 void step() {
 	float now = micros() / 1000000.0;
 	dt = now - t;
--- a/flix/util.ino
+++ b/flix/util.ino
@@ -3,14 +3,10 @@

 // Utility functions

-#pragma once
-
 #include <math.h>
 #include <soc/soc.h>
 #include <soc/rtc_cntl_reg.h>

-const float ONE_G = 9.80665;
-
 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;
 }
@@ -30,7 +26,17 @@ float wrapAngle(float angle) {
 	return angle;
 }

+template <typename T>
+void printArray(T arr[], int size) {
+	Serial.print("{");
+	for (uint8_t i = 0; i < size; i++) {
+		Serial.print(arr[i]);
+		if (i < size - 1) Serial.print(", ");
+	}
+	Serial.println("}");
+}
+
 // Disable reset on low voltage
 void disableBrownOut() {
-	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
+	WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
 }
--- a/flix/vector.h
+++ b/flix/vector.h
@@ -13,18 +13,14 @@ public:

 	Vector(float x, float y, float z): x(x), y(y), z(z) {};

-	bool zero() const {
-		return x == 0 && y == 0 && z == 0;
-	}
-
-	bool finite() const {
-		return isfinite(x) && isfinite(y) && isfinite(z);
-	}
-
 	float norm() const {
 		return sqrt(x * x + y * y + z * z);
 	}

+	bool zero() const {
+		return x == 0 && y == 0 && z == 0;
+	}
+
 	void normalize() {
 		float n = norm();
 		x /= n;
@@ -32,10 +28,6 @@ public:
 		z /= n;
 	}

-	Vector operator + (const float b) const {
-		return Vector(x + b, y + b, z + b);
-	}
-
 	Vector operator * (const float b) const {
 		return Vector(x * b, y * b, z * b);
 	}
@@ -52,14 +44,6 @@ public:
 		return Vector(x - b.x, y - b.y, z - b.z);
 	}

-	Vector& operator += (const Vector& b) {
-		return *this = *this + b;
-	}
-
-	Vector& operator -= (const Vector& b) {
-		return *this = *this - b;
-	}
-
 	// Element-wise multiplication
 	Vector operator * (const Vector& b) const {
 		return Vector(x * b.x, y * b.y, z * b.z);
@@ -70,14 +54,18 @@ public:
 		return Vector(x / b.x, y / b.y, z / b.z);
 	}

-	bool operator == (const Vector& b) const {
+	inline bool operator == (const Vector& b) const {
 		return x == b.x && y == b.y && z == b.z;
 	}

-	bool operator != (const Vector& b) const {
+	inline bool operator != (const Vector& b) const {
 		return !(*this == b);
 	}

+	inline bool finite() const {
+		return isfinite(x) && isfinite(y) && isfinite(z);
+	}
+
 	static float dot(const Vector& a, const Vector& b) {
 		return a.x * b.x + a.y * b.y + a.z * b.z;
 	}
@@ -86,18 +74,18 @@ public:
 		return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
 	}

-	static float angleBetween(const Vector& a, const Vector& b) {
+	static float angleBetweenVectors(const Vector& a, const Vector& b) {
 		return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1));
 	}

-	static Vector rotationVectorBetween(const Vector& a, const Vector& b) {
+	static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) {
 		Vector direction = cross(a, b);
 		if (direction.zero()) {
 			// vectors are opposite, return any perpendicular vector
 			return cross(a, Vector(1, 0, 0));
 		}
 		direction.normalize();
-		float angle = angleBetween(a, b);
+		float angle = angleBetweenVectors(a, b);
 		return direction * angle;
 	}

@@ -108,6 +96,3 @@ public:
 			p.print(z, 15);
 	}
 };
-
-Vector operator * (const float a, const Vector& b) { return b * a; }
-Vector operator + (const float a, const Vector& b) { return b + a; }
--- a/flix/wifi.ino
+++ b/flix/wifi.ino
@@ -3,7 +3,7 @@

 // Wi-Fi support

-#if WIFI_ENABLED
+#if WIFI_ENABLED == 1

 #include <WiFi.h>
 #include <WiFiAP.h>
@@ -11,19 +11,20 @@

 #define WIFI_SSID "flix"
 #define WIFI_PASSWORD "flixwifi"
+#define WIFI_UDP_IP "255.255.255.255"
 #define WIFI_UDP_PORT 14550
-#define WIFI_UDP_REMOTE_PORT 14550

 WiFiUDP udp;

 void setupWiFi() {
 	Serial.println("Setup Wi-Fi");
 	WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
+	IPAddress myIP = WiFi.softAPIP();
 	udp.begin(WIFI_UDP_PORT);
 }

 void sendWiFi(const uint8_t *buf, int len) {
-	udp.beginPacket(WiFi.softAPBroadcastIP(), WIFI_UDP_REMOTE_PORT);
+	udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT);
 	udp.write(buf, len);
 	udp.endPacket();
 }
--- a/gazebo/Arduino.h
+++ b/gazebo/Arduino.h
@@ -99,7 +99,7 @@ public:
 class HardwareSerial: public Print {
 public:
 	void begin(unsigned long baud) {
-		// server is running in background by default, so it doesn't have access to stdin
+		// server is running in background by default, so doesn't have access to stdin
 		// https://github.com/gazebosim/gazebo-classic/blob/d45feeb51f773e63960616880b0544770b8d1ad7/gazebo/gazebo_main.cc#L216
 		// set foreground process group to current process group to allow reading from stdin
 		// https://stackoverflow.com/questions/58918188/why-is-stdin-not-propagated-to-child-process-of-different-process-group
@@ -133,9 +133,6 @@ void delay(uint32_t ms) {
 	std::this_thread::sleep_for(std::chrono::milliseconds(ms));
 }

-bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
-bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
-
 unsigned long __micros;
 unsigned long __resetTime = 0;

--- a/gazebo/SBUS.h
+++ b/gazebo/SBUS.h
@@ -14,12 +14,11 @@ public:
 	SBUS(HardwareSerial& bus, const bool inv = true) {};
 	SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
 	void begin() {};
-	bool read() { return joystickInit(); };
+	bool read() { return joystickGet(); };
 	SBUSData data() {
 		SBUSData data;
-		joystickGet(data.ch);
-		for (int i = 0; i < 16; i++) {
-			data.ch[i] = map(data.ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
+		for (uint8_t i = 0; i < 16; i++) {
+			data.ch[i] = channels[i];
 		}
 		return data;
 	};
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -10,12 +10,22 @@
 #include "Arduino.h"
 #include "wifi.h"

+#define RC_CHANNELS 16
+
+#define MOTOR_REAR_LEFT 0
+#define MOTOR_FRONT_LEFT 3
+#define MOTOR_FRONT_RIGHT 2
+#define MOTOR_REAR_RIGHT 1
+
 #define WIFI_ENABLED 1

 float t = NAN;
 float dt;
+float loopRate;
 float motors[4];
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
+int16_t channels[16]; // raw rc channels
+float controls[RC_CHANNELS];
+float controlsTime;
 Vector acc;
 Vector gyro;
 Vector rates;
@@ -31,11 +41,9 @@ void controlAttitude();
 void controlRate();
 void controlTorque();
 void showTable();
-void sendMotors();
 bool motorsActive();
-void doCommand(const String& command);
-void normalizeRC();
-void printRCCalibration();
+void cliTestMotor(uint8_t n);
+void printRCCal();
 void processMavlink();
 void sendMavlink();
 void sendMessage(const void *msg);
@@ -43,11 +51,12 @@ void receiveMavlink();
 void handleMavlink(const void *_msg);
 void failsafe();
 void descend();
-inline Quaternion fluToFrd(const Quaternion &q);
+inline Quaternion FLU2FRD(const Quaternion &q);

 // mocks
 void setLED(bool on) {};
 void calibrateGyro() { printf("Skip gyro calibrating\n"); };
 void calibrateAccel() { printf("Skip accel calibrating\n"); };
-void printIMUCalibration() { printf("cal: N/A\n"); };
+void sendMotors() {};
+void printIMUCal() { printf("cal: N/A\n"); };
 void printIMUInfo() {};
--- a/gazebo/joystick.h
+++ b/gazebo/joystick.h
@@ -8,26 +8,22 @@
 #include <iostream>

 // simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
-const int channelZeroOverride[] = {1500, 0, 1000, 1500, 1500, 1000};
-const int channelMaxOverride[] = {2000, 2000, 2000, 2000, 2000, 2000};
+const int channelNeutralOverride[] = {-258, -258, -27349, 0, -27349, 0};
+const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1};

-// channels mapping overrides
-const int rollChannelOverride = 3;
-const int pitchChannelOverride = 4;
-const int throttleChannelOverride = 5;
-const int yawChannelOverride = 0;
-const int armedChannelOverride = 2;
-const int modeChannelOverride = 1;
+#define RC_CHANNEL_ROLL 0
+#define RC_CHANNEL_PITCH 1
+#define RC_CHANNEL_THROTTLE 2
+#define RC_CHANNEL_YAW 3
+#define RC_CHANNEL_ARMED 5
+#define RC_CHANNEL_MODE 4

 SDL_Joystick *joystick;
+bool joystickInitialized = false, warnShown = false;

 void normalizeRC();

-bool joystickInit() {
-	static bool joystickInitialized = false;
-	static bool warnShown = false;
-	if (joystickInitialized) return true;
-
+void joystickInit() {
 	SDL_Init(SDL_INIT_JOYSTICK);
 	joystick = SDL_JoystickOpen(0);
 	if (joystick != NULL) {
@@ -38,28 +34,23 @@ bool joystickInit() {
 		warnShown = true;
 	}

-	// apply overrides
-	extern int channelZero[16];
-	extern int channelMax[16];
-	memcpy(channelZero, channelZeroOverride, sizeof(channelZeroOverride));
+	// apply calibration overrides
+	extern int channelNeutral[RC_CHANNELS];
+	extern int channelMax[RC_CHANNELS];
+	memcpy(channelNeutral, channelNeutralOverride, sizeof(channelNeutralOverride));
 	memcpy(channelMax, channelMaxOverride, sizeof(channelMaxOverride));
-
-	extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
-	rollChannel = rollChannelOverride;
-	pitchChannel = pitchChannelOverride;
-	throttleChannel = throttleChannelOverride;
-	yawChannel = yawChannelOverride;
-	armedChannel = armedChannelOverride;
-	modeChannel = modeChannelOverride;
-
-	return joystickInitialized;
 }

-bool joystickGet(int16_t ch[16]) {
+bool joystickGet() {
+	if (!joystickInitialized) {
+		joystickInit();
+		return false;
+	}
+
 	SDL_JoystickUpdate();

-	for (uint8_t i = 0; i < 16; i++) {
-		ch[i] = SDL_JoystickGetAxis(joystick, i);
+	for (uint8_t i = 0; i < 8; i++) {
+		channels[i] = SDL_JoystickGetAxis(joystick, i);
 	}
 	return true;
 }
--- a/gazebo/models/flix/flix.sdf
+++ b/gazebo/models/flix/flix.sdf
@@ -1,7 +1,6 @@
 <?xml version="1.0"?>
 <sdf version="1.5">
 	<model name="flix">
-		<plugin name="flix" filename="libflix.so"/>
 		<link name="body">
 			<inertial>
 				<mass>0.065</mass>
@@ -24,14 +23,38 @@
 				<update_rate>1000</update_rate>
 				<imu>
 					<angular_velocity>
-						<x><noise type="gaussian"><stddev>0.00174533</stddev></noise></x><!-- 0.1 degrees per second -->
-						<y><noise type="gaussian"><stddev>0.00174533</stddev></noise></y>
-						<z><noise type="gaussian"><stddev>0.00174533</stddev></noise></z>
+						<x>
+							<noise type="gaussian">
+								<stddev>0.00174533</stddev><!-- 0.1 degrees per second -->
+							</noise>
+						</x>
+						<y>
+							<noise type="gaussian">
+								<stddev>0.00174533</stddev>
+							</noise>
+						</y>
+						<z>
+							<noise type="gaussian">
+								<stddev>0.00174533</stddev>
+							</noise>
+						</z>
 					</angular_velocity>
 					<linear_acceleration>
-						<x><noise type="gaussian"><stddev>0.0784</stddev></noise></x><!-- 8 mg -->
-						<y><noise type="gaussian"><stddev>0.0784</stddev></noise></y>
-						<z><noise type="gaussian"><stddev>0.0784</stddev></noise></z>
+						<x>
+							<noise type="gaussian">
+								<stddev>0.0784</stddev><!-- 8 mg -->
+							</noise>
+						</x>
+						<y>
+							<noise type="gaussian">
+								<stddev>0.0784</stddev>
+							</noise>
+						</y>
+						<z>
+							<noise type="gaussian">
+								<stddev>0.0784</stddev>
+							</noise>
+						</z>
 					</linear_acceleration>
 				</imu>
 			</sensor>
@@ -67,5 +90,6 @@
 				<material><ambient>1 1 1 0.5</ambient><diffuse>1 1 1 0.5</diffuse></material>
 			</visual>
 		</link>
+		<plugin name="flix" filename="libflix.so"/>
 	</model>
 </sdf>
--- a/gazebo/simulator.cpp
+++ b/gazebo/simulator.cpp
@@ -17,9 +17,9 @@

 #include "Arduino.h"
 #include "flix.h"
+#include "util.ino"
 #include "rc.ino"
 #include "time.ino"
-#include "motors.ino"
 #include "estimate.ino"
 #include "control.ino"
 #include "log.ino"
@@ -70,8 +70,8 @@ public:

 		// read rc
 		readRC();
-		controlMode = 1; // 0 acro, 1 stab
-		controlArmed = 1; // armed
+		controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
+		controls[RC_CHANNEL_ARMED] = 1; // armed

 		estimate();

@@ -79,7 +79,7 @@ public:
 		attitude.setYaw(this->model->WorldPose().Yaw());

 		control();
-		handleInput();
+		parseInput();
 		processMavlink();

 		applyMotorForces();
--- a/gazebo/soc/rtc_cntl_reg.h
+++ b/gazebo/soc/rtc_cntl_reg.h
@@ -1 +1 @@
-// Dummy file to make it possible to compile simulator with Flix' util.h
+// Dummy file to make it possible to compile simulator with util.ino
--- a/gazebo/soc/soc.h
+++ b/gazebo/soc/soc.h
@@ -1,4 +1,3 @@
-// Dummy file to make it possible to compile simulator with Flix' util.h
+// Dummy file to make it possible to compile simulator with util.ino

 #define WRITE_PERI_REG(addr, val) {}
-#define REG_CLR_BIT(_r, _b) {}
--- a/gazebo/wifi.h
+++ b/gazebo/wifi.h
@@ -11,8 +11,8 @@
 #include <sys/poll.h>
 #include <gazebo/gazebo.hh>

-#define WIFI_UDP_PORT 14580
-#define WIFI_UDP_REMOTE_PORT 14550
+#define WIFI_UDP_PORT_LOCAL 14580
+#define WIFI_UDP_PORT_REMOTE 14550

 int wifiSocket;

@@ -21,11 +21,11 @@ void setupWiFi() {
 	sockaddr_in addr; // local address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_ANY;
-	addr.sin_port = htons(WIFI_UDP_PORT);
+	addr.sin_port = htons(WIFI_UDP_PORT_LOCAL);
 	bind(wifiSocket, (sockaddr *)&addr, sizeof(addr));
 	int broadcast = 1;
 	setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
-	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
+	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT_LOCAL << " (remote port " << WIFI_UDP_PORT_REMOTE << ")" << std::endl;
 }

 void sendWiFi(const uint8_t *buf, int len) {
@@ -33,7 +33,7 @@ void sendWiFi(const uint8_t *buf, int len) {
 	sockaddr_in addr; // remote address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_BROADCAST; // send UDP broadcast
-	addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
+	addr.sin_port = htons(WIFI_UDP_PORT_REMOTE);
 	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
 }
Author	SHA1	Message	Date
Oleg Kalachev	00286c3b67	Try to build the sim on the latest macos	2024-12-10 09:36:36 +03:00
Oleg Kalachev	09466b1d61	Remove verbosity and debug	2024-12-10 09:36:19 +03:00
Oleg Kalachev	d46438baaa	Merge branch 'master' into fix-macos-ci	2024-12-10 06:15:23 +03:00
Oleg Kalachev	6e7aa78680	Try different version of new Gazebo to install on macOS	2024-12-10 06:02:09 +03:00
Oleg Kalachev	0c59188c6c	Try to debug and fix macos simulation build failure	2024-11-23 19:32:43 +03:00
Oleg Kalachev	bb6d5aa2f0	Use macos-14 to build simulator	2024-11-23 18:57:38 +03:00