Merge branch 'master' into run-sim

Test simulator run in CI
2026-01-11 13:36:43 +00:00 · 2024-05-24 22:35:36 +03:00 · 2024-01-31 17:23:07 +03:00 · 2024-01-25 23:53:08 +03:00
77 changed files with 610 additions and 7957 deletions
--- a/.editorconfig
+++ b/.editorconfig
@@ -4,7 +4,7 @@ root = true
 end_of_line = lf
 insert_final_newline = true

-[*.{ino,cpp,c,h,hpp,sdf,world,json}]
+[*.{ino,cpp,c,h,hpp,sdf,world}]
 charset = utf-8
 indent_style = tab
 tab_width = 4
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -7,77 +7,75 @@ on:
    branches: [ master ]

 jobs:
-  build_linux:
+  build:
    runs-on: ubuntu-latest
    steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v3
    - name: Install Arduino CLI
      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
    steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v3
    - name: Install Arduino CLI
      run: brew install arduino-cli
    - name: Build firmware
      run: make
-    - name: Check c_cpp_properties.json
-      run: tools/check_c_cpp_properties.py

  build_windows:
    runs-on: windows-latest
    steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v3
    - name: Install Arduino CLI
      run: choco install arduino-cli
    - name: Install Make
      run: choco install make
    - name: Build firmware
      run: make
-    - name: Check c_cpp_properties.json
-      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
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v3
    - name: Install Gazebo
      run: curl -sSL http://get.gazebosim.org | sh
    - name: Install SDL2
      run: sudo apt-get install libsdl2-dev
    - name: Build simulator
      run: make build_simulator
-    - uses: actions/upload-artifact@v4
+    - name: Run simulator
+      run: timeout --preserve-status 30 make simulator GAZEBO=gzserver || [ $? -eq 143 ]
+    - 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-latest
+    steps:
+    - name: Install Arduino CLI
+      run: brew install arduino-cli
+    - uses: actions/checkout@v3
+    - 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
+    - name: Run simulator
+      run: |
+        brew install coreutils
+        timeout --preserve-status 30 make simulator GAZEBO=gzserver || [ $? -eq 143 ]
--- a/.github/workflows/tools.yml
+++ b/.github/workflows/tools.yml
@@ -1,33 +0,0 @@
-name: Build tools
-
-on:
-  push:
-    branches: [ '*' ]
-  pull_request:
-    branches: [ master ]
-
-jobs:
-  csv_to_ulog:
-    runs-on: ubuntu-latest
-    steps:
-    - uses: actions/checkout@v4
-    - name: Build csv_to_ulog
-      run: cd tools/csv_to_ulog && mkdir build && cd build && cmake .. && make
-    - name: Test csv_to_ulog
-      run: |
-        cd tools/csv_to_ulog/build
-        echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv
-        ./csv_to_ulog log.csv
-        test $(stat -c %s log.ulg) -eq 196
-  python_tools:
-    runs-on: ubuntu-latest
-    steps:
-    - uses: actions/checkout@v4
-    - name: Install Python dependencies
-      run: pip install -r tools/requirements.txt
-    - name: Test csv_to_mcap tool
-      run: |
-        cd tools
-        echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv
-        ./csv_to_mcap.py log.csv
-        test $(stat -c %s log.mcap) -eq 883
--- a/.gitignore
+++ b/.gitignore
@@ -3,10 +3,3 @@
 build/
 tools/log/
 .dependencies
-.vscode/*
-!.vscode/settings.json
-!.vscode/c_cpp_properties.json
-!.vscode/tasks.json
-!.vscode/launch.json
-!.vscode/extensions.json
-!.vscode/intellisense.h
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -1,144 +0,0 @@
-{
-	"configurations": [
-		{
-			"name": "Linux",
-			"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",
-				"~/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",
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino"
-			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
-			"cStandard": "c11",
-			"cppStandard": "c++17",
-			"defines": [
-				"F_CPU=240000000L",
-				"ARDUINO=10607",
-				"ARDUINO_D1_MINI32",
-				"ARDUINO_ARCH_ESP32",
-				"ARDUINO_BOARD=D1_MINI32",
-				"ARDUINO_VARIANT=d1_mini32",
-				"ARDUINO_PARTITION_default",
-				"ESP32",
-				"CORE_DEBUG_LEVEL=0",
-				"ARDUINO_USB_CDC_ON_BOOT="
-			]
-		},
-		{
-			"name": "Mac",
-			"includePath": [
-				"${workspaceFolder}/flix",
-				// "${workspaceFolder}/gazebo",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/include/**",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
-				"~/Documents/Arduino/libraries/**",
-				"/opt/homebrew/include/**"
-			],
-			"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",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino"
-			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
-			"cStandard": "c11",
-			"cppStandard": "c++17",
-			"defines": [
-				"F_CPU=240000000L",
-				"ARDUINO=10607",
-				"ARDUINO_D1_MINI32",
-				"ARDUINO_ARCH_ESP32",
-				"ARDUINO_BOARD=D1_MINI32",
-				"ARDUINO_VARIANT=d1_mini32",
-				"ARDUINO_PARTITION_default",
-				"ARDUINO_FQBN=esp32:esp32:d1_mini32",
-				"ESP32",
-				"CORE_DEBUG_LEVEL=0",
-				"ARDUINO_USB_CDC_ON_BOOT="
-			]
-		},
-		{
-			"name": "Win32",
-			"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",
-				"~/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",
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino"
-			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
-			"cStandard": "c11",
-			"cppStandard": "c++17",
-			"defines": [
-				"F_CPU=240000000L",
-				"ARDUINO=10607",
-				"ARDUINO_D1_MINI32",
-				"ARDUINO_ARCH_ESP32",
-				"ARDUINO_BOARD=D1_MINI32",
-				"ARDUINO_VARIANT=d1_mini32",
-				"ARDUINO_PARTITION_default",
-				"ARDUINO_FQBN=esp32:esp32:d1_mini32",
-				"ESP32",
-				"CORE_DEBUG_LEVEL=0",
-				"ARDUINO_USB_CDC_ON_BOOT="
-			]
-		}
-	],
-	"version": 4
-}
--- a/.vscode/extensions.json
+++ b/.vscode/extensions.json
@@ -1,9 +0,0 @@
-{
-	// See https://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
-	"recommendations": [
-		"ms-vscode.cpptools",
-		"ms-vscode.cmake-tools",
-		"ms-python.python"
-	],
-	"unwantedRecommendations": []
-}
--- a/.vscode/intellisense.h
+++ b/.vscode/intellisense.h
@@ -1,5 +0,0 @@
-#ifdef __INTELLISENSE__
-	#pragma diag_suppress 144, 513
-	// diag 144: a value of type "enum <unnamed>" cannot be used to initialize an entity of type "enum <unnamed>"C/C++
-	// diag 513: a value of type "enum <unnamed>" cannot be assigned to an entity of type "enum <unnamed>"C/C++
-#endif
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -1,25 +0,0 @@
-{
-	"version": "0.2.0",
-	"configurations": [
-		{
-			"name": "Debug simulation",
-			"type": "cppdbg",
-			"request": "launch",
-			"program": "/usr/bin/gzserver",
-			"osx": {
-				"program": "/opt/homebrew/bin/gzserver",
-				"MIMode": "lldb",
-			},
-			"args": ["--verbose", "${workspaceFolder}/gazebo/flix.world"],
-			"stopAtEntry": false,
-			"cwd": "${fileDirname}",
-			"environment": [
-				{"name": "GAZEBO_MODEL_PATH", "value": "${workspaceFolder}/gazebo/models"},
-				{"name": "GAZEBO_PLUGIN_PATH", "value": "${workspaceFolder}/gazebo/build"}
-			],
-			"MIMode": "gdb",
-			"preLaunchTask": "Build simulator",
-			"externalConsole": true,
-		},
-	]
-}
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,13 +0,0 @@
-{
-	"C_Cpp.intelliSenseEngineFallback": "enabled",
-	"files.associations": {
-		"*.sdf": "xml",
-		"*.ino": "cpp",
-		"*.h": "cpp"
-	},
-	"C_Cpp.vcFormat.newLine.beforeOpenBrace.function": "newLine",
-	"C_Cpp.vcFormat.newLine.beforeOpenBrace.block": "sameLine",
-	"C_Cpp.vcFormat.newLine.beforeOpenBrace.lambda": "sameLine",
-	"C_Cpp.vcFormat.newLine.beforeOpenBrace.namespace": "sameLine",
-	"C_Cpp.vcFormat.newLine.beforeOpenBrace.type": "sameLine"
-}
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -1,31 +0,0 @@
-{
-	"tasks": [
-		{
-			"label": "Build firmware",
-			"type": "shell",
-			"command": "make",
-			"problemMatcher": [ "$gcc" ],
-			"presentation": { "clear": true, "showReuseMessage": false },
-		},
-		{
-			"label": "Upload firmware",
-			"type": "shell",
-			"command": "make upload",
-			"problemMatcher": [ "$gcc" ],
-			"presentation": { "clear": true, "showReuseMessage": false }
-		},
-		{
-			"label": "Build simulator",
-			"type": "shell",
-			"command": "make build_simulator",
-			"problemMatcher": [ "$gcc" ],
-			"presentation": { "clear": true, "showReuseMessage": false }
-		},
-		{
-			"label": "Clean",
-			"type": "shell",
-			"command": "make clean",
-		}
-	],
-	"version": "2.0.0"
-}
--- a/7
+++ b/7
@@ -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@2.0.11 --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.1
 	touch .dependencies

 gazebo/build cmake: gazebo/CMakeLists.txt
@@ -26,10 +26,11 @@ gazebo/build cmake: gazebo/CMakeLists.txt
 build_simulator: .dependencies gazebo/build
 	make -C gazebo/build

+GAZEBO ?= gazebo
 simulator: build_simulator
 	GAZEBO_MODEL_PATH=$$GAZEBO_MODEL_PATH:${CURDIR}/gazebo/models \
 	GAZEBO_PLUGIN_PATH=$$GAZEBO_PLUGIN_PATH:${CURDIR}/gazebo/build \
-	gazebo --verbose ${CURDIR}/gazebo/flix.world
+	$(GAZEBO) --verbose ${CURDIR}/gazebo/flix.world

 log:
 	PORT=$(PORT) tools/grab_log.py
--- a/README.md
+++ b/README.md
@@ -1,7 +1,69 @@
-# Flix
+# 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.
+<img src="docs/img/flix.jpg" width=500 alt="Flix quadcopter">

-<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.
+* Command line interface through USB port.
+* Wi-Fi support.
+* MAVLink support.
+* Control using mobile phone (with QGroundControl app).
+* ESCs with reverse mode support.
+* *Textbook and videos for students on writing a flight controller\*.*
+* *Completely 3D-printed frame*.*
+* *Position control and autonomous flights using external camera\**.
+* [Building and running instructions](docs/build.md).
+
+*\* — planned.*
+
+## It actually flies
+
+<a href="https://youtu.be/8GzzIQ3C6DQ"><img width=500 src="https://i3.ytimg.com/vi/8GzzIQ3C6DQ/maxresdefault.jpg"></a>
+
+See YouTube demo video: https://youtu.be/8GzzIQ3C6DQ.
+
+## Simulation
+
+Simulation in Gazebo using a plugin that runs original Arduino code is implemented:
+
+<img src="docs/img/simulator.png" width=500 alt="Flix simulator">
+
+## Schematics
+
+<img src="docs/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.
+
+*\* — SBUS inverter is not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).*
+
+## Components (version 0)
+
+|Component|Type|Image|Quantity|
+|-|-|-|-|
+|ESP32 Mini|Microcontroller board|<img src="docs/img/esp32.jpg" width=100>|1|
+|GY-91|IMU+LDO+barometer board|<img src="docs/img/gy-91.jpg" width=100>|1|
+|K100|Quadcopter frame|<img src="docs/img/frame.jpg" width=100>|1|
+|8520 3.7V brushed motor (**shaft 0.8mm!**)|Motor|<img src="docs/img/motor.jpeg" width=100>|4|
+|Hubsan 55 mm| Propeller|<img src="docs/img/prop.jpg" width=100>|4|
+|2.7A 1S Dual Way Micro Brush ESC|Motor ESC|<img src="docs/img/esc.jpg" width=100>|4|
+|KINGKONG TINY X8|RC transmitter|<img src="docs/img/tx.jpg" width=100>|1|
+|DF500 (SBUS)|RC receiver|<img src="docs/img/rx.jpg" width=100>|1|
+||~~SBUS inverter~~*|<img src="docs/img/inv.jpg" width=100>|~~1~~|
+|3.7 Li-Po 850 MaH 60C|Battery|||
+||Battery charger|<img src="docs/img/charger.jpg" width=100>|1|
+||Wires, connectors, tape, ...|||
+||3D-printed frame parts|||
+
+*\* — not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).*
+
+## Materials
+
+Subscribe to Telegram-channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter.
+
+Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/.
--- a/docs/assets/esp32-holder.step
+++ b/docs/assets/esp32-holder.step
--- a/docs/assets/esp32-holder.stl
+++ b/docs/assets/esp32-holder.stl
--- a/docs/assets/flix-frame.step
+++ b/docs/assets/flix-frame.step
--- a/docs/assets/flix-frame.stl
+++ b/docs/assets/flix-frame.stl
--- a/docs/assets/washer-m3.step
+++ b/docs/assets/washer-m3.step
@@ -1,200 +0,0 @@
-ISO-10303-21;
-HEADER;
-
-FILE_DESCRIPTION(
-/* description */ (''),
-/* implementation_level */ '2;1');
-
-FILE_NAME(
-/* name */ 'washer-m3.step',
-/* time_stamp */ '2024-10-29T13:59:42+03:00',
-/* author */ (''),
-/* organization */ (''),
-/* preprocessor_version */ '',
-/* originating_system */ '',
-
-/* authorisation */ '');
-
-FILE_SCHEMA (('AUTOMOTIVE_DESIGN { 1 0 10303 214 3 1 1 }'));
-ENDSEC;
-
-DATA;
-#10=MECHANICAL_DESIGN_GEOMETRIC_PRESENTATION_REPRESENTATION('',(#13),#125);
-#11=SHAPE_REPRESENTATION_RELATIONSHIP('SRR','None',#132,#12);
-#12=ADVANCED_BREP_SHAPE_REPRESENTATION('',(#14),#124);
-#13=STYLED_ITEM('',(#141),#14);
-#14=MANIFOLD_SOLID_BREP('Body1',#65);
-#15=FACE_BOUND('',#26,.T.);
-#16=FACE_BOUND('',#28,.T.);
-#17=PLANE('',#85);
-#18=PLANE('',#86);
-#19=FACE_OUTER_BOUND('',#23,.T.);
-#20=FACE_OUTER_BOUND('',#24,.T.);
-#21=FACE_OUTER_BOUND('',#25,.T.);
-#22=FACE_OUTER_BOUND('',#27,.T.);
-#23=EDGE_LOOP('',(#47,#48,#49,#50));
-#24=EDGE_LOOP('',(#51,#52,#53,#54));
-#25=EDGE_LOOP('',(#55));
-#26=EDGE_LOOP('',(#56));
-#27=EDGE_LOOP('',(#57));
-#28=EDGE_LOOP('',(#58));
-#29=LINE('',#112,#31);
-#30=LINE('',#118,#32);
-#31=VECTOR('',#93,1.7);
-#32=VECTOR('',#100,2.7);
-#33=CIRCLE('',#80,1.7);
-#34=CIRCLE('',#81,1.7);
-#35=CIRCLE('',#83,2.7);
-#36=CIRCLE('',#84,2.7);
-#37=VERTEX_POINT('',#109);
-#38=VERTEX_POINT('',#111);
-#39=VERTEX_POINT('',#115);
-#40=VERTEX_POINT('',#117);
-#41=EDGE_CURVE('',#37,#37,#33,.T.);
-#42=EDGE_CURVE('',#37,#38,#29,.T.);
-#43=EDGE_CURVE('',#38,#38,#34,.T.);
-#44=EDGE_CURVE('',#39,#39,#35,.T.);
-#45=EDGE_CURVE('',#39,#40,#30,.T.);
-#46=EDGE_CURVE('',#40,#40,#36,.T.);
-#47=ORIENTED_EDGE('',*,*,#41,.F.);
-#48=ORIENTED_EDGE('',*,*,#42,.T.);
-#49=ORIENTED_EDGE('',*,*,#43,.T.);
-#50=ORIENTED_EDGE('',*,*,#42,.F.);
-#51=ORIENTED_EDGE('',*,*,#44,.F.);
-#52=ORIENTED_EDGE('',*,*,#45,.T.);
-#53=ORIENTED_EDGE('',*,*,#46,.T.);
-#54=ORIENTED_EDGE('',*,*,#45,.F.);
-#55=ORIENTED_EDGE('',*,*,#44,.T.);
-#56=ORIENTED_EDGE('',*,*,#41,.T.);
-#57=ORIENTED_EDGE('',*,*,#46,.F.);
-#58=ORIENTED_EDGE('',*,*,#43,.F.);
-#59=CYLINDRICAL_SURFACE('',#79,1.7);
-#60=CYLINDRICAL_SURFACE('',#82,2.7);
-#61=ADVANCED_FACE('',(#19),#59,.F.);
-#62=ADVANCED_FACE('',(#20),#60,.T.);
-#63=ADVANCED_FACE('',(#21,#15),#17,.T.);
-#64=ADVANCED_FACE('',(#22,#16),#18,.F.);
-#65=CLOSED_SHELL('',(#61,#62,#63,#64));
-#66=DERIVED_UNIT_ELEMENT(#68,1.);
-#67=DERIVED_UNIT_ELEMENT(#127,-3.);
-#68=(
-MASS_UNIT()
-NAMED_UNIT(*)
-SI_UNIT(.KILO.,.GRAM.)
-);
-#69=DERIVED_UNIT((#66,#67));
-#70=MEASURE_REPRESENTATION_ITEM('density measure',
-POSITIVE_RATIO_MEASURE(7850.),#69);
-#71=PROPERTY_DEFINITION_REPRESENTATION(#76,#73);
-#72=PROPERTY_DEFINITION_REPRESENTATION(#77,#74);
-#73=REPRESENTATION('material name',(#75),#124);
-#74=REPRESENTATION('density',(#70),#124);
-#75=DESCRIPTIVE_REPRESENTATION_ITEM('Steel','Steel');
-#76=PROPERTY_DEFINITION('material property','material name',#134);
-#77=PROPERTY_DEFINITION('material property','density of part',#134);
-#78=AXIS2_PLACEMENT_3D('',#107,#87,#88);
-#79=AXIS2_PLACEMENT_3D('',#108,#89,#90);
-#80=AXIS2_PLACEMENT_3D('',#110,#91,#92);
-#81=AXIS2_PLACEMENT_3D('',#113,#94,#95);
-#82=AXIS2_PLACEMENT_3D('',#114,#96,#97);
-#83=AXIS2_PLACEMENT_3D('',#116,#98,#99);
-#84=AXIS2_PLACEMENT_3D('',#119,#101,#102);
-#85=AXIS2_PLACEMENT_3D('',#120,#103,#104);
-#86=AXIS2_PLACEMENT_3D('',#121,#105,#106);
-#87=DIRECTION('axis',(0.,0.,1.));
-#88=DIRECTION('refdir',(1.,0.,0.));
-#89=DIRECTION('center_axis',(0.,0.,1.));
-#90=DIRECTION('ref_axis',(1.,0.,0.));
-#91=DIRECTION('center_axis',(0.,0.,-1.));
-#92=DIRECTION('ref_axis',(1.,0.,0.));
-#93=DIRECTION('',(0.,0.,-1.));
-#94=DIRECTION('center_axis',(0.,0.,-1.));
-#95=DIRECTION('ref_axis',(1.,0.,0.));
-#96=DIRECTION('center_axis',(0.,0.,1.));
-#97=DIRECTION('ref_axis',(1.,0.,0.));
-#98=DIRECTION('center_axis',(0.,0.,1.));
-#99=DIRECTION('ref_axis',(1.,0.,0.));
-#100=DIRECTION('',(0.,0.,-1.));
-#101=DIRECTION('center_axis',(0.,0.,1.));
-#102=DIRECTION('ref_axis',(1.,0.,0.));
-#103=DIRECTION('center_axis',(0.,0.,1.));
-#104=DIRECTION('ref_axis',(1.,0.,0.));
-#105=DIRECTION('center_axis',(0.,0.,1.));
-#106=DIRECTION('ref_axis',(1.,0.,0.));
-#107=CARTESIAN_POINT('',(0.,0.,0.));
-#108=CARTESIAN_POINT('Origin',(0.,0.,0.));
-#109=CARTESIAN_POINT('',(-1.7,-2.0818995585505E-16,2.));
-#110=CARTESIAN_POINT('Origin',(0.,0.,2.));
-#111=CARTESIAN_POINT('',(-1.7,-2.0818995585505E-16,0.));
-#112=CARTESIAN_POINT('',(-1.7,-2.0818995585505E-16,0.));
-#113=CARTESIAN_POINT('Origin',(0.,0.,0.));
-#114=CARTESIAN_POINT('Origin',(0.,0.,0.));
-#115=CARTESIAN_POINT('',(-2.7,-3.30654635769785E-16,2.));
-#116=CARTESIAN_POINT('Origin',(0.,0.,2.));
-#117=CARTESIAN_POINT('',(-2.7,-3.30654635769785E-16,0.));
-#118=CARTESIAN_POINT('',(-2.7,-3.30654635769785E-16,0.));
-#119=CARTESIAN_POINT('Origin',(0.,0.,0.));
-#120=CARTESIAN_POINT('Origin',(0.,0.,2.));
-#121=CARTESIAN_POINT('Origin',(0.,0.,0.));
-#122=UNCERTAINTY_MEASURE_WITH_UNIT(LENGTH_MEASURE(0.01),#126,
-'DISTANCE_ACCURACY_VALUE',
-'Maximum model space distance between geometric entities at asserted c
-onnectivities');
-#123=UNCERTAINTY_MEASURE_WITH_UNIT(LENGTH_MEASURE(0.01),#126,
-'DISTANCE_ACCURACY_VALUE',
-'Maximum model space distance between geometric entities at asserted c
-onnectivities');
-#124=(
-GEOMETRIC_REPRESENTATION_CONTEXT(3)
-GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT((#122))
-GLOBAL_UNIT_ASSIGNED_CONTEXT((#126,#128,#129))
-REPRESENTATION_CONTEXT('','3D')
-);
-#125=(
-GEOMETRIC_REPRESENTATION_CONTEXT(3)
-GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT((#123))
-GLOBAL_UNIT_ASSIGNED_CONTEXT((#126,#128,#129))
-REPRESENTATION_CONTEXT('','3D')
-);
-#126=(
-LENGTH_UNIT()
-NAMED_UNIT(*)
-SI_UNIT(.MILLI.,.METRE.)
-);
-#127=(
-LENGTH_UNIT()
-NAMED_UNIT(*)
-SI_UNIT($,.METRE.)
-);
-#128=(
-NAMED_UNIT(*)
-PLANE_ANGLE_UNIT()
-SI_UNIT($,.RADIAN.)
-);
-#129=(
-NAMED_UNIT(*)
-SI_UNIT($,.STERADIAN.)
-SOLID_ANGLE_UNIT()
-);
-#130=SHAPE_DEFINITION_REPRESENTATION(#131,#132);
-#131=PRODUCT_DEFINITION_SHAPE('',$,#134);
-#132=SHAPE_REPRESENTATION('',(#78),#124);
-#133=PRODUCT_DEFINITION_CONTEXT('part definition',#138,'design');
-#134=PRODUCT_DEFINITION('washer-m3','washer-m3',#135,#133);
-#135=PRODUCT_DEFINITION_FORMATION('',$,#140);
-#136=PRODUCT_RELATED_PRODUCT_CATEGORY('washer-m3','washer-m3',(#140));
-#137=APPLICATION_PROTOCOL_DEFINITION('international standard',
-'automotive_design',2009,#138);
-#138=APPLICATION_CONTEXT(
-'Core Data for Automotive Mechanical Design Process');
-#139=PRODUCT_CONTEXT('part definition',#138,'mechanical');
-#140=PRODUCT('washer-m3','washer-m3',$,(#139));
-#141=PRESENTATION_STYLE_ASSIGNMENT((#142));
-#142=SURFACE_STYLE_USAGE(.BOTH.,#143);
-#143=SURFACE_SIDE_STYLE('',(#144));
-#144=SURFACE_STYLE_FILL_AREA(#145);
-#145=FILL_AREA_STYLE('Steel - Satin',(#146));
-#146=FILL_AREA_STYLE_COLOUR('Steel - Satin',#147);
-#147=COLOUR_RGB('Steel - Satin',0.627450980392157,0.627450980392157,0.627450980392157);
-ENDSEC;
-END-ISO-10303-21;
--- a/docs/assets/washer-m3.stl
+++ b/docs/assets/washer-m3.stl
--- a/docs/build.md
+++ b/docs/build.md
@@ -11,12 +11,10 @@ cd flix

 ### Ubuntu

-The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you have a newer version, consider using a virtual machine.
-
 1. Install Arduino CLI:

   ```bash
-   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh
+   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh
   ```

 2. Install Gazebo 11:
@@ -80,41 +78,22 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you h
   make simulator
   ```

-### Setup and flight
+### Flight

-#### Control with smartphone
-
-1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
-2. Connect your smartphone to the same Wi-Fi network as the machine running the simulator.
-3. If you're using a virtual machine, make sure that its network is set to the **bridged** mode with Wi-Fi adapter selected.
-4. Run the simulation.
-5. Open QGroundControl app. It should connect and begin showing the virtual drone's telemetry automatically.
-6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
-7. Use the virtual joystick to fly the drone!
-
-#### Control with USB remote control
-
-1. Connect your USB remote control to the machine running the simulator.
-2. Run the simulation.
-3. Calibrate the RC using `cr` command in the command line interface and stop the simulation.
-4. Copy the calibration results to the source code (`gazebo/joystick.h`).
-5. Run the simulation again.
-6. Use the USB remote control to fly the drone!
+Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* setting enabled) to control the drone. *Auto-Center Throttle* setting **should be disabled**.

 ## Firmware

 ### Arduino IDE (Windows, Linux, macOS)

 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
-2. Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).
-3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
-4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
-   * `FlixPeriph`, the latest version.
-   * `MAVLink`, version 2.0.16.
-5. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
-6. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
-7. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
-8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
+2. Install ESP32 core using [Boards Manager](https://docs.arduino.cc/learn/starting-guide/cores).
+3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
+   * `FlixPeriph`.
+   * `MAVLink`, version 2.0.1.
+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)

@@ -140,36 +119,12 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you h

 See other available Make commands in the [Makefile](../Makefile).

-### Setup and flight
-
-Before flight you need to calibrate the accelerometer:
-
-1. Open Serial Monitor in Arduino IDE (use use `make monitor` command in the command line).
-2. Type `ca` command there.
-3. Copy calibration results to the source code (`flix/imu.ino`).
-
-#### 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.
-4. Open QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
-5. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
-6. Use the virtual joystick to fly the drone!
-
-#### Control with remote control
-
-Before flight using remote control, you need to calibrate it:
-
-1. Open Serial Monitor in Arduino IDE (use use `make monitor` command in the command line).
-2. Type `cr` command there.
-3. Copy calibration results to the source code (`flix/rc.ino`).
-
-Then you can use your remote control to fly the drone!
-
-> [!NOTE]
-> If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article.
-
 ### Firmware code structure

 See [firmware overview](firmware.md) for more details.
+
+## Setup
+
+Before flight in simulation and on the real drone, you need to calibrate your remote control. Use drone's command line interface (`make monitor` on the real drone) and type `cr` command. Copy calibration results to the source code (`flix/rc.ino` and/or `gazebo/joystick.h`).
+
+On the real drone, you also need to calibrate the accelerometer and the gyroscope. Use `ca` and `cg` commands for that. Copy calibration results to the source code (`flix/imu.ino`).
--- 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/100n03a.jpg
+++ b/docs/img/100n03a.jpg
--- a/docs/img/battery.jpg
+++ b/docs/img/battery.jpg
--- a/docs/img/esp32-holder.jpg
+++ b/docs/img/esp32-holder.jpg
--- a/docs/img/flight-video.jpg
+++ b/docs/img/flight-video.jpg
--- a/docs/img/flightplot.png
+++ b/docs/img/flightplot.png
--- a/docs/img/flix1.jpg
+++ b/docs/img/flix1.jpg
--- a/docs/img/foxglove.png
+++ b/docs/img/foxglove.png
--- a/docs/img/frame1.jpg
+++ b/docs/img/frame1.jpg
--- a/docs/img/gy91-lfd.svg
+++ b/docs/img/gy91-lfd.svg
--- a/docs/img/icm-20948.jpg
+++ b/docs/img/icm-20948.jpg
--- a/docs/img/mosfet-connection.png
+++ b/docs/img/mosfet-connection.png
--- a/docs/img/mx.png
+++ b/docs/img/mx.png
--- a/docs/img/plotjuggler.png
+++ b/docs/img/plotjuggler.png
--- a/docs/img/resistor10k.jpg
+++ b/docs/img/resistor10k.jpg
--- a/docs/img/schematics1.svg
+++ b/docs/img/schematics1.svg
@@ -1,256 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<svg version="1.1" id="Layer_1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px"
-	 viewBox="0 0 1920 1080" style="enable-background:new 0 0 1920 1080;" xml:space="preserve">
-<style type="text/css">
-	.st0{clip-path:url(#SVGID_00000116955662310502408250000008996271717606231736_);fill:#FFFFFF;}
-	.st1{clip-path:url(#SVGID_00000116955662310502408250000008996271717606231736_);fill:#0076BA;}
-	.st2{clip-path:url(#SVGID_00000116955662310502408250000008996271717606231736_);fill:none;stroke:#0076BA;stroke-width:6;}
-	.st3{clip-path:url(#SVGID_00000055674346191406539380000013421132283630177205_);}
-	.st4{fill:#FFFFFF;}
-	.st5{font-family:'Tahoma';}
-	.st6{font-size:60px;}
-	.st7{clip-path:url(#SVGID_00000057846011469822040540000011754501750068092081_);fill:none;stroke:#0076BA;stroke-width:6;}
-	
-		.st8{clip-path:url(#SVGID_00000057846011469822040540000011754501750068092081_);fill:none;stroke:#0076BA;stroke-width:6;stroke-dasharray:12,12;}
-	.st9{clip-path:url(#SVGID_00000031912132892401345140000018376817810309323944_);}
-	.st10{letter-spacing:-1;}
-	.st11{clip-path:url(#SVGID_00000096022444481931167570000018189382621257791423_);fill:none;stroke:#D5D5D5;stroke-width:6;}
-	.st12{clip-path:url(#SVGID_00000096022444481931167570000018189382621257791423_);fill:#D5D5D5;}
-	.st13{clip-path:url(#SVGID_00000096022444481931167570000018189382621257791423_);}
-	.st14{font-size:40px;}
-	
-		.st15{clip-path:url(#SVGID_00000096022444481931167570000018189382621257791423_);fill:none;stroke:#D5D5D5;stroke-width:6;stroke-dasharray:12,12;}
-	.st16{letter-spacing:-3;}
-	.st17{clip-path:url(#SVGID_00000096022444481931167570000018189382621257791423_);fill:none;stroke:#0076BA;stroke-width:6;}
-	.st18{clip-path:url(#SVGID_00000178923025368094801390000008109591059568692644_);}
-	.st19{clip-path:url(#SVGID_00000170245726615134129150000014803823133490835355_);fill:none;stroke:#D5D5D5;stroke-width:6;}
-	.st20{clip-path:url(#SVGID_00000170245726615134129150000014803823133490835355_);fill:#D5D5D5;}
-	.st21{clip-path:url(#SVGID_00000170245726615134129150000014803823133490835355_);}
-	.st22{clip-path:url(#SVGID_00000170245726615134129150000014803823133490835355_);fill:none;stroke:#FF9300;stroke-width:6;}
-	.st23{clip-path:url(#SVGID_00000137829079020238483810000004945639728221863820_);}
-	.st24{clip-path:url(#SVGID_00000127742992435002359440000017943924755103710901_);fill:none;stroke:#0076BA;stroke-width:6;}
-	.st25{clip-path:url(#SVGID_00000172413095142863711730000006643673940354901182_);}
-	.st26{fill:#333333;}
-	.st27{clip-path:url(#SVGID_00000057841300642942441540000015331613196773382808_);fill:none;stroke:#0076BA;stroke-width:3;}
-	.st28{clip-path:url(#SVGID_00000015346235126698654330000007941041458912523396_);}
-	.st29{font-size:30px;}
-</style>
-<g>
-	<defs>
-		<rect id="SVGID_1_" width="1920" height="1080"/>
-	</defs>
-	<clipPath id="SVGID_00000171677823290121104880000004951624621648774806_">
-		<use xlink:href="#SVGID_1_"  style="overflow:visible;"/>
-	</clipPath>
-	
-		<rect style="clip-path:url(#SVGID_00000171677823290121104880000004951624621648774806_);fill:#FFFFFF;" width="1920" height="1080"/>
-	<path style="clip-path:url(#SVGID_00000171677823290121104880000004951624621648774806_);fill:#0076BA;" d="M781,353.4h358
-		c8.4,0,13.4,0,16.7,1.4c4.8,1.8,8.6,5.6,10.4,10.4c1.4,3.3,1.4,8.4,1.4,16.7v268.8c0,8.4,0,13.4-1.4,16.7
-		c-1.8,4.8-5.6,8.6-10.4,10.4c-3.3,1.4-8.4,1.4-16.7,1.4H781c-8.4,0-13.4,0-16.7-1.4c-4.8-1.8-8.6-5.6-10.4-10.4
-		c-1.4-3.3-1.4-8.4-1.4-16.7V381.9c0-8.4,0-13.4,1.4-16.7c1.8-4.8,5.6-8.6,10.4-10.4C767.6,353.4,772.6,353.4,781,353.4z"/>
-	
-		<path style="clip-path:url(#SVGID_00000171677823290121104880000004951624621648774806_);fill:none;stroke:#0076BA;stroke-width:6;" d="
-		M781,353.4h358c8.4,0,13.4,0,16.7,1.4c4.8,1.8,8.6,5.6,10.4,10.4c1.4,3.3,1.4,8.4,1.4,16.7v268.8c0,8.4,0,13.4-1.4,16.7
-		c-1.8,4.8-5.6,8.6-10.4,10.4c-3.3,1.4-8.4,1.4-16.7,1.4H781c-8.4,0-13.4,0-16.7-1.4c-4.8-1.8-8.6-5.6-10.4-10.4
-		c-1.4-3.3-1.4-8.4-1.4-16.7V381.9c0-8.4,0-13.4,1.4-16.7c1.8-4.8,5.6-8.6,10.4-10.4C767.6,353.4,772.6,353.4,781,353.4z"/>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000039115730048533150280000004801426134829330596_" x="737.4" y="353.4" width="445.2" height="325.8"/>
-	</defs>
-	<clipPath id="SVGID_00000016774010820287457580000002865762276517841055_">
-		<use xlink:href="#SVGID_00000039115730048533150280000004801426134829330596_"  style="overflow:visible;"/>
-	</clipPath>
-	<g style="clip-path:url(#SVGID_00000016774010820287457580000002865762276517841055_);">
-		<text transform="matrix(1 0 0 1 877.1631 540.0012)" class="st4 st5 st6">ESP32</text>
-	</g>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000075151113810300876960000013536007874996673433_" width="1920" height="1080"/>
-	</defs>
-	<clipPath id="SVGID_00000107586269070498659130000000925674275700912771_">
-		<use xlink:href="#SVGID_00000075151113810300876960000013536007874996673433_"  style="overflow:visible;"/>
-	</clipPath>
-	
-		<path style="clip-path:url(#SVGID_00000107586269070498659130000000925674275700912771_);fill:none;stroke:#0076BA;stroke-width:6;" d="
-		M107.1,424.9h369.6c6.7,0,10.7,0,13.4,1.1c3.9,1.4,6.9,4.4,8.3,8.3c1.1,2.7,1.1,6.7,1.1,13.4v153.2c0,6.7,0,10.7-1.1,13.4
-		c-1.4,3.9-4.4,6.9-8.3,8.3c-2.7,1.1-6.7,1.1-13.4,1.1H107.1c-6.7,0-10.7,0-13.4-1.1c-3.9-1.4-6.9-4.4-8.3-8.3
-		c-1.1-2.7-1.1-6.7-1.1-13.4V447.6c0-6.7,0-10.7,1.1-13.4c1.4-3.9,4.4-6.9,8.3-8.3C96.4,424.9,100.4,424.9,107.1,424.9z"/>
-	
-		<path style="clip-path:url(#SVGID_00000107586269070498659130000000925674275700912771_);fill:none;stroke:#0076BA;stroke-width:6;stroke-dasharray:12,12;" d="
-		M777.9,846.9h364.3c7.5,0,11.9,0,14.9,1.2c4.3,1.6,7.7,5,9.3,9.3c1.2,3,1.2,7.5,1.2,14.9V974c0,7.5,0,11.9-1.2,14.9
-		c-1.6,4.3-5,7.7-9.3,9.3c-3,1.2-7.5,1.2-14.9,1.2H777.9c-7.5,0-11.9,0-14.9-1.2c-4.3-1.6-7.7-5-9.3-9.3c-1.2-3-1.2-7.5-1.2-14.9
-		V872.4c0-7.5,0-11.9,1.2-14.9c1.6-4.3,5-7.7,9.3-9.3C765.9,846.9,770.4,846.9,777.9,846.9z"/>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000128454988870213717900000008065144461254055839_" x="737.3" y="846.9" width="445.4" height="152.5"/>
-	</defs>
-	<clipPath id="SVGID_00000124119398570885020400000016506660910002275730_">
-		<use xlink:href="#SVGID_00000128454988870213717900000008065144461254055839_"  style="overflow:visible;"/>
-	</clipPath>
-	<g style="clip-path:url(#SVGID_00000124119398570885020400000016506660910002275730_);">
-		<text transform="matrix(1 0 0 1 802.3304 947.0799)"><tspan x="0" y="0" class="st5 st6">RC </tspan><tspan x="92" y="0" class="st5 st6 st10">R</tspan><tspan x="128.2" y="0" class="st5 st6">ecei</tspan><tspan x="232.7" y="0" class="st5 st6">v</tspan><tspan x="262.2" y="0" class="st5 st6">er</tspan></text>
-	</g>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000130607252134697782620000007941153577788369085_" width="1920" height="1080"/>
-	</defs>
-	<clipPath id="SVGID_00000168829390248542060830000011029723519784449157_">
-		<use xlink:href="#SVGID_00000130607252134697782620000007941153577788369085_"  style="overflow:visible;"/>
-	</clipPath>
-	
-		<path style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:none;stroke:#D5D5D5;stroke-width:6;" d="
-		M429.1,526.4c96.1,19.6,194.9,26.9,296.4,22l3-0.2"/>
-	<polygon style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:#D5D5D5;" points="726.2,560.4 
-		749.5,547.1 724.8,536.5 	"/>
-	<g style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);">
-		<text transform="matrix(1 0 0 1 571.0244 599.9149)" class="st5 st14">SPI</text>
-	</g>
-	
-		<path style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:none;stroke:#D5D5D5;stroke-width:6;stroke-dasharray:12,12;" d="
-		M991.9,843.9c7.9-38.5,9.3-84.5,4.4-137.8l-0.3-3"/>
-	<polygon style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:#D5D5D5;" points="1008.2,704.8 
-		993.7,682.3 984.4,707.4 	"/>
-	<g style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);">
-		<text transform="matrix(1 0 0 1 746.5599 778.8257)"><tspan x="0" y="0" class="st5 st14">SBUS (</tspan><tspan x="122.2" y="0" class="st5 st14">U</tspan><tspan x="148.3" y="0" class="st5 st14">A</tspan><tspan x="172.2" y="0" class="st5 st14 st10">R</tspan><tspan x="196" y="0" class="st5 st14">T</tspan><tspan x="219.8" y="0" class="st5 st14">)</tspan></text>
-	</g>
-	
-		<path style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:none;stroke:#D5D5D5;stroke-width:6;" d="
-		M1170.5,537.8c72.8,3.7,147.2,3.8,223,0.3l3-0.2"/>
-	<polygon style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:#D5D5D5;" points="1394.1,550 
-		1417.5,536.8 1392.9,526 	"/>
-	<g style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);">
-		<text transform="matrix(1 0 0 1 1236.025 523.2462)" class="st5 st14">PWM</text>
-	</g>
-	
-		<path style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:none;stroke:#D5D5D5;stroke-width:6;" d="
-		M1612.3,595.6c-10.7,73.2-11.3,149.1-2,227.5l0.4,3"/>
-	<polygon style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:#D5D5D5;" points="1598.5,824.7 
-		1613.5,846.9 1622.2,821.6 	"/>
-	<g style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);">
-		<text transform="matrix(1 0 0 1 1450.277 736.9998)"><tspan x="0" y="0" class="st5 st14 st10">V</tspan><tspan x="22.1" y="0" class="st5 st14">o</tspan><tspan x="43.8" y="0" class="st5 st14">l</tspan><tspan x="53" y="0" class="st5 st14">tage</tspan></text>
-	</g>
-	<g style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);">
-		<text transform="matrix(1 0 0 1 212.8847 595.784)"><tspan x="0" y="0" class="st5 st6">G</tspan><tspan x="40" y="0" class="st5 st6 st16">Y</tspan><tspan x="70.8" y="0" class="st5 st6">-91</tspan></text>
-	</g>
-	
-		<path style="clip-path:url(#SVGID_00000168829390248542060830000011029723519784449157_);fill:none;stroke:#0076BA;stroke-width:6;" d="
-		M777.9,77.5h364.3c7.5,0,11.9,0,14.9,1.2c4.3,1.6,7.7,5,9.3,9.3c1.2,3,1.2,7.5,1.2,14.9v101.7c0,7.5,0,11.9-1.2,14.9
-		c-1.6,4.3-5,7.7-9.3,9.3c-3,1.2-7.5,1.2-14.9,1.2H777.9c-7.5,0-11.9,0-14.9-1.2c-4.3-1.6-7.7-5-9.3-9.3c-1.2-3-1.2-7.5-1.2-14.9
-		V103c0-7.5,0-11.9,1.2-14.9c1.6-4.3,5-7.7,9.3-9.3C765.9,77.5,770.4,77.5,777.9,77.5z"/>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000137820956330754408580000017691839315522467728_" x="737.3" y="77.5" width="445.4" height="152.5"/>
-	</defs>
-	<clipPath id="SVGID_00000016789657776342930330000013964875638603798149_">
-		<use xlink:href="#SVGID_00000137820956330754408580000017691839315522467728_"  style="overflow:visible;"/>
-	</clipPath>
-	<g style="clip-path:url(#SVGID_00000016789657776342930330000013964875638603798149_);">
-		<text transform="matrix(1 0 0 1 865.0551 177.6901)"><tspan x="0" y="0" class="st5 st6">B</tspan><tspan x="35.6" y="0" class="st5 st6">a</tspan><tspan x="67.1" y="0" class="st5 st6">t</tspan><tspan x="86.7" y="0" class="st5 st6">tery</tspan></text>
-	</g>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000016756559387490335270000000552908433054081441_" width="1920" height="1080"/>
-	</defs>
-	<clipPath id="SVGID_00000171720285722730791730000010936644841389779363_">
-		<use xlink:href="#SVGID_00000016756559387490335270000000552908433054081441_"  style="overflow:visible;"/>
-	</clipPath>
-	
-		<path style="clip-path:url(#SVGID_00000171720285722730791730000010936644841389779363_);fill:none;stroke:#D5D5D5;stroke-width:6;" d="
-		M925.7,233.1c-5.3,27.2-6.8,58.4-4.3,93.4l0.3,3"/>
-	<polygon style="clip-path:url(#SVGID_00000171720285722730791730000010936644841389779363_);fill:#D5D5D5;" points="909.4,327.6 
-		923.5,350.4 933.3,325.4 	"/>
-	<g style="clip-path:url(#SVGID_00000171720285722730791730000010936644841389779363_);">
-		<text transform="matrix(1 0 0 1 937.7745 299.4789)"><tspan x="0" y="0" class="st5 st14">&gt;3</tspan><tspan x="50.9" y="0" class="st5 st14">.</tspan><tspan x="62.1" y="0" class="st5 st14">7V</tspan></text>
-	</g>
-	
-		<path style="clip-path:url(#SVGID_00000171720285722730791730000010936644841389779363_);fill:none;stroke:#D5D5D5;stroke-width:6;" d="
-		M1170.6,161.5c228,25.6,371.6,110,430.8,253.1l1.1,2.8"/>
-	<polygon style="clip-path:url(#SVGID_00000171720285722730791730000010936644841389779363_);fill:#D5D5D5;" points="1590.2,418.8 
-		1609.8,437.1 1612.6,410.4 	"/>
-	<g style="clip-path:url(#SVGID_00000171720285722730791730000010936644841389779363_);">
-		<text transform="matrix(1 0 0 1 1396.229 204.8126)"><tspan x="0" y="0" class="st5 st14">&gt;3</tspan><tspan x="50.9" y="0" class="st5 st14">.</tspan><tspan x="62.1" y="0" class="st5 st14">7V</tspan></text>
-	</g>
-	
-		<path style="clip-path:url(#SVGID_00000171720285722730791730000010936644841389779363_);fill:none;stroke:#FF9300;stroke-width:6;" d="
-		M1445.9,849.9h364.3c7.5,0,11.9,0,14.9,1.2c4.3,1.6,7.7,5,9.3,9.3c1.2,3,1.2,7.5,1.2,14.9V977c0,7.5,0,11.9-1.2,14.9
-		c-1.6,4.3-5,7.7-9.3,9.3c-3,1.2-7.5,1.2-14.9,1.2h-364.3c-7.5,0-11.9,0-14.9-1.2c-4.3-1.6-7.7-5-9.3-9.3c-1.2-3-1.2-7.5-1.2-14.9
-		V875.4c0-7.5,0-11.9,1.2-14.9c1.6-4.3,5-7.7,9.3-9.3C1434,849.9,1438.5,849.9,1445.9,849.9z"/>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000125590519588794066770000004908208256101094292_" x="1405.4" y="849.9" width="445.4" height="152.5"/>
-	</defs>
-	<clipPath id="SVGID_00000020367246295901477730000005237269270844408247_">
-		<use xlink:href="#SVGID_00000125590519588794066770000004908208256101094292_"  style="overflow:visible;"/>
-	</clipPath>
-	<g style="clip-path:url(#SVGID_00000020367246295901477730000005237269270844408247_);">
-		<text transform="matrix(1 0 0 1 1507.9561 950.0799)" class="st5 st6">Motors </text>
-	</g>
-	<g style="clip-path:url(#SVGID_00000020367246295901477730000005237269270844408247_);">
-		<text transform="matrix(1 0 0 1 1706.5596 950.0799)" class="st5 st14">x4</text>
-	</g>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000078757851010072822240000012642622956562684587_" width="1920" height="1080"/>
-	</defs>
-	<clipPath id="SVGID_00000132802581807603645050000011682039405646066589_">
-		<use xlink:href="#SVGID_00000078757851010072822240000012642622956562684587_"  style="overflow:visible;"/>
-	</clipPath>
-	
-		<path style="clip-path:url(#SVGID_00000132802581807603645050000011682039405646066589_);fill:none;stroke:#0076BA;stroke-width:6;" d="
-		M1445.9,440.1h364.3c7.5,0,11.9,0,14.9,1.2c4.3,1.6,7.7,5,9.3,9.3c1.2,3,1.2,7.5,1.2,14.9v101.7c0,7.5,0,11.9-1.2,14.9
-		c-1.6,4.3-5,7.7-9.3,9.3c-3,1.2-7.5,1.2-14.9,1.2h-364.3c-7.5,0-11.9,0-14.9-1.2c-4.3-1.6-7.7-5-9.3-9.3c-1.2-3-1.2-7.5-1.2-14.9
-		V465.5c0-7.5,0-11.9,1.2-14.9c1.6-4.3,5-7.7,9.3-9.3C1434,440.1,1438.5,440.1,1445.9,440.1z"/>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000121968229548182703620000008187702022402881724_" x="1405.4" y="440.1" width="445.4" height="152.5"/>
-	</defs>
-	<clipPath id="SVGID_00000130637550574198708210000000321319364498620830_">
-		<use xlink:href="#SVGID_00000121968229548182703620000008187702022402881724_"  style="overflow:visible;"/>
-	</clipPath>
-	<g style="clip-path:url(#SVGID_00000130637550574198708210000000321319364498620830_);">
-		<text transform="matrix(1 0 0 1 1486.833 540.213)" class="st5 st6">MOSFE</text>
-	</g>
-	<g style="clip-path:url(#SVGID_00000130637550574198708210000000321319364498620830_);">
-		<text transform="matrix(1 0 0 1 1673.8936 540.213)" class="st26 st5 st6">T</text>
-	</g>
-	<g style="clip-path:url(#SVGID_00000130637550574198708210000000321319364498620830_);">
-		<text transform="matrix(1 0 0 1 1708.9326 540.213)" class="st5 st6"> </text>
-	</g>
-	<g style="clip-path:url(#SVGID_00000130637550574198708210000000321319364498620830_);">
-		<text transform="matrix(1 0 0 1 1727.6826 540.213)" class="st5 st14">x4</text>
-	</g>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000038395453607794357290000016650519059911045024_" width="1920" height="1080"/>
-	</defs>
-	<clipPath id="SVGID_00000077303055156502403190000000114138446408493755_">
-		<use xlink:href="#SVGID_00000038395453607794357290000016650519059911045024_"  style="overflow:visible;"/>
-	</clipPath>
-	
-		<path style="clip-path:url(#SVGID_00000077303055156502403190000000114138446408493755_);fill:none;stroke:#0076BA;stroke-width:3;" d="
-		M167.8,451H416c4.5,0,7.1,0,8.9,0.7c2.6,0.9,4.6,3,5.5,5.5c0.7,1.8,0.7,4.5,0.7,8.9v46.6c0,4.5,0,7.1-0.7,8.9
-		c-0.9,2.6-3,4.6-5.5,5.5c-1.8,0.7-4.5,0.7-8.9,0.7H167.8c-4.5,0-7.1,0-8.9-0.7c-2.6-0.9-4.6-3-5.5-5.5c-0.7-1.8-0.7-4.5-0.7-8.9
-		v-46.6c0-4.5,0-7.1,0.7-8.9c0.9-2.6,3-4.6,5.5-5.5C160.7,451,163.4,451,167.8,451z"/>
-</g>
-<g>
-	<defs>
-		<rect id="SVGID_00000077301502947632695740000005629016913768095395_" x="145.2" y="451" width="293.5" height="77"/>
-	</defs>
-	<clipPath id="SVGID_00000004518590643829462700000008680026774255780777_">
-		<use xlink:href="#SVGID_00000077301502947632695740000005629016913768095395_"  style="overflow:visible;"/>
-	</clipPath>
-	<g style="clip-path:url(#SVGID_00000004518590643829462700000008680026774255780777_);">
-		<text transform="matrix(1 0 0 1 197.8359 501.2101)" class="st5 st29">MPU9250 IMU</text>
-	</g>
-</g>
-</svg>
--- a/docs/img/screw-m1.4.jpg
+++ b/docs/img/screw-m1.4.jpg
--- a/docs/img/screw-m3.jpg
+++ b/docs/img/screw-m3.jpg
--- a/docs/img/washer-m3.jpg
+++ b/docs/img/washer-m3.jpg
--- a/docs/img/wire-28awg.jpg
+++ b/docs/img/wire-28awg.jpg
--- a/docs/log.md
+++ b/docs/log.md
@@ -1,72 +0,0 @@
-# Log analysis
-
-Flix quadcopter uses RAM to store flight log data. The default log capacity is 10 seconds at 100 Hz. This configuration can be adjusted in the `log.ino` file.
-
-To perform log analysis, you need to download the log right after the flight without powering off the drone. Then you can use several tools to analyze the log data.
-
-## Log download
-
-To download the log, connect the ESP32 using USB right after the flight and run the following command:
-
-```bash
-make log
-```
-
-Logs are stored in `tools/log/*.csv` files.
-
-## Analysis
-
-### PlotJuggler
-
-The recommended tool for log analysis is PlotJuggler.
-
-<img src="img/plotjuggler.png" width="500">
-
-1. Install PlotJuggler using the [official instructions](https://github.com/facontidavide/PlotJuggler?tab=readme-ov-file#installation).
-
-2. Run PlotJuggler and drag'n'drop the downloaded log file there. Choose `t` column to be used as X axis.
-
-   You can open the most recent downloaded file using the command:
-
-   ```bash
-   make plot
-   ```
-
-   You can perform both log download and run PlotJuggler in one command:
-
-   ```bash
-   make log plot
-   ```
-
-### FlightPlot
-
-FlightPlot is a powerful tool for analyzing logs in [ULog format](https://docs.px4.io/main/en/dev_log/ulog_file_format.html). This format is used in PX4 and ArduPilot flight software.
-
-<img src="img/flightplot.png" width="500">
-
-1. [Install FlightPlot](https://github.com/PX4/FlightPlot).
-2. Flix repository contains a tool for converting CSV logs to ULog format. Build the tool using [the instructions](../tools/csv_to_ulog/README.md) and convert the log you want to analyze.
-3. Run FlightPlot and drag'n'drop the converted ULog-file there.
-
-### Foxglove Studio
-
-Foxglove is a tool for visualizing and analyzing robotics data with very rich functionality. It can import various formats, but mainly focuses on its own format, called [MCAP](https://mcap.dev).
-
-<img src="img/foxglove.png" width="500">
-
-1. Install Foxglove Studio from the [official website](https://foxglove.dev/download).
-
-2. Flix repository contains a tool for converting CSV logs to MCAP format. First, install its dependencies:
-
-   ```bash
-   cd tools
-   pip install -r requirements.txt
-   ```
-
-3. Convert the log you want to analyze:
-
-   ```bash
-   csv_to_mcap.py log_file.csv
-   ```
-
-4. Open the log in Foxglove Studio using *Open local file* command.
--- 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,42 +19,71 @@ 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 appropriate motor\n"
+"fullmot <n> - full motor test\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", &loopFreq, 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);
 	} else if (command == "imu") {
-		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();
-		Serial.printf("rate: %f\n", loopRate);
+		printIMUCal();
+		Serial.printf("frequency: %f\n", loopFreq);
 	} 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,77 @@ 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 == "fullmot") {
+		fullMotorTest(value.toInt(), false);
 	} 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;
+	sendMotors();
+	delay(5000);
+	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,11 +50,8 @@ 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();
 	if (mode == STAB) {
 		controlAttitude();
 		controlRate();
@@ -69,39 +65,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; // up pitch stick means tilt clockwise in frd
+		ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;

 	} 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;

 	} 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()) {
@@ -118,18 +113,17 @@ void controlAttitude() {
 		return;
 	}

-	const Vector up(0, 0, 1);
-	Vector upActual = Quaternion::rotateVector(up, attitude);
-	Vector upTarget = Quaternion::rotateVector(up, attitudeTarget);
+	const Vector up(0, 0, -1);
+	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);

 	if (yawMode == YAW) {
-		float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
-		ratesTarget.z = yawPID.update(yawError, dt);
+		ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);
 	}
 }

@@ -155,10 +149,10 @@ void controlTorque() {
 		return;
 	}

-	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;
-	motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;
-	motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
-	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
+	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
+	motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
+	motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;
+	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;

 	motors[0] = constrain(motors[0], 0, 1);
 	motors[1] = constrain(motors[1], 0, 1);
@@ -166,6 +160,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);
@@ -16,6 +16,7 @@ LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
 void estimate() {
 	applyGyro();
 	applyAcc();
+	signalizeHorizontality();
 }

 void applyGyro() {
@@ -23,7 +24,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 +36,15 @@ 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();
+}
+
+void signalizeHorizontality() {
+	float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
+	setLED(angle < radians(15));
 }
--- a/flix/failsafe.ino
+++ b/flix/failsafe.ino
@@ -1,26 +0,0 @@
-// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-// 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 float controlTime;
-
-// RC loss failsafe
-void failsafe() {
-	if (t - controlTime > RC_LOSS_TIMEOUT) {
-		descend();
-	}
-}
-
-// Smooth descend on RC lost
-void descend() {
-	mode = STAB;
-	controlRoll = 0;
-	controlPitch = 0;
-	controlYaw = 0;
-	controlThrottle -= dt / DESCEND_TIME;
-	if (controlThrottle < 0) controlThrottle = 0;
-}
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -5,34 +5,50 @@

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

 #define SERIAL_BAUDRATE 115200
-#define WIFI_ENABLED 1
+
+#define WIFI_ENABLED 0
+
+#define RC_CHANNELS 6
+#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 loopFreq; // loop frequency, Hz
+int16_t channels[16]; // raw rc channels
+float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
 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 +58,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
@@ -5,13 +5,14 @@

 #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() {
@@ -23,15 +24,14 @@ void setupIMU() {
 			delay(1000);
 		}
 	}
-	configureIMU();
 	calibrateGyro();
 }

 void configureIMU() {
 	IMU.setAccelRange(IMU.ACCEL_RANGE_4G);
 	IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
-	IMU.setDLPF(IMU.DLPF_MAX);
-	IMU.setRate(IMU.RATE_1KHZ_APPROX);
+	IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ);
+	IMU.setSrd(0); // set sample rate to 1000 Hz
 }

 void readIMU() {
@@ -41,16 +41,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,41 +56,36 @@ void calibrateGyro() {
 	}
 	gyroBias = gyroBias / samples;

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

 void calibrateAccel() {
 	Serial.println("Calibrating accelerometer");
 	IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode
+	IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_20HZ);
+	IMU.setSrd(19);

 	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();
 }

 void calibrateAccelOnce() {
-	const int samples = 1000;
+	const int samples = 100;
 	static Vector accMax(-INFINITY, -INFINITY, -INFINITY);
 	static Vector accMin(INFINITY, INFINITY, INFINITY);

@@ -121,18 +106,16 @@ 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() {
+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() {
-	Serial.printf("model: %s\n", IMU.getModel());
-	Serial.printf("who am I: 0x%02X\n", IMU.whoAmI());
-}
--- a/flix/led.ino
+++ b/flix/led.ino
@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix

-// Board's LED control
+// Main LED control

 #define BLINK_PERIOD 500000

@@ -14,10 +14,9 @@ void setupLED() {
 }

 void setLED(bool on) {
-	static bool state = false;
-	if (on == state) {
-		return; // don't call digitalWrite if the state is the same
-	}
 	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);
 	}
@@ -41,11 +39,13 @@ void sendMavlink() {

 		const float zeroQuat[] = {0, 0, 0, 0};
 		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, attitude.w, attitude.x, attitude.y, attitude.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 +54,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,20 +82,18 @@ 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

-	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;
 	}
 }

--- a/flix/motors.ino
+++ b/flix/motors.ino
@@ -1,61 +1,69 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix

-// Motors output control using MOSFETs
+// Motors output control
+// Motor: 8520 3.7V
+// ESC: KINGDUO Micro Mini 4A 1S Brushed Esc 3.6-6V

-#include "util.h"
-
-#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 MOTOR_0_PIN 12
+#define MOTOR_1_PIN 13
+#define MOTOR_2_PIN 14
+#define MOTOR_3_PIN 15
+#define PWM_FREQUENCY 200
+#define PWM_RESOLUTION 8
+#define PWM_NEUTRAL 1500
+#define PWM_MIN 1600
+#define PWM_MAX 2300
+#define PWM_REVERSE_MIN 1400
+#define PWM_REVERSE_MAX 700

 void setupMotors() {
 	Serial.println("Setup Motors");

-	// configure pins
-	ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
-	ledcAttach(MOTOR_1_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
-	ledcAttach(MOTOR_2_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
-	ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
+	// configure PWM channels
+	ledcSetup(0, PWM_FREQUENCY, PWM_RESOLUTION);
+	ledcSetup(1, PWM_FREQUENCY, PWM_RESOLUTION);
+	ledcSetup(2, PWM_FREQUENCY, PWM_RESOLUTION);
+	ledcSetup(3, PWM_FREQUENCY, PWM_RESOLUTION);
+
+	// attach channels to motor pins
+	ledcAttachPin(MOTOR_0_PIN, 0);
+	ledcAttachPin(MOTOR_1_PIN, 1);
+	ledcAttachPin(MOTOR_2_PIN, 2);
+	ledcAttachPin(MOTOR_3_PIN, 3);

 	sendMotors();
 	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);
+uint16_t getPWM(float val, int n) {
+	if (val == 0) {
+		return PWM_NEUTRAL;
+	} else if (val > 0) {
+		return mapff(val, 0, 1, PWM_MIN, PWM_MAX);
+	} else {
+		return mapff(val, 0, -1, PWM_REVERSE_MIN, PWM_REVERSE_MAX);
+	}
+}
+
+uint8_t pwmToDutyCycle(uint16_t pwm) {
+	return map(pwm, 0, 1000000 / PWM_FREQUENCY, 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]));
+	ledcWrite(0, pwmToDutyCycle(getPWM(motors[0], 0)));
+	ledcWrite(1, pwmToDutyCycle(getPWM(motors[1], 1)));
+	ledcWrite(2, pwmToDutyCycle(getPWM(motors[2], 2)));
+	ledcWrite(3, pwmToDutyCycle(getPWM(motors[3], 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");
+void fullMotorTest(int n, bool reverse) {
+	printf("Full test for motor %d\n", n);
+	for (int pwm = PWM_NEUTRAL; pwm <= 2300 && pwm >= 700; pwm += reverse ? -100 : 100) {
+		printf("Motor %d: %d\n", n, pwm);
+		ledcWrite(n, pwmToDutyCycle(pwm));
+		delay(3000);
+	}
+	printf("Motor %d: %d\n", n, PWM_NEUTRAL);
+	ledcWrite(n, pwmToDutyCycle(PWM_NEUTRAL));
 }
--- 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;
@@ -99,7 +75,7 @@ public:
 		float sqz = z * z;
 		float sqw = w * w;
 		// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
-		float sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
+		float sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
 		if (sarg <= -0.99999) {
 			euler.x = 0;
 			euler.y = -0.5 * PI;
@@ -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,51 @@
 // 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};
+int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};

-// Channels mapping:
-int rollChannel = 0;
-int pitchChannel = 1;
-int throttleChannel = 2;
-int yawChannel = 3;
-int armedChannel = 4;
-int modeChannel = 5;
+SBUS RC(Serial2);

 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;
 	}
-	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;
@@ -14,16 +12,16 @@ void step() {
 		dt = 0; // assume dt to be zero on first step and on reset
 	}

-	computeLoopRate();
+	computeLoopFreq();
 }

-void computeLoopRate() {
+void computeLoopFreq() {
 	static float windowStart = 0;
-	static uint32_t rate = 0;
-	rate++;
+	static uint32_t freq = 0;
+	freq++;
 	if (t - windowStart >= 1) { // 1 second window
-		loopRate = rate;
+		loopFreq = freq;
 		windowStart = t;
-		rate = 0;
+		freq = 0;
 	}
 }
--- 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;
 }
@@ -19,6 +15,14 @@ float mapff(float x, float in_min, float in_max, float out_min, float out_max) {
 	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }

+int8_t sign(float x) {
+	return (x > 0) - (x < 0);
+}
+
+float randomFloat(float min, float max) {
+	return min + (max - min) * (float)rand() / RAND_MAX;
+}
+
 // Wrap angle to [-PI, PI)
 float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);
@@ -30,7 +34,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
@@ -7,7 +7,6 @@

 #include <cmath>
 #include <string>
-#include <stdint.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <sys/poll.h>
@@ -99,7 +98,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
@@ -117,7 +116,7 @@ public:
 	int read() {
 		if (available()) {
 			char c;
-			size_t res = ::read(STDIN_FILENO, &c, 1); // use raw read to avoid C++ buffering
+			::read(STDIN_FILENO, &c, 1); // use raw read to avoid C++ buffering
 			// https://stackoverflow.com/questions/45238997/does-getchar-function-has-its-own-buffer-to-store-remaining-input
 			return c;
 		}
@@ -133,12 +132,8 @@ 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;

 unsigned long micros() {
-	return __micros + __resetTime; // keep the time monotonic
+	return __micros;
 }
--- a/gazebo/CMakeLists.txt
+++ b/gazebo/CMakeLists.txt
@@ -15,7 +15,6 @@ set(CMAKE_BUILD_TYPE RelWithDebInfo)
 add_library(flix SHARED simulator.cpp)
 target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})
 target_include_directories(flix PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_compile_options(flix PRIVATE -Wno-address-of-packed-member) # disable unneeded mavlink warnings

 # Include dir for MAVLink-Arduino library
 target_include_directories(flix PUBLIC $ENV{HOME}/Arduino/libraries/MAVLink)
--- a/gazebo/SBUS.h
+++ b/gazebo/SBUS.h
@@ -12,14 +12,12 @@ struct SBUSData {
 class SBUS {
 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,44 +10,50 @@
 #include "Arduino.h"
 #include "wifi.h"

+#define RC_CHANNELS 6
+
+#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 loopFreq;
 float motors[4];
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
+int16_t channels[16]; // raw rc channels
+float controls[RC_CHANNELS];
 Vector acc;
 Vector gyro;
 Vector rates;
 Quaternion attitude;

 // declarations
-void computeLoopRate();
+void computeLoopFreq();
 void applyGyro();
 void applyAcc();
+void signalizeHorizontality();
 void control();
 void interpretRC();
 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);
 void receiveMavlink();
 void handleMavlink(const void *_msg);
-void failsafe();
-void descend();
-inline Quaternion fluToFrd(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 printIMUInfo() {};
+void fullMotorTest(int n, bool reverse) { printf("Skip full motor test\n"); };
+void sendMotors() {};
+void printIMUCal() { printf("cal: N/A\n"); };
--- a/gazebo/flix.world
+++ b/gazebo/flix.world
@@ -9,7 +9,7 @@
 		</scene>
 		<gui>
 			<camera name="user_camera">
-				<pose>-2.3 0 1.1 0 0.3 0</pose>
+				<pose>-2 -0.3 1.5 0 0.5 0.1</pose>
 			</camera>
 		</gui>
 		<physics type="ode">
@@ -23,7 +23,7 @@
 		</include>
 		<include>
 			<uri>model://flix</uri>
-			<pose>0 0 0.3 0 0 0</pose>
+			<pose>0 0 0.2 0 0 0</pose>
 		</include>
 	</world>
 </sdf>
--- 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/flix0.dae
+++ b/gazebo/models/flix/flix0.dae
--- 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>
@@ -14,58 +13,57 @@
 			<collision name="collision">
 				<geometry>
 					<box>
-						<size>0.095 0.095 0.0276</size>
+						<size>0.125711 0.125711 0.022</size>
 					</box>
 				</geometry>
 			</collision>
+			<visual name="body">
+				<geometry>
+					<mesh><uri>model://flix/flix.dae</uri></mesh>
+				</geometry>
+			</visual>
 			<sensor name="imu" type="imu">
 				<always_on>1</always_on>
 				<visualize>1</visualize>
 				<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>
-			<visual name="body">
-				<geometry>
-					<mesh><uri>model://flix/flix.stl</uri></mesh>
-				</geometry>
-				<material>
-					<ambient>0.5 0.5 0.6 1</ambient>
-					<diffuse>0.5 0.5 0.6 1</diffuse>
-					<specular>0 0 0 1</specular>
-					<emissive>0 0 0 1</emissive>
-				</material>
-			</visual>
-			<visual name="prop0"><!-- rear left -->
-				<geometry><cylinder><radius>0.0275</radius><length>0</length></cylinder></geometry>
-				<pose>-0.04243 0.04243 0.0142 0 0 0</pose>
-				<material><ambient>0.8 0.3 0.3 0.5</ambient><diffuse>0.8 0.3 0.3 0.5</diffuse></material>
-			</visual>
-			<visual name="prop1"><!-- rear right -->
-				<geometry><cylinder><radius>0.0275</radius><length>0</length></cylinder></geometry>
-				<pose>-0.04243 -0.04243 0.0142 0 0 0</pose>
-				<material><ambient>0.8 0.3 0.3 0.5</ambient><diffuse>0.8 0.3 0.3 0.5</diffuse></material>
-			</visual>
-			<visual name="prop2"><!-- front right -->
-				<geometry><cylinder><radius>0.0275</radius><length>0</length></cylinder></geometry>
-				<pose>0.04243 -0.04243 0.0142 0 0 0</pose>
-				<material><ambient>1 1 1 0.5</ambient><diffuse>1 1 1 0.5</diffuse></material>
-			</visual>
-			<visual name="prop3"><!-- front left -->
-				<geometry><cylinder><radius>0.0275</radius><length>0</length></cylinder></geometry>
-				<pose>0.04243 0.04243 0.0142 0 0 0</pose>
-				<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/models/flix/flix.stl
+++ b/gazebo/models/flix/flix.stl
--- a/gazebo/simulator.cpp
+++ b/gazebo/simulator.cpp
@@ -17,15 +17,15 @@

 #include "Arduino.h"
 #include "flix.h"
+#include "util.h"
+#include "util.ino"
 #include "rc.ino"
 #include "time.ino"
-#include "motors.ino"
 #include "estimate.ino"
 #include "control.ino"
 #include "log.ino"
 #include "cli.ino"
 #include "mavlink.ino"
-#include "failsafe.ino"
 #include "lpf.h"

 using ignition::math::Vector3d;
@@ -47,8 +47,8 @@ public:
 		this->model = _parent;
 		this->body = this->model->GetLink("body");
 		this->imu = dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor(model->GetScopedName(true) + "::body::imu")); // default::flix::body::imu
-		this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&ModelFlix::OnUpdate, this));
-		this->resetConnection = event::Events::ConnectWorldReset(std::bind(&ModelFlix::OnReset, this));
+		this->updateConnection = event::Events::ConnectWorldUpdateBegin(bind(&ModelFlix::OnUpdate, this));
+		this->resetConnection = event::Events::ConnectWorldReset(bind(&ModelFlix::OnReset, this));
 		initNode();
 		Serial.begin(0);
 		gzmsg << "Flix plugin loaded" << endl;
@@ -56,7 +56,6 @@ public:

 	void OnReset() {
 		attitude = Quaternion(); // reset estimated attitude
-		__resetTime += __micros;
 		gzmsg << "Flix plugin reset" << endl;
 	}

@@ -64,22 +63,22 @@ public:
 		__micros = model->GetWorld()->SimTime().Double() * 1000000;
 		step();

-		// read virtual imu
-		gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());
-		acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));
+		// read imu
+		gyro = flu2frd(imu->AngularVelocity());
+		acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));

 		// 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();

 		// correct yaw to the actual yaw
-		attitude.setYaw(this->model->WorldPose().Yaw());
+		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/util.h
+++ b/gazebo/util.h
@@ -0,0 +1,14 @@
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+
+using ignition::math::Vector3d;
+using ignition::math::Pose3d;
+
+Pose3d flu2frd(const Pose3d& p) {
+	return ignition::math::Pose3d(p.Pos().X(), -p.Pos().Y(), -p.Pos().Z(),
+	                              p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().Z());
+}
+
+Vector flu2frd(const Vector3d& v) {
+	return Vector(v.X(), -v.Y(), -v.Z());
+}
--- a/gazebo/wifi.h
+++ b/gazebo/wifi.h
@@ -11,29 +11,29 @@
 #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;

 void setupWiFi() {
 	wifiSocket = socket(AF_INET, SOCK_DGRAM, 0);
-	sockaddr_in addr; // local address
+	sockaddr_in addr;
 	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) {
 	if (wifiSocket == 0) setupWiFi();
-	sockaddr_in addr; // remote address
+	sockaddr_in addr;
 	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));
 }

--- a/tools/check_c_cpp_properties.py
+++ b/tools/check_c_cpp_properties.py
@@ -1,61 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import platform
-import json
-import re
-
-path = '.vscode/c_cpp_properties.json' if os.path.exists('./.vscode/c_cpp_properties.json') else '../.vscode/c_cpp_properties.json'
-txt = open(path).read()
-# remove comments
-txt = re.sub(r'//.*', '', txt)
-props = json.loads(txt)
-
-env = props.get('env', {})
-env['workspaceFolder'] = '.'
-
-def check_path(s):
-    source = s
-    # replace env
-    for key, value in env.items():
-        s = s.replace('${' + key + '}', value)
-    # remove globs from the end
-    if s.endswith('**'):
-        s = s[:-2]
-    elif s.endswith('*'):
-        s = s[:-1]
-    s = os.path.expanduser(s)
-    if s == '':
-        s = '.'
-    print('Check', source, '->', s)
-    assert os.path.exists(s), 'Path does not exist: ' + s
-
-# linux, macos or windows:
-platform = platform.system().lower()
-if platform == 'darwin':
-    platform = 'mac'
-elif platform == 'windows':
-    platform = 'win32'
-elif platform == 'linux':
-    pass
-else:
-    raise Exception('Unknown platform: ' + platform)
-
-for configuration in props['configurations']:
-    if platform not in configuration['name'].lower():
-        print('Skip configuration', configuration['name'])
-        continue
-
-    print('Check configuration', configuration['name'])
-
-    for include_path in configuration.get('includePath', []):
-        check_path(include_path)
-
-    for forced_include in configuration.get('forcedInclude', []):
-        check_path(forced_include)
-
-    for browse in configuration.get('browse', {}).get('path', []):
-        check_path(browse)
-
-    if 'compilerPath' in configuration:
-        check_path(configuration['compilerPath'])
--- a/tools/csv_to_mcap.py
+++ b/tools/csv_to_mcap.py
@@ -1,46 +0,0 @@
-#!/usr/bin/env python3
-
-"""Convert CSV log file to MCAP file.
-
-Usage:
-  csv_to_mcap.py <csv_file> [<mcap_file>]
-"""
-
-import csv
-import json
-import docopt
-from mcap.writer import Writer
-
-args = docopt.docopt(__doc__)
-input_file = args['<csv_file>']
-output_file = args['<mcap_file>'] or input_file.replace('.csv', '.mcap')
-if input_file == output_file:
-    raise ValueError('Input and output files are the same')
-
-csv_file = open(input_file, 'r')
-csv_reader = csv.reader(csv_file, delimiter=',')
-header = next(csv_reader)
-
-mcap_file = open(output_file, 'wb')
-writer = Writer(mcap_file)
-writer.start()
-
-properties = {key: {'type': 'number'} for key in header}
-schema_id = writer.register_schema(
-    name="state",
-    encoding="jsonschema",
-    data=json.dumps({"type": "object", "properties": properties}).encode(),
-)
-
-channel_id = writer.register_channel(
-    schema_id=schema_id,
-    topic="state",
-    message_encoding="json",
-)
-
-for row in csv_reader:
-    data = {key: float(value) for key, value in zip(header, row)}
-    timestamp = round(float(row[0]) * 1e9)
-    writer.add_message(channel_id=channel_id, log_time=timestamp, data=json.dumps(data).encode(), publish_time=timestamp,)
-
-writer.finish()
--- a/tools/csv_to_ulog/CMakeLists.txt
+++ b/tools/csv_to_ulog/CMakeLists.txt
@@ -1,23 +0,0 @@
-cmake_minimum_required(VERSION 3.15)
-project(csv_to_ulog)
-include(FetchContent)
-set(CMAKE_CXX_STANDARD 17)
-
-FetchContent_Declare(
-  ulog_cpp
-  GIT_REPOSITORY https://github.com/PX4/ulog_cpp.git
-  GIT_TAG cf24ec6
-)
-
-FetchContent_Declare(
-  rapidcsv
-  GIT_REPOSITORY https://github.com/d99kris/rapidcsv.git
-  GIT_TAG v8.82
-)
-
-FetchContent_MakeAvailable(ulog_cpp)
-FetchContent_MakeAvailable(rapidcsv)
-
-add_executable(csv_to_ulog csv_to_ulog.cpp)
-target_link_libraries(csv_to_ulog PUBLIC ulog_cpp::ulog_cpp)
-target_include_directories(csv_to_ulog PUBLIC ${rapidcsv_SOURCE_DIR}/src)
--- a/tools/csv_to_ulog/README.md
+++ b/tools/csv_to_ulog/README.md
@@ -1,20 +0,0 @@
-# csv_to_ulog
-
-Tool for converting CSV flight logs to ULog format so they can be analyzed using [FlightPlot](https://github.com/PX4/FlightPlot) software.
-
-To build, go to the `<flix>/tools/csv_to_ulog` directory and run:
-
-```bash
-mkdir build
-cd build
-cmake ..
-make
-```
-
-Convert a CSV file to ULog:
-
-```bash
-./csv_to_ulog log_file.csv
-```
-
-ULog file will be saved in the same directory.
--- a/tools/csv_to_ulog/csv_to_ulog.cpp
+++ b/tools/csv_to_ulog/csv_to_ulog.cpp
@@ -1,72 +0,0 @@
-// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-// Tool for conversion CSV log file to ULog format
-
-#include <ulog_cpp/simple_writer.hpp>
-#include <rapidcsv.h>
-#include <vector>
-#include <string>
-#include <filesystem>
-
-using std::vector;
-using std::string;
-
-struct Data {
-	uint64_t timestamp;
-	float values[30];
-};
-
-int main(int argc, char** argv)
-{
-	if (argc < 2) {
-		printf("Usage: %s file.csv [file.ulg]\n", argv[0]);
-		return -1;
-	}
-
-	// check input file exists
-	if (!std::filesystem::exists(argv[1])) {
-		printf("Input file \"%s\" does not exist\n", argv[1]);
-		return -1;
-	}
-
-	// open csv file
-	rapidcsv::Document csv(argv[1]);
-	auto columns = csv.GetColumnNames();
-
-
-	// open ulog file
-	string ulog_file;
-	if (argc < 3) {
-		ulog_file = std::filesystem::path(argv[1]).replace_extension(".ulg").string();
-	} else {
-		ulog_file = argv[2];
-	}
-	ulog_cpp::SimpleWriter writer(ulog_file.c_str(), 0);
-	writer.writeInfo("sys_name", "flix");
-
-	vector<ulog_cpp::Field> fields;
-	fields.push_back(ulog_cpp::Field("uint64_t", "timestamp"));
-	columns.erase(columns.begin()); // remove timestamp column
-	for (auto& column : columns) {
-		// Valid field name for ULog: [a-z0-9_]+
-		std::replace(column.begin(), column.end(), '.', '_'); // replace dots with underscores
-		std::transform(column.begin(), column.end(), column.begin(), [](unsigned char c) { return std::tolower(c); }); // lowercase column name
-		fields.push_back(ulog_cpp::Field("float", column));
-	}
-
-	const char* msg_name = "state";
-	writer.writeMessageFormat(msg_name, fields);
-	writer.headerComplete();
-
-	const uint16_t msg_id = writer.writeAddLoggedMessage(msg_name);
-
-	for (size_t i = 0; i < csv.GetRowCount(); i++) {
-		Data data;
-		data.timestamp = csv.GetCell<float>(0, i) * 1000000.0;
-		for (size_t j = 1; j <= columns.size(); j++) {
-			data.values[j - 1] = csv.GetCell<float>(j, i);
-		}
-		writer.writeData(msg_id, data);
-	}
-}
--- a/tools/requirements.txt
+++ b/tools/requirements.txt
@@ -1,3 +1,2 @@
 docopt
 matplotlib
-mcap
Author	SHA1	Message	Date
Oleg Kalachev	d0eb2dd9ba	Merge branch 'master' into run-sim	2024-05-24 22:35:36 +03:00
Oleg Kalachev	44ed3cf42c	Merge branch 'master' into run-sim	2024-01-31 17:23:07 +03:00
Oleg Kalachev	aa02e6344b	Test simulator run in CI	2024-01-25 23:53:08 +03:00