Calibrate gyro continuously when landed and stationary

.github/workflows/build.yml

@@ -5,7 +5,6 @@ on:
     branches: [ '*' ]
   pull_request:
     branches: [ master ]
-  workflow_dispatch:
 
 jobs:
   build_linux:
@@ -16,8 +15,6 @@ 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
 
@@ -46,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
@@ -57,49 +54,28 @@ 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_docker:
-    runs-on: ubuntu-latest
-    container:
-      image: ubuntu:20.04
-    steps:
-    - 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
-    - name: Install SDL2
-      run: apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install build-essential libsdl2-dev -y
-    - name: Build simulator
-      run: make build_simulator
-    - uses: actions/upload-artifact@v4
-      with:
-        name: gazebo-plugin-binary
-        path: gazebo/build/*.so
-        retention-days: 1
-
-  build_simulator_macos:
-    runs-on: macos-latest
-    if: github.event_name == 'workflow_dispatch'
-    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-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

.vscode/c_cpp_properties.json

@@ -5,18 +5,18 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/libraries/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/**",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/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.1.0/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.1.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,10 +28,11 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
+				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino",
 				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2405/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -52,18 +53,18 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/libraries/**",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/include/**",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/dio_qspi/include",
+				"~/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.1.0/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.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",
@@ -75,10 +76,11 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
+				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino",
 				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2405/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": [
@@ -100,17 +102,17 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/libraries/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/**",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/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.1.0/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.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",
@@ -122,10 +124,11 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
+				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino",
 				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2405/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": [

.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"
 	],

Makefile

@@ -13,10 +13,10 @@ monitor:
 
 dependencies .dependencies:
 	arduino-cli core update-index --config-file arduino-cli.yaml
-	arduino-cli core install esp32:esp32@3.1.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

README.md

@@ -4,11 +4,11 @@
 
 <table>
   <tr>
-    <td align=center><strong>Version 1.1</strong> (3D-printed frame)</td>
+    <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.1.jpg" width=500 alt="Flix quadcopter"></td>
+    <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>
@@ -32,17 +32,13 @@
 
 ## It actually flies
 
-See detailed demo video: https://youtu.be/hT46CZ1CgC4.
-
-<a href="https://youtu.be/hT46CZ1CgC4"><img width=500 src="https://i3.ytimg.com/vi/hT46CZ1CgC4/maxresdefault.jpg"></a>
-
-Version 0 demo video: https://youtu.be/8GzzIQ3C6DQ.
+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>
 
-See the [user builds gallery](docs/user.md).
+Version 1 test flight: https://t.me/opensourcequadcopter/42.
 
-<a href="docs/user.md"><img src="docs/img/user/user.jpg" width=400></a>
+<a href="https://t.me/opensourcequadcopter/42"><img width=500 src="docs/img/flight-video.jpg"></a>
 
 ## Simulation
 
@@ -57,20 +53,19 @@ See [instructions on running the simulation](docs/build.md).
 |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), 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).<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|
+|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|
-|Battery connector cable|MX2.0 2P female|<img src="docs/img/mx.png" width=100>|1|
 |Li-Po Battery charger|Any|<img src="docs/img/charger.jpg" width=100>|1|
 |Screws for IMU board mounting|M3x5|<img src="docs/img/screw-m3.jpg" width=100>|2|
 |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-1.1.stl`](docs/assets/flix-frame-1.1.stl) [`flix-frame-1.1.step`](docs/assets/flix-frame-1.1.step)|<img src="docs/img/frame1.jpg" width=100>|1|
+|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>|2|
-|*RC transmitter (optional)*|*KINGKONG TINY X8 (warning: lacks USB support) or other⁵*|<img src="docs/img/tx.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||||
@@ -100,9 +95,7 @@ Motor connection scheme:
 
 <img src="docs/img/mosfet-connection.png" height=400 alt="MOSFET connection scheme">
 
-You can see a user-contributed [variant of complete circuit diagram](https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612338222067&cot=14) of the drone.
-
-See [assembly guide](docs/assembly.md) for instructions on assembling the drone.
+Complete diagram is Work-in-Progress.
 
 ### Notes
 
@@ -123,10 +116,10 @@ See [assembly guide](docs/assembly.md) for instructions on assembling the drone.
 
   |Motor|Position|Direction|Wires|GPIO|
   |-|-|-|-|-|
-  |Motor 0|Rear left|Counter-clockwise|Black & White|GPIO12 (*TDI*)|
-  |Motor 1|Rear right|Clockwise|Blue & Red|GPIO13 (*TCK*)|
-  |Motor 2|Front right|Counter-clockwise|Black & White|GPIO14 (*TMS*)|
-  |Motor 3|Front left|Clockwise|Blue & Red|GPIO15 (*TD0*)|
+  |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.
 
@@ -135,8 +128,8 @@ See [assembly guide](docs/assembly.md) for instructions on assembling the drone.
   |Receiver pin|ESP32 pin|
   |-|-|
   |GND|GND|
-  |VIN|VCC (or 3.3V depending on the receiver)|
-  |Signal (TX)|GPIO4⁶|
+  |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.*
 

docs/assembly.md

@@ -1,29 +0,0 @@
-# Brief assembly guide
-
-Soldered components ([schematics variant](https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612338222067&cot=14)):
-
-<img src="img/assembly/1.jpg" width=600>
-
-<br>Use double-sided tape to attach ESP32 to the top frame part (ESP32 holder):
-
-<img src="img/assembly/2.jpg" width=600>
-
-<br>Use two washers to screw the IMU board to the frame:
-
-<img src="img/assembly/3.jpg" width=600>
-
-<br>Screw the IMU with M3x5 screws as shown:
-
-<img src="img/assembly/4.jpg" width=600>
-
-<br>Install the motors, attach MOSFETs to the frame using tape:
-
-<img src="img/assembly/5.jpg" width=600>
-
-<br>Screw the ESP32 holder with M1.4x5 screws to the frame:
-
-<img src="img/assembly/6.jpg" width=600>
-
-<br>Assembled drone:
-
-<img src="img/assembly/7.jpg" width=600>

docs/book/README.md

@@ -3,7 +3,7 @@
 > [!IMPORTANT]
 > Flix — это проект по созданию открытого квадрокоптера на базе ESP32 с нуля и учебника по разработке полетных контроллеров.
 
-<img src="img/flix1.1.jpg" class="border" width=500 alt="Flix quadcopter">
+<img src="img/flix1.jpg" class="border" width=500 alt="Flix quadcopter">
 
 <p class="github">GitHub:&nbsp;<a href="https://github.com/okalachev/flix">github.com/okalachev/flix</a>.</p>
 

docs/book/SUMMARY.md

@@ -12,7 +12,7 @@
 * [Моторы]()
 * [Радиоуправление]()
 * [Гироскоп](gyro.md)
-* [Акселерометр]()
+* [Акселерометр]()s
 * [Оценка состояния]()
 * [PID-регулятор]()
 * [Режим ACRO]()

docs/book/gyro.md

@@ -139,7 +139,7 @@ void loop() {
 
 ### Частота сэмплов
 
-Большинство IMU могут обновлять данные с разной частотой. В полетных контроллерах обычно используется частота обновления от 500 Гц до 8 кГц. Чем выше частота сэмплов, тем выше точность управления полетом, но и больше нагрузка на микроконтроллер.
+Большинство IMU могут обновлять данные с разной частотой. В полетных контроллерах обычно используется частота обновления от 500 Гц до 8 кГц. Чем выше частота сэмплов, тем выше точность управления полетом, но и больше нагрузка на микроконтроллер. В Flix используется частота сэмплов 1 кГц.
 
 Частота сэмплов устанавливается методом `setSampleRate()`. В Flix используется частота 1 кГц:
 

docs/build.md

@@ -84,7 +84,7 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 20.04. If you h
 
 #### 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).
+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. 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.
@@ -105,26 +105,17 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 20.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.1.0 (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.
-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)
 
 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:
 
@@ -146,21 +137,18 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 20.04. If you h
 
 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).
+1. Open Serial Monitor in Arduino IDE (use 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`).
+3. Connect your smartphone to the appeared `flix` Wi-Fi network.
 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!
@@ -169,33 +157,10 @@ Before flight you need to calibrate the accelerometer:
 
 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).
+1. Open Serial Monitor in Arduino IDE (use 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">
+Then you can use your remote control to fly the drone!
 
 > [!NOTE]
 > If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article.

docs/firmware.md

@@ -6,7 +6,7 @@
 
 The main loop is running at 1000 Hz. All the dataflow is happening 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>*.

docs/troubleshooting.md

@@ -14,16 +14,14 @@ 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. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
+* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands.
 * **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**.
-* **Check the IMU is working**. Perform `imu` command and check its output:
-  * The `status` field should be `OK`.
-  * The `rate` field should be about 1000 (Hz).
-  * The `accel` and `gyro` fields should change as you move the drone.
+* **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. Type `ca` command in Serial Monitor and follow the instructions.
 * **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct.
 * **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, make sure `rotateIMU` function is defined correctly in `imu.ino` file.
-* **Check the motors type**. Motors with exact 3.7V voltage are needed, not ranged working voltage (3.7V — 6V).
 * **Check the motors**. Perform the following commands using Serial Monitor:
   * `mfr` — should rotate front right motor (counter-clockwise).
   * `mfl` — should rotate front left motor (clockwise).

docs/user.md

@@ -1,97 +0,0 @@
-# Hall of fame
-
-This page contains user-built drones based on the Flix project. Publish your projects into the official Telegram-chat: [@opensourcequadcopterchat](https://t.me/opensourcequadcopterchat) or send materials as a pull request.
-
----
-
-Author: [@cryptokobans](https://t.me/cryptokobans).<br>
-Features: ESP32-C3 SuperMini board, INA226 power monitor, IRLZ44N MOSFETs, MPU-6500 IMU.
-
-**Flight video:**
-
-<a href="https://drive.google.com/file/d/1-4ciDsj8slTEaxxRl1-QAkx0cUDkb8iy/view?usp=sharing"><img height=300 src="img/user/cryptokobans/video.jpg"></a>
-
-<img src="img/user/cryptokobans/1.jpg" height=150> <img src="img/user/cryptokobans/2.jpg" height=150>
-
----
-
-Author: [@jeka_chex](https://t.me/jeka_chex).<br>
-Features: custom frame, FPV camera, 3-blade 31 mm propellers.<br>
-Motor drivers: AON7410 MOSFET + capacitors.<br>
-Custom frame files: https://drive.google.com/drive/folders/1QCIc-_YYFxJN4cMhVLjL5SflqegvCowm?usp=share_link.<br>
-
-**Flight video:**
-
-<a href="https://drive.google.com/file/d/1VnWI5YVPojfqsfpyLX4v2V9zHi9adwcd/view?usp=sharing"><img height=300 src="img/user/jeka_chex/video.jpg"></a>
-
-**FPV flight video:**
-
-<a href="https://drive.google.com/file/d/1RSU6VWs9omsge4hGloH5NQqnxvLyhMKB/view?usp=sharing"><img height=300 src="img/user/jeka_chex/video-fpv.jpg"></a>
-
-<img src="img/user/jeka_chex/1.jpg" height=150> <img src="img/user/jeka_chex/2.jpg" height=150> <img src="img/user/jeka_chex/3.jpg" height=150> <img src="img/user/jeka_chex/4.jpg" height=150> <img src="img/user/jeka_chex/5.jpg" height=150>
-
----
-
-Author: [@fisheyeu](https://t.me/fisheyeu).<br>
-[Video](https://drive.google.com/file/d/1IT4eMmWPZpmaZR_qsIRmNJ52hYkFB_0q/view?usp=share_link).
-
-<img src="img/user/fisheyeu/1.jpg" height=300> <img src="img/user/fisheyeu/2.jpg" height=300>
-
----
-
-Author: [@p_kabakov](https://t.me/p_kabakov).<br>
-Custom propellers guard 3D-model: https://drive.google.com/file/d/1TKnzwvrZYzYuRTLLERNmnKH71H9n4Xj_/view?usp=share_link.<br>
-Features: ESP32-C3 microcontroller is used.<br>
-[Video](https://drive.google.com/file/d/1B0NMcsk0fgwUgNr9XuLOdR2yYCuaj008/view?usp=share_link).
-
-<img src="img/user/p_kabakov/1.jpg" width=150> <img src="img/user/p_kabakov/2.jpg" width=150> <img src="img/user/p_kabakov/3.jpg" width=150>
-
-**Custom Wi-Fi RC control:**
-
-<a href="https://github.com/pavelkabakov/flix/blob/master/rc_control_v1/IMG_20250221_195756.jpg"><img height=300 src="img/user/p_kabakov/wifirc.jpg"></a>
-
-See source and description (in Russian): https://github.com/pavelkabakov/flix/tree/master/rc_control_v1.
-
----
-
-Author: [@yi_lun](https://t.me/yi_lun).<br>
-[Video](https://drive.google.com/file/d/1TkSuvHQ_0qQPFUpY5XjJzmhnpX_07cTg/view?usp=share_link).
-
-<img src="img/user/yi_lun/1.jpg" width=300> <img src="img/user/yi_lun/2.jpg" width=300>
-
----
-
-Author: [@peter_ukhov](https://t.me/peter_ukhov).<br>
-Features: customized ESP32 holder, GY-ICM20948V2 IMU board, boost-converter for powering the ESP32.<br>
-Files for 3D-printing: https://drive.google.com/file/d/1Sma-FEzFBj2HA5ixJtUyH0uWixvr6vdK/view?usp=share_link.<br>
-Schematics: https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612179508274&cot=14.<br>
-
-<a href="https://www.youtube.com/watch?v=wi4w_hOmKcQ"><img width=500 src="img/user/peter_ukhov-2/video.jpg"></a>
-
-<img src="img/user/peter_ukhov-2/1.jpg" width=300> <img src="img/user/peter_ukhov-2/2.jpg" width=300>
-
----
-
-Author: [@Alexey_Karakash](https://t.me/Alexey_Karakash).<br>
-Files for 3D printing of the custom frame: https://drive.google.com/file/d/1tkNmujrHrKpTMVtsRH3mor2zdAM0JHum/view?usp=share_link.<br>
-
-<a href="https://t.me/opensourcequadcopter/61"><img width=500 src="img/user/alexey_karakash/video.jpg"></a>
-
-<img src="img/user/alexey_karakash/1.jpg" height=150> <img src="img/user/alexey_karakash/2.jpg" height=150> <img src="img/user/alexey_karakash/3.jpg" height=150> <img src="img/user/alexey_karakash/4.jpg" height=150> <img src="img/user/alexey_karakash/5.jpg" height=150>
-
----
-
-Author: [@rudpa](https://t.me/rudpa).<br>
-
-<a href="https://t.me/opensourcequadcopter/46"><img width=500 src="img/user/rudpa/video.jpg"></a>
-
-<img src="img/user/rudpa/1.jpg" height=150> <img src="img/user/rudpa/2.jpg" height=150> <img src="img/user/rudpa/3.jpg" height=150>
-
----
-
-Author: [@peter_ukhov](https://t.me/peter_ukhov).<br>
-Schematics: https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612338222067&cot=14.<br>
-
-<a href="https://t.me/opensourcequadcopter/24"><img width=500 src="img/user/peter_ukhov/video.jpg"></a>
-
-<img src="img/user/peter_ukhov/1.jpg" height=150> <img src="img/user/peter_ukhov/2.jpg" height=150> <img src="img/user/peter_ukhov/3.jpg" height=150>

docs/version0.md

@@ -27,4 +27,4 @@ Flix version 0 (obsolete):
 
 <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=/?moveToWidget=3458764574482511443&cot=14) of the drone.
+You can also check a user contributed [variant of complete circuit diagram](https://miro.com/app/board/uXjVN-dTjoo=/) of the drone.

flix/cli.ino

@@ -5,12 +5,9 @@
 
 #include "pid.h"
 #include "vector.h"
-#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 int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
+extern LowPassFilter<Vector> ratesFilter;
 
 const char* motd =
 "\nWelcome to\n"
@@ -26,7 +23,6 @@ const char* motd =
 "p <name> - show parameter\n"
 "p <name> <value> - set parameter\n"
 "preset - reset parameters\n"
-"time - show time info\n"
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
@@ -34,106 +30,66 @@ const char* motd =
 "mot - show motor output\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"
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
 
-void print(const char* format, ...) {
-	char buf[1000];
-	va_list args;
-	va_start(args, format);
-	vsnprintf(buf, sizeof(buf), format, args);
-	va_end(args);
-	Serial.print(buf);
-#if WIFI_ENABLED
-	mavlinkPrint(buf);
-#endif
-}
-
-void pause(float duration) {
-#if ARDUINO
-	double start = t;
-	while (t - start < duration) {
-		step();
-		handleInput();
-#if WIFI_ENABLED
-		processMavlink();
-#endif
-	}
-#else
-	// Code above won't work in the simulation
-	delay(duration * 1000);
-#endif
-}
-
-void doCommand(String str, bool echo = false) {
-	// parse command
-	String command, arg0, arg1;
-	splitString(str, command, arg0, arg1);
-
-	// echo command
-	if (echo && !command.isEmpty()) {
-		print("> %s\n", str.c_str());
-	}
-
-	// execute command
+void doCommand(String& command, String& arg0, String& arg1) {
 	if (command == "help" || command == "motd") {
-		print("%s\n", motd);
+		Serial.println(motd);
 	} else if (command == "p" && arg0 == "") {
 		printParameters();
 	} else if (command == "p" && arg0 != "" && arg1 == "") {
-		print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
+		Serial.printf("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
 	} else if (command == "p") {
 		bool success = setParameter(arg0.c_str(), arg1.toFloat());
 		if (success) {
-			print("%s = %g\n", arg0.c_str(), arg1.toFloat());
+			Serial.printf("%s = %g\n", arg0.c_str(), arg1.toFloat());
 		} else {
-			print("Parameter not found: %s\n", arg0.c_str());
+			Serial.printf("Parameter not found: %s\n", arg0.c_str());
 		}
 	} else if (command == "preset") {
 		resetParameters();
-	} else if (command == "time") {
-		print("Time: %f\n", t);
-		print("Loop rate: %f\n", loopRate);
-		print("dt: %f\n", dt);
 	} else if (command == "ps") {
 		Vector a = attitude.toEulerZYX();
-		print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
+		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") {
-		print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
+		Serial.printf("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
 	} 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);
+		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);
 		printIMUCal();
-		print("rate: %f\n", loopRate);
-		print("landed: %d\n", landed);
+		Serial.printf("rate: %f\n", loopRate);
 	} else if (command == "rc") {
-		print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
-			channels[throttleChannel], channels[yawChannel], channels[pitchChannel],
-			channels[rollChannel], channels[armedChannel], channels[modeChannel]);
-		print("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",
-			controls[throttleChannel], controls[yawChannel], controls[pitchChannel],
-			controls[rollChannel], controls[armedChannel], controls[modeChannel]);
-		print("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") {
-		print("Motors: front-right %g front-left %g rear-right %g rear-left %g\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();
 	} else if (command == "cr") {
 		calibrateRC();
+	} else if (command == "cg") {
+		calibrateGyro();
 	} 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 == "reboot") {
@@ -141,26 +97,48 @@ void doCommand(String str, bool echo = false) {
 	} else if (command == "") {
 		// do nothing
 	} else {
-		print("Invalid command: %s\n", command.c_str());
+		Serial.println("Invalid command: " + command);
 	}
 }
 
-void handleInput() {
+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;
 
 	if (showMotd) {
-		print("%s\n", motd);
+		Serial.println(motd);
 		showMotd = false;
 	}
 
 	while (Serial.available()) {
 		char c = Serial.read();
 		if (c == '\n') {
-			doCommand(input);
+			char chars[input.length() + 1];
+			input.toCharArray(chars, input.length() + 1);
+			String command = stringToken(chars, " ");
+			String arg0 = stringToken(NULL, " ");
+			String arg1 = stringToken(NULL, "");
+			doCommand(command, arg0, arg1);
 			input.clear();
 		} else {
 			input += c;
 		}
 	}
 }
+
+// Helper function for parsing input
+String stringToken(char* str, const char* delim) {
+	char* token = strtok(str, delim);
+	return token == NULL ? "" : token;
+}

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
 
@@ -45,17 +44,12 @@ PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
 PID rollPID(ROLL_P, ROLL_I, ROLL_D);
 PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
 PID yawPID(YAW_P, 0, 0);
-Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
-float tiltMax = TILT_MAX;
 
 Quaternion attitudeTarget;
 Vector ratesTarget;
 Vector torqueTarget;
 float thrustTarget;
 
-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
-
 void control() {
 	interpretRC();
 	failsafe();
@@ -72,39 +66,38 @@ void control() {
 }
 
 void interpretRC() {
-	armed = controls[throttleChannel] >= 0.05 &&
-		(controls[armedChannel] >= 0.5 || isnan(controls[armedChannel])); // assume armed if armed channel is not defined
+	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 (controls[modeChannel] < 0.25) {
+	if (controls[RC_CHANNEL_MODE] < 0.25) {
 		mode = STAB;
-	} else if (controls[modeChannel] < 0.75) {
+	} else if (controls[RC_CHANNEL_MODE] < 0.75) {
 		mode = STAB;
 	} else {
 		mode = STAB;
 	}
 
-	thrustTarget = controls[throttleChannel];
+	thrustTarget = controls[RC_CHANNEL_THROTTLE];
 
 	if (mode == ACRO) {
 		yawMode = YAW_RATE;
-		ratesTarget.x = controls[rollChannel] * maxRate.x;
-		ratesTarget.y = controls[pitchChannel] * maxRate.y;
-		ratesTarget.z = -controls[yawChannel] * maxRate.z; // 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 = controls[yawChannel] == 0 ? YAW : YAW_RATE;
+		yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
 
 		attitudeTarget = Quaternion::fromEulerZYX(Vector(
-			controls[rollChannel] * tiltMax,
-			controls[pitchChannel] * tiltMax,
+			controls[RC_CHANNEL_ROLL] * MAX_TILT,
+			controls[RC_CHANNEL_PITCH] * MAX_TILT,
 			attitudeTarget.getYaw()));
-		ratesTarget.z = -controls[yawChannel] * maxRate.z; // 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(controls[rollChannel], controls[pitchChannel], -controls[yawChannel]) * 0.01;
+		torqueTarget = Vector(controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_PITCH], -controls[RC_CHANNEL_YAW]) * 0.01;
 	}
 
 	if (yawMode == YAW_RATE || !motorsActive()) {
@@ -122,8 +115,8 @@ void controlAttitude() {
 	}
 
 	const Vector up(0, 0, 1);
-	Vector upActual = attitude.rotateVector(up);
-	Vector upTarget = attitudeTarget.rotateVector(up);
+	Vector upActual = attitude.rotate(up);
+	Vector upTarget = attitudeTarget.rotate(up);
 
 	Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);
 
@@ -169,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";

flix/estimate.ino

@@ -6,9 +6,8 @@
 #include "quaternion.h"
 #include "vector.h"
 #include "lpf.h"
-#include "util.h"
 
-#define WEIGHT_ACC 0.003
+#define WEIGHT_ACC 0.5f
 #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
 LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
@@ -23,20 +22,23 @@ void applyGyro() {
 	rates = ratesFilter.update(gyro);
 
 	// apply rates to attitude
-	attitude = attitude.rotate(Quaternion::fromAngularRates(rates * dt));
+	attitude *= Quaternion::fromAngularRates(rates * dt);
+	attitude.normalize();
 }
 
 void applyAcc() {
 	// test should we apply accelerometer gravity correction
 	float accNorm = acc.norm();
-	landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
+	bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
 
+	setLED(landed);
 	if (!landed) return;
 
 	// calculate accelerometer correction
-	Vector up = attitude.rotateVector(Vector(0, 0, 1));
-	Vector correction = Vector::angularRatesBetweenVectors(acc, up) * WEIGHT_ACC;
+	Vector up = attitude.rotate(Vector(0, 0, 1));
+	Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;
 
 	// apply correction
-	attitude = attitude.rotate(Quaternion::fromAngularRates(correction));
+	attitude *= Quaternion::fromAngularRates(correction);
+	attitude.normalize();
 }

flix/failsafe.ino

@@ -1,41 +1,23 @@
 // Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Fail-safe functions
+// Fail-safe for RC loss
 
 #define RC_LOSS_TIMEOUT 0.2
 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero
 
-extern double controlsTime;
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel;
-
 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 (controlsTime > 0 && controls[throttleChannel] < 0.05) zeroThrottleTime = controlsTime;
-	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 - controlsTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }
 
-// Smooth descend on RC lost
 void descend() {
+	// Smooth descend on RC lost
 	mode = STAB;
-	controls[rollChannel] = 0;
-	controls[pitchChannel] = 0;
-	controls[yawChannel] = 0;
-	controls[throttleChannel] -= dt / DESCEND_TIME;
-	if (controls[throttleChannel] < 0) controls[throttleChannel] = 0;
+	controls[RC_CHANNEL_ROLL] = 0;
+	controls[RC_CHANNEL_PITCH] = 0;
+	controls[RC_CHANNEL_YAW] = 0;
+	controls[RC_CHANNEL_THROTTLE] -= dt / DESCEND_TIME;
+	if (controls[RC_CHANNEL_THROTTLE] < 0) controls[RC_CHANNEL_THROTTLE] = 0;
 }

flix/flix.ino

@@ -5,15 +5,32 @@
 
 #include "vector.h"
 #include "quaternion.h"
-#include "util.h"
 
 #define SERIAL_BAUDRATE 115200
+
 #define WIFI_ENABLED 1
 
-double t = NAN; // current step time, s
+#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
+
+#define ONE_G 9.80665
+
+float t = NAN; // current step time, s
 float dt; // time delta from previous step, s
-int16_t channels[16]; // raw rc channels
-float controls[16]; // normalized controls in range [-1..1] ([0..1] for throttle)
+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
@@ -23,19 +40,20 @@ float motors[4]; // normalized motors thrust in range [-1..1]
 
 void setup() {
 	Serial.begin(SERIAL_BAUDRATE);
-	print("Initializing flix");
+	Serial.println("Initializing flix");
 	disableBrownOut();
 	setupParameters();
 	setupLED();
 	setupMotors();
 	setLED(true);
-#if WIFI_ENABLED
+#if WIFI_ENABLED == 1
 	setupWiFi();
 #endif
 	setupIMU();
 	setupRC();
+
 	setLED(false);
-	print("Initializing complete");
+	Serial.println("Initializing complete");
 }
 
 void loop() {
@@ -45,10 +63,10 @@ void loop() {
 	estimate();
 	control();
 	sendMotors();
-	handleInput();
-#if WIFI_ENABLED
+	parseInput();
+#if WIFI_ENABLED == 1
 	processMavlink();
 #endif
 	logData();
-	syncParameters();
+	flushParameters();
 }

flix/imu.ino

@@ -5,8 +5,6 @@
 
 #include <SPI.h>
 #include <MPU9250.h>
-#include "lpf.h"
-#include "util.h"
 
 MPU9250 IMU(SPI);
 
@@ -15,9 +13,16 @@ Vector gyroBias;
 Vector accScale(1, 1, 1);
 
 void setupIMU() {
-	print("Setup IMU\n");
-	IMU.begin();
+	Serial.println("Setup IMU");
+	bool status = IMU.begin();
+	if (!status) {
+		while (true) {
+			Serial.println("IMU begin error");
+			delay(1000);
+		}
+	}
 	configureIMU();
+	// calibrateGyro();
 }
 
 void configureIMU() {
@@ -48,39 +53,48 @@ void rotateIMU(Vector& data) {
 }
 
 void calibrateGyroOnce() {
-	static float landedTime = 0;
-	landedTime = landed ? landedTime + dt : 0;
-	if (landedTime < 2) return; // calibrate only if definitely stationary
+	if (!landed) return;
+	static float samples = 0; // overflows after 49 days at 1000 Hz
+	samples++;
+	gyroBias = gyroBias + (gyro - gyroBias) / samples; // running average
+}
 
-	static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
-	gyroBias = gyroCalibrationFilter.update(gyro);
+void calibrateGyro() {
+	const int samples = 1000;
+	Serial.println("Calibrating gyro, stand still");
+	IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode
+
+	gyroBias = Vector(0, 0, 0);
+	for (int i = 0; i < samples; i++) {
+		IMU.waitForData();
+		IMU.getGyro(gyro.x, gyro.y, gyro.z);
+		gyroBias = gyroBias + gyro;
+	}
+	gyroBias = gyroBias / samples;
+
+	printIMUCal();
+	configureIMU();
 }
 
 void calibrateAccel() {
-	print("Calibrating accelerometer\n");
+	Serial.println("Calibrating accelerometer");
 	IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode
 
-	print("Place level [8 sec]\n");
-	pause(8);
+	Serial.setTimeout(60000);
+	Serial.print("Place level [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	print("Place nose up [8 sec]\n");
-	pause(8);
+	Serial.print("Place nose up [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	print("Place nose down [8 sec]\n");
-	pause(8);
+	Serial.print("Place nose down [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	print("Place on right side [8 sec]\n");
-	pause(8);
+	Serial.print("Place on right side [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	print("Place on left side [8 sec]\n");
-	pause(8);
+	Serial.print("Place on left side [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	print("Place upside down [8 sec]\n");
-	pause(8);
+	Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n');
 	calibrateAccelOnce();
 
 	printIMUCal();
-	print("✓ Calibration done!\n");
 	configureIMU();
 }
 
@@ -106,19 +120,21 @@ 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 printIMUCal() {
-	print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
-	print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
-	print("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);
+	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() {
-	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());
+	Serial.printf("model: %s\n", IMU.getModel());
+	Serial.printf("who am I: 0x%02X\n", IMU.whoAmI());
 }

flix/log.ino

@@ -3,60 +3,36 @@
 
 // In-RAM logging
 
-#include "vector.h"
-
 #define LOG_RATE 100
 #define LOG_DURATION 10
 #define LOG_PERIOD 1.0 / LOG_RATE
 #define LOG_SIZE LOG_DURATION * LOG_RATE
+#define LOG_COLUMNS 14
 
-float tFloat;
-Vector attitudeEuler;
-Vector attitudeTargetEuler;
-
-struct LogEntry {
-	const char *name;
-	float *value;
-};
-
-LogEntry logEntries[] = {
-	{"t", &tFloat},
-	{"rates.x", &rates.x},
-	{"rates.y", &rates.y},
-	{"rates.z", &rates.z},
-	{"ratesTarget.x", &ratesTarget.x},
-	{"ratesTarget.y", &ratesTarget.y},
-	{"ratesTarget.z", &ratesTarget.z},
-	{"attitude.x", &attitudeEuler.x},
-	{"attitude.y", &attitudeEuler.y},
-	{"attitude.z", &attitudeEuler.z},
-	{"attitudeTarget.x", &attitudeTargetEuler.x},
-	{"attitudeTarget.y", &attitudeTargetEuler.y},
-	{"attitudeTarget.z", &attitudeTargetEuler.z},
-	{"thrustTarget", &thrustTarget}
-};
-
-const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
-float logBuffer[LOG_SIZE][logColumns];
-
-void prepareLogData() {
-	tFloat = t;
-	attitudeEuler = attitude.toEulerZYX();
-	attitudeTargetEuler = attitudeTarget.toEulerZYX();
-}
+float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)
+int logPointer = 0;
 
 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;
 
-	prepareLogData();
-
-	for (int i = 0; i < logColumns; i++) {
-		logBuffer[logPointer][i] = *logEntries[i].value;
-	}
+	logBuffer[logPointer][0] = t;
+	logBuffer[logPointer][1] = rates.x;
+	logBuffer[logPointer][2] = rates.y;
+	logBuffer[logPointer][3] = rates.z;
+	logBuffer[logPointer][4] = ratesTarget.x;
+	logBuffer[logPointer][5] = ratesTarget.y;
+	logBuffer[logPointer][6] = ratesTarget.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++;
 	if (logPointer >= LOG_SIZE) {
@@ -65,15 +41,13 @@ void logData() {
 }
 
 void dumpLog() {
-	// Print header
-	for (int i = 0; i < logColumns; i++) {
-		print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n");
-	}
-	// Print data
+	Serial.printf("t,rates.x,rates.y,rates.z,ratesTarget.x,ratesTarget.y,ratesTarget.z,"
+		"attitude.x,attitude.y,attitude.z,attitudeTarget.x,attitudeTarget.y,attitudeTarget.z,thrustTarget\n");
 	for (int i = 0; i < LOG_SIZE; i++) {
 		if (logBuffer[i][0] == 0) continue; // skip empty records
-		for (int j = 0; j < logColumns; j++) {
-			print("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n");
-		}
+		for (int j = 0; j < LOG_COLUMNS - 1; j++) {
+			Serial.printf("%f,", logBuffer[i][j]);
+		}
+		Serial.printf("%f\n", logBuffer[i][LOG_COLUMNS - 1]);
 	}
 }

flix/mavlink.ino

@@ -3,7 +3,7 @@
 
 // MAVLink communication
 
-#if WIFI_ENABLED
+#if WIFI_ENABLED == 1
 
 #include <MAVLink.h>
 
@@ -13,19 +13,14 @@
 #define MAVLINK_CONTROL_SCALE 0.7f
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
 
-float mavlinkControlScale = 0.7;
-
-extern double controlsTime;
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
-
 void processMavlink() {
 	sendMavlink();
 	receiveMavlink();
 }
 
 void sendMavlink() {
-	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;
@@ -33,26 +28,22 @@ 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);
-
-		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) {
 		lastFast = t;
 
 		const float zeroQuat[] = {0, 0, 0, 0};
-		Quaternion att = fluToFrd(attitude); // MAVLink uses FRD coordinate system
+		Quaternion attitudeFRD = FLU2FRD(attitude); // MAVLink uses FRD coordinate system
 		mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
-			time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat);
+			time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat);
 		sendMessage(&msg);
 
-		mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,
+		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);
@@ -92,29 +83,23 @@ 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 m;
-		mavlink_msg_manual_control_decode(&msg, &m);
-		if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast
-
-		controls[throttleChannel] = m.z / 1000.0f;
-		controls[pitchChannel] = m.x / 1000.0f * mavlinkControlScale;
-		controls[rollChannel] = m.y / 1000.0f * mavlinkControlScale;
-		controls[yawChannel] = m.r / 1000.0f * mavlinkControlScale;
-		controls[modeChannel] = 1; // STAB mode
-		controls[armedChannel] = 1; // armed
+	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 (abs(controls[yawChannel]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[yawChannel] = 0;
+		if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
 	}
 
-	if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
-		mavlink_param_request_list_t m;
-		mavlink_msg_param_request_list_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
-
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
 		mavlink_message_t msg;
 		for (int i = 0; i < parametersCount(); i++) {
 			mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
@@ -123,94 +108,62 @@ void handleMavlink(const void *_msg) {
 		}
 	}
 
-	if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
-		mavlink_param_request_read_t m;
-		mavlink_msg_param_request_read_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
-
-		char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
-		strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
-		float value = strlen(name) == 0 ? getParameter(m.param_index) : getParameter(name);
-		if (m.param_index != -1) {
-			memcpy(name, getParameterName(m.param_index), 16);
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
+		mavlink_param_request_read_t paramRequestRead;
+		mavlink_msg_param_request_read_decode(msg, &paramRequestRead);
+		char name[16 + 1];
+		strlcpy(name, paramRequestRead.param_id, sizeof(name)); // param_id might be not null-terminated
+		float value = strlen(name) == 0 ? getParameter(paramRequestRead.param_index) : getParameter(name);
+		if (paramRequestRead.param_index != -1) {
+			memcpy(name, getParameterName(paramRequestRead.param_index), 16);
 		}
 		mavlink_message_t msg;
 		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
-			name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
+			name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), paramRequestRead.param_index);
 		sendMessage(&msg);
 	}
 
-	if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
-		mavlink_param_set_t m;
-		mavlink_msg_param_set_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
-
-		char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
-		strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
-		setParameter(name, m.param_value);
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+		mavlink_param_set_t paramSet;
+		mavlink_msg_param_set_decode(msg, &paramSet);
+		char name[16 + 1];
+		strlcpy(name, paramSet.param_id, sizeof(name)); // param_id might be not null-terminated
+		setParameter(name, paramSet.param_value);
 		// send ack
 		mavlink_message_t msg;
 		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
-			m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
+			paramSet.param_id, paramSet.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
 		sendMessage(&msg);
 	}
 
-	if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
-		mavlink_mission_request_list_t m;
-		mavlink_msg_mission_request_list_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
-
+	if (msg->msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
 		mavlink_message_t msg;
 		mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
 		sendMessage(&msg);
 	}
 
-	if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
-		mavlink_serial_control_t m;
-		mavlink_msg_serial_control_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
-
-		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
-		strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated
-		doCommand(data, true);
-	}
-
 	// 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;
+	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
+		mavlink_command_long_t commandLong;
+		mavlink_msg_command_long_decode(msg, &commandLong);
 		mavlink_message_t ack;
 		mavlink_message_t response;
 
-		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);
+		if (commandLong.command == MAV_CMD_REQUEST_MESSAGE && commandLong.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
+			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, commandLong.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg->sysid, msg->compid);
 			sendMessage(&ack);
 			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);
+			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, commandLong.command, MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg->sysid, msg->compid);
 			sendMessage(&ack);
 		}
 	}
 }
 
-// Send shell output to GCS
-void mavlinkPrint(const char* str) {
-	// Send data in chunks
-	for (int i = 0; i < strlen(str); i += MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN) {
-		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
-		strlcpy(data, str + i, sizeof(data));
-		mavlink_message_t msg;
-		mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
-			SERIAL_CONTROL_DEV_SHELL, 0, 0, 0, strlen(data), (uint8_t *)data, 0, 0);
-		sendMessage(&msg);
-	}
-}
-
 // Convert Forward-Left-Up to Forward-Right-Down quaternion
-inline Quaternion fluToFrd(const Quaternion &q) {
+inline Quaternion FLU2FRD(const Quaternion &q) {
 	return Quaternion(q.w, q.x, -q.y, -q.z);
 }
 

flix/motors.ino

@@ -2,29 +2,19 @@
 // Repository: https://github.com/okalachev/flix
 
 // Motors output control using MOSFETs
-// In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
-
-#include "util.h"
+// 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 1000
-#define PWM_RESOLUTION 12
-#define PWM_STOP 0
-#define PWM_MIN 0
-#define PWM_MAX 1000000 / PWM_FREQUENCY
-
-// 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() {
-	print("Setup Motors\n");
+	Serial.println("Setup Motors");
 
 	// configure pins
 	ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
@@ -33,35 +23,17 @@ void setupMotors() {
 	ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
 
 	sendMotors();
-	print("Motors initialized\n");
+	Serial.println("Motors initialized");
 }
 
-int getDutyCycle(float value) {
-	value = constrain(value, 0, 1);
-	float pwm = mapff(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);
-	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(uint8_t n) {
-	print("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();
-	pause(3);
-	motors[n] = 0;
-	sendMotors();
-	print("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]));
 }

flix/parameters.ino

@@ -1,13 +1,10 @@
-// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-// Parameters storage in flash memory
+#pragma once
 
 #include <Preferences.h>
+#include <vector>
 
-extern float channelNeutral[16];
-extern float channelMax[16];
-extern float mavlinkControlScale;
+extern float channelNeutral[RC_CHANNELS];
+extern float channelMax[RC_CHANNELS];
 
 Preferences storage;
 
@@ -37,10 +34,6 @@ Parameter parameters[] = {
 	{"PITCH_I", &pitchPID.i},
 	{"PITCH_D", &pitchPID.d},
 	{"YAW_P", &yawPID.p},
-	{"PITCHRATE_MAX", &maxRate.y},
-	{"ROLLRATE_MAX", &maxRate.x},
-	{"YAWRATE_MAX", &maxRate.z},
-	{"TILT_MAX", &tiltMax},
 	// imu
 	{"ACC_BIAS_X", &accBias.x},
 	{"ACC_BIAS_Y", &accBias.y},
@@ -48,6 +41,9 @@ Parameter parameters[] = {
 	{"ACC_SCALE_X", &accScale.x},
 	{"ACC_SCALE_Y", &accScale.y},
 	{"ACC_SCALE_Z", &accScale.z},
+	// {"GYRO_BIAS_X", &gyroBias.x},
+	// {"GYRO_BIAS_Y", &gyroBias.y},
+	// {"GYRO_BIAS_Z", &gyroBias.z},
 	// rc
 	{"RC_NEUTRAL_0", &channelNeutral[0]},
 	{"RC_NEUTRAL_1", &channelNeutral[1]},
@@ -64,11 +60,7 @@ Parameter parameters[] = {
 	{"RC_MAX_4", &channelMax[4]},
 	{"RC_MAX_5", &channelMax[5]},
 	{"RC_MAX_6", &channelMax[6]},
-	{"RC_MAX_7", &channelMax[7]},
-#if WIFI_ENABLED
-	// MAVLink
-	{"MAV_CTRL_SCALE", &mavlinkControlScale},
-#endif
+	{"RC_MAX_7", &channelMax[7]}
 };
 
 void setupParameters() {
@@ -76,6 +68,7 @@ void setupParameters() {
 	// Read parameters from storage
 	for (auto &parameter : parameters) {
 		if (!storage.isKey(parameter.name)) {
+			Serial.printf("Define new parameter %s = %f\n", parameter.name, *parameter.variable);
 			storage.putFloat(parameter.name, *parameter.variable);
 		}
 		*parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
@@ -88,12 +81,10 @@ int parametersCount() {
 }
 
 const char *getParameterName(int index) {
-	if (index < 0 || index >= parametersCount()) return "";
 	return parameters[index].name;
 }
 
 float getParameter(int index) {
-	if (index < 0 || index >= parametersCount()) return NAN;
 	return *parameters[index].variable;
 }
 
@@ -116,11 +107,11 @@ bool setParameter(const char *name, const float value) {
 	return false;
 }
 
-void syncParameters() {
-	static double lastSync = 0;
-	if (t - lastSync < 1) return; // sync once per second
+void flushParameters() {
+	static float lastFlush = 0;
+	if (t - lastFlush < 1) return; // flush once per second
 	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
-	lastSync = t;
+	lastFlush = t;
 
 	for (auto &parameter : parameters) {
 		if (parameter.value == *parameter.variable) continue;
@@ -132,7 +123,7 @@ void syncParameters() {
 
 void printParameters() {
 	for (auto &parameter : parameters) {
-		print("%s = %g\n", parameter.name, *parameter.variable);
+		Serial.printf("%s = %g\n", parameter.name, *parameter.variable);
 	}
 }
 

flix/quaternion.h

@@ -60,7 +60,7 @@ public:
 		return ret;
 	}
 
-	void toAxisAngle(float& a, float& b, float& c, float& angle) const {
+	void toAxisAngle(float& a, float& b, float& c, float& angle) {
 		angle = acos(w) * 2;
 		a = x / sin(angle / 2);
 		b = y / sin(angle / 2);
@@ -126,7 +126,7 @@ public:
 		return (*this = ret);
 	}
 
-	Quaternion operator * (const Quaternion& q) const {
+	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,
@@ -155,33 +155,24 @@ public:
 		z /= n;
 	}
 
-	Vector conjugate(const Vector& v) const {
+	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 vector by quaternion
-	Vector rotateVector(const Vector& v) const {
+	inline Vector rotate(const Vector& v) {
 		return conjugateInversed(v);
 	}
 
-	// Rotate quaternion by quaternion
-	Quaternion rotate(const Quaternion& q, const bool normalize = true) const {
-		Quaternion rotated = (*this) * q;
-		if (normalize) {
-			rotated.normalize();
-		}
-		return rotated;
-	}
-
-	bool finite() const {
+	inline bool finite() const {
 		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
 	}
 

flix/rc.ino

@@ -4,66 +4,52 @@
 // Work with the RC receiver
 
 #include <SBUS.h>
-#include "util.h"
+
+float channelNeutral[RC_CHANNELS] = {NAN}; // first element NAN means not calibrated
+float channelMax[RC_CHANNELS];
 
 SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
-// RC channels mapping:
-int rollChannel = 0;
-int pitchChannel = 1;
-int throttleChannel = 2;
-int yawChannel = 3;
-int armedChannel = 4;
-int modeChannel = 5;
-
-double controlsTime; // time of the last controls update
-float channelNeutral[16] = {NAN}; // first element NAN means not calibrated
-float channelMax[16];
-
 void setupRC() {
-	print("Setup RC\n");
+	Serial.println("Setup RC");
 	RC.begin();
 }
 
-bool readRC() {
+void readRC() {
 	if (RC.read()) {
 		SBUSData data = RC.data();
 		memcpy(channels, data.ch, sizeof(channels)); // copy channels data
 		normalizeRC();
 		controlsTime = t;
-		return true;
 	}
-	return false;
 }
 
 void normalizeRC() {
 	if (isnan(channelNeutral[0])) return; // skip if not calibrated
-	for (uint8_t i = 0; i < 16; i++) {
+	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
 		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
 	}
 }
 
 void calibrateRC() {
-	print("Calibrate RC: move all sticks to maximum positions [4 sec]\n");
-	print("··o     ··o\n···     ···\n···     ···\n");
-	pause(4);
-	while (!readRC());
-	for (int i = 0; i < 16; i++) {
+	Serial.println("Calibrate RC: move all sticks to maximum positions within 4 seconds");
+	Serial.println("··o     ··o\n···     ···\n···     ···");
+	delay(4000);
+	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];
 	}
-	print("Calibrate RC: move all sticks to neutral positions [4 sec]\n");
-	print("···     ···\n···     ·o·\n·o·     ···\n");
-	pause(4);
-	while (!readRC());
-	for (int i = 0; i < 16; i++) {
+	Serial.println("Calibrate RC: move all sticks to neutral positions within 4 seconds");
+	Serial.println("···     ···\n···     ·o·\n·o·     ···");
+	delay(4000);
+	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];
 	}
 	printRCCal();
 }
 
 void printRCCal() {
-	for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) print("%g ", channelNeutral[i]);
-	print("\n");
-	for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) print("%g ", channelMax[i]);
-	print("\n");
+	printArray(channelNeutral, RC_CHANNELS);
+	printArray(channelMax, RC_CHANNELS);
 }

flix/time.ino

@@ -3,10 +3,8 @@
 
 // Time related functions
 
-float loopRate; // Hz
-
 void step() {
-	double now = micros() / 1000000.0;
+	float now = micros() / 1000000.0;
 	dt = now - t;
 	t = now;
 
@@ -18,7 +16,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.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,17 +26,17 @@ float wrapAngle(float angle) {
 	return angle;
 }
 
-// Disable reset on low voltage
-void disableBrownOut() {
-	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
+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("}");
 }
 
-// Trim and split string by spaces
-void splitString(String& str, String& token0, String& token1, String& token2) {
-	str.trim();
-	char chars[str.length() + 1];
-	str.toCharArray(chars, str.length() + 1);
-	token0 = strtok(chars, " ");
-	token1 = strtok(NULL, " "); // String(NULL) creates empty string
-	token2 = strtok(NULL, "");
+// Disable reset on low voltage
+void disableBrownOut() {
+	WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
 }

flix/vector.h

@@ -54,15 +54,15 @@ 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);
 	}
 
-	bool finite() const {
+	inline bool finite() const {
 		return isfinite(x) && isfinite(y) && isfinite(z);
 	}
 

flix/wifi.ino

@@ -3,7 +3,7 @@
 
 // Wi-Fi support
 
-#if WIFI_ENABLED
+#if WIFI_ENABLED == 1
 
 #include <WiFi.h>
 #include <WiFiAP.h>
@@ -17,13 +17,13 @@
 WiFiUDP udp;
 
 void setupWiFi() {
-	print("Setup Wi-Fi\n");
+	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) {
-	if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
 	udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT);
 	udp.write(buf, len);
 	udp.endPacket();

gazebo/Arduino.h

@@ -41,17 +41,13 @@ class __FlashStringHelper;
 // https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/
 class String: public std::string {
 public:
-	String(const char *str = "") : std::string(str ? str : "") {}
+	String(const char *str = "") : std::string(str) {}
 	long toInt() const { return atol(this->c_str()); }
 	float toFloat() const { return atof(this->c_str()); }
 	bool isEmpty() const { return this->empty(); }
 	void toCharArray(char *buf, unsigned int bufsize, unsigned int index = 0) const {
 		strlcpy(buf, this->c_str() + index, bufsize);
 	}
-	void trim() {
-		this->erase(0, this->find_first_not_of(" \t\n\r"));
-		this->erase(this->find_last_not_of(" \t\n\r") + 1);
-	}
 };
 
 class Print;
@@ -115,7 +111,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
@@ -154,9 +150,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;
 

gazebo/Preferences.h

@@ -36,6 +36,8 @@ public:
 		return true;
 	}
 
+	void end();
+
 	bool isKey(const char *key) {
 		return storage.find(key) != storage.end();
 	}

gazebo/SBUS.h

@@ -14,7 +14,7 @@ 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 joystickInitialized; };
 	SBUSData data() {
 		SBUSData data;
 		joystickGet(data.ch);

gazebo/flix.h

@@ -10,21 +10,30 @@
 #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
 
-double t = NAN;
+#define ONE_G 9.80665
+
+float t = NAN;
 float dt;
+float loopRate;
 float motors[4];
 int16_t channels[16]; // raw rc channels
-float controls[16];
+float controls[RC_CHANNELS];
+float controlsTime;
 Vector acc;
 Vector gyro;
 Vector rates;
 Quaternion attitude;
-bool landed;
 
 // declarations
-void step();
 void computeLoopRate();
 void applyGyro();
 void applyAcc();
@@ -33,41 +42,26 @@ void interpretRC();
 void controlAttitude();
 void controlRate();
 void controlTorque();
-const char* getModeName();
-void sendMotors();
+void showTable();
 bool motorsActive();
-void testMotor(uint8_t n);
-void print(const char* format, ...);
-void pause(float duration);
-void doCommand(String str, bool echo);
-void handleInput();
-void calibrateRC();
+void cliTestMotor(uint8_t n);
+String stringToken(char* str, const char* delim);
 void normalizeRC();
 void printRCCal();
-void dumpLog();
 void processMavlink();
 void sendMavlink();
 void sendMessage(const void *msg);
 void receiveMavlink();
 void handleMavlink(const void *_msg);
-void mavlinkPrint(const char* str);
-inline Quaternion fluToFrd(const Quaternion &q);
 void failsafe();
-void armingFailsafe();
-void rcLossFailsafe();
 void descend();
-int parametersCount();
-const char *getParameterName(int index);
-float getParameter(int index);
-float getParameter(const char *name);
-bool setParameter(const char *name, const float value);
-void printParameters();
-void resetParameters();
+inline Quaternion FLU2FRD(const Quaternion &q);
 
 // mocks
 void setLED(bool on) {};
-void calibrateGyro() { print("Skip gyro calibrating\n"); };
-void calibrateAccel() { print("Skip accel calibrating\n"); };
-void printIMUCal() { print("cal: N/A\n"); };
+void calibrateGyro() { printf("Skip gyro calibrating\n"); };
+void calibrateAccel() { printf("Skip accel calibrating\n"); };
+void sendMotors() {};
+void printIMUCal() { printf("cal: N/A\n"); };
 void printIMUInfo() {};
 Vector accBias, gyroBias, accScale(1, 1, 1);

gazebo/joystick.h

@@ -7,13 +7,17 @@
 #include <gazebo/gazebo.hh>
 #include <iostream>
 
+#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;
 
-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) {
@@ -23,13 +27,17 @@ bool joystickInit() {
 		gzwarn << "Joystick not found, begin waiting for joystick..." << std::endl;
 		warnShown = true;
 	}
-	return joystickInitialized;
 }
 
 bool joystickGet(int16_t ch[16]) {
+	if (!joystickInitialized) {
+		joystickInit();
+		return false;
+	}
+
 	SDL_JoystickUpdate();
 
-	for (uint8_t i = 0; i < 16; i++) {
+	for (uint8_t i = 0; i < sizeof(channels) / sizeof(channels[0]); i++) {
 		ch[i] = SDL_JoystickGetAxis(joystick, i);
 	}
 	return true;

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>

gazebo/simulator.cpp

@@ -17,18 +17,17 @@
 
 #include "Arduino.h"
 #include "flix.h"
-
-#include "cli.ino"
-#include "control.ino"
-#include "estimate.ino"
-#include "failsafe.ino"
-#include "log.ino"
-#include "lpf.h"
-#include "mavlink.ino"
-#include "motors.ino"
-#include "parameters.ino"
+#include "util.ino"
 #include "rc.ino"
 #include "time.ino"
+#include "estimate.ino"
+#include "control.ino"
+#include "log.ino"
+#include "parameters.ino"
+#include "cli.ino"
+#include "mavlink.ino"
+#include "failsafe.ino"
+#include "lpf.h"
 
 using ignition::math::Vector3d;
 using namespace gazebo;
@@ -73,8 +72,8 @@ public:
 
 		// read rc
 		readRC();
-		controls[modeChannel] = 1; // 0 acro, 1 stab
-		controls[armedChannel] = 1; // armed
+		controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
+		controls[RC_CHANNEL_ARMED] = 1; // armed
 
 		estimate();
 
@@ -82,13 +81,13 @@ public:
 		attitude.setYaw(this->model->WorldPose().Yaw());
 
 		control();
-		handleInput();
+		parseInput();
 		processMavlink();
 
 		applyMotorForces();
 		publishTopics();
 		logData();
-		syncParameters();
+		flushParameters();
 	}
 
 	void applyMotorForces() {

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

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) {}

gazebo/wifi.h

@@ -22,10 +22,7 @@ void setupWiFi() {
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_ANY;
 	addr.sin_port = htons(WIFI_UDP_PORT_LOCAL);
-	if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
-		gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT_LOCAL << std::endl;
-		return;
-	}
+	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_LOCAL << " (remote port " << WIFI_UDP_PORT_REMOTE << ")" << std::endl;