Fix rates, acc and gyro coordinate frame in mavlink

All of them should be in frd.
Get rid of fluToFrd function - there is no big need for it.

.editorconfig

@@ -4,7 +4,7 @@ root = true
 end_of_line = lf
 insert_final_newline = true
 
-[*.{ino,cpp,c,h,hpp,sdf,world}]
+[*.{ino,cpp,c,h,hpp,sdf,world,json}]
 charset = utf-8
 indent_style = tab
 tab_width = 4

.github/workflows/build.yml

@@ -7,69 +7,77 @@ on:
     branches: [ master ]
 
 jobs:
-  build:
+  build_linux:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@v3
+    - uses: actions/checkout@v4
     - 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@v3
+    - uses: actions/checkout@v4
     - 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@v3
+    - uses: actions/checkout@v4
     - 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-latest
+    runs-on: ubuntu-22.04
     steps:
     - name: Install Arduino CLI
       uses: arduino/setup-arduino-cli@v1.1.1
-    - uses: actions/checkout@v3
+    - uses: actions/checkout@v4
     - 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@v3
+    - uses: actions/upload-artifact@v4
       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@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
+#  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

.github/workflows/tools.yml

@@ -0,0 +1,33 @@
+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

.gitignore

@@ -3,3 +3,10 @@
 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

.vscode/c_cpp_properties.json

@@ -0,0 +1,144 @@
+{
+	"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
+}

.vscode/extensions.json

@@ -0,0 +1,9 @@
+{
+	// 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": []
+}

.vscode/intellisense.h

@@ -0,0 +1,5 @@
+#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

.vscode/launch.json

@@ -0,0 +1,25 @@
+{
+	"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,
+		},
+	]
+}

.vscode/settings.json

@@ -0,0 +1,13 @@
+{
+	"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"
+}

.vscode/tasks.json

@@ -0,0 +1,31 @@
+{
+	"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"
+}

Makefile

@@ -13,10 +13,10 @@ monitor:
 
 dependencies .dependencies:
 	arduino-cli core update-index --config-file arduino-cli.yaml
-	arduino-cli core install esp32:esp32@2.0.11 --config-file arduino-cli.yaml
+	arduino-cli core install esp32:esp32@3.2.0 --config-file arduino-cli.yaml
 	arduino-cli lib update-index
 	arduino-cli lib install "FlixPeriph"
-	arduino-cli lib install "MAVLink"@2.0.1
+	arduino-cli lib install "MAVLink"@2.0.16
 	touch .dependencies
 
 gazebo/build cmake: gazebo/CMakeLists.txt

README.md

@@ -1,69 +1,7 @@
-# flix
+# Flix
 
-**flix** (*flight + X*) — making an open source ESP32-based quadcopter from scratch.
+Minimal **Flix** quadcopter firmware implementation for the [book](https://quadcopter.dev).
 
-<img src="docs/img/flix.jpg" width=500 alt="Flix quadcopter">
+See the full code and documentation in the main branch: https://github.com/okalachev/flix.
 
-## 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/.
+<img src="docs/img/flix1.jpg" width=500 alt="Flix quadcopter">

docs/assets/washer-m3.step

@@ -0,0 +1,200 @@
+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;

docs/build.md

@@ -11,10 +11,12 @@ 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=/usr/local/bin sh
+   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh
    ```
 
 2. Install Gazebo 11:
@@ -78,22 +80,41 @@ cd flix
    make simulator
    ```
 
-### Flight
+### Setup and flight
 
-Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* setting enabled) to control the drone. *Auto-Center Throttle* setting **should be disabled**.
+#### 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!
 
 ## Firmware
 
 ### Arduino IDE (Windows, Linux, macOS)
 
 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
-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.
+2. Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).
+3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
+4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
+   * `FlixPeriph`, the latest version.
+   * `MAVLink`, version 2.0.16.
+5. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
+6. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
+7. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
+8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
 
 ### Command line (Windows, Linux, macOS)
 
@@ -119,12 +140,36 @@ Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* set
 
 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`).

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.
-* `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.
+* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].
+* `motors` *(float[])* — motor outputs, normalized to [0, 1] range; reverse rotation is possible.
 
 ## Source files
 

docs/img/schematics1.svg

@@ -0,0 +1,256 @@
+<?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>

docs/log.md

@@ -0,0 +1,72 @@
+# 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.

flix/cli.ino

@@ -6,8 +6,9 @@
 #include "pid.h"
 #include "vector.h"
 
-extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
-extern LowPassFilter<Vector> ratesFilter;
+extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
+extern float loopRate;
+extern uint16_t channels[16];
 
 const char* motd =
 "\nWelcome to\n"
@@ -19,71 +20,42 @@ 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 data\n"
+"mot - show motor output\n"
 "log - dump in-RAM log\n"
 "cr - calibrate RC\n"
 "cg - calibrate gyro\n"
 "ca - calibrate accel\n"
-"mfr, mfl, mrr, mrl - test appropriate motor\n"
-"fullmot <n> - full motor test\n"
+"mfr, mfl, mrr, mrl - test motor (remove props)\n"
 "reset - reset drone's state\n";
 
-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) {
+void doCommand(const String& command) {
 	if (command == "help" || command == "motd") {
 		Serial.println(motd);
-	} else if (command == "show") {
-		showTable();
 	} else if (command == "ps") {
-		Vector a = attitude.toEulerZYX();
+		Vector a = attitude.toEuler();
 		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);
-		printIMUCal();
-		Serial.printf("frequency: %f\n", loopFreq);
+		printIMUCalibration();
+		Serial.printf("rate: %f\n", loopRate);
 	} else if (command == "rc") {
-		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());
+		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());
 	} else if (command == "mot") {
-		Serial.printf("MOTOR front-right %f front-left %f rear-right %f rear-left %f\n",
+		Serial.printf("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();
@@ -94,77 +66,38 @@ void doCommand(String& command, String& value) {
 	} else if (command == "ca") {
 		calibrateAccel();
 	} else if (command == "mfr") {
-		cliTestMotor(MOTOR_FRONT_RIGHT);
+		testMotor(MOTOR_FRONT_RIGHT);
 	} else if (command == "mfl") {
-		cliTestMotor(MOTOR_FRONT_LEFT);
+		testMotor(MOTOR_FRONT_LEFT);
 	} else if (command == "mrr") {
-		cliTestMotor(MOTOR_REAR_RIGHT);
+		testMotor(MOTOR_REAR_RIGHT);
 	} else if (command == "mrl") {
-		cliTestMotor(MOTOR_REAR_LEFT);
-	} else if (command == "fullmot") {
-		fullMotorTest(value.toInt(), false);
+		testMotor(MOTOR_REAR_LEFT);
 	} else if (command == "reset") {
 		attitude = Quaternion();
+	} else if (command == "") {
+		// do nothing
 	} else {
-		float val = value.toFloat();
-		// TODO: on error returns 0, check invalid value
-
-		for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
-			if (command == params[i].name) {
-				*params[i].value = val;
-				if (params[i].value2 != nullptr) *params[i].value2 = val;
-				Serial.print(command);
-				Serial.print(" = ");
-				Serial.println(val, 4);
-				return;
-			}
-		}
 		Serial.println("Invalid command: " + command);
 	}
 }
 
-void 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() {
+void handleInput() {
 	static bool showMotd = true;
-	static String command;
-	static String value;
-	static bool parsingCommand = true;
+	static String input;
 
 	if (showMotd) {
-		Serial.println(motd);
+		Serial.printf("%s\n", motd);
 		showMotd = false;
 	}
 
 	while (Serial.available()) {
 		char c = Serial.read();
 		if (c == '\n') {
-			parsingCommand = true;
-			if (!command.isEmpty()) {
-				doCommand(command, value);
-			}
-			command.clear();
-			value.clear();
-		} else if (c == ' ') {
-			parsingCommand = false;
+			doCommand(input);
+			input.clear();
 		} else {
-			(parsingCommand ? command : value) += c;
+			input += c;
 		}
 	}
 }

flix/control.ino

@@ -7,6 +7,7 @@
 #include "quaternion.h"
 #include "pid.h"
 #include "lpf.h"
+#include "util.h"
 
 #define PITCHRATE_P 0.05
 #define PITCHRATE_I 0.2
@@ -29,8 +30,8 @@
 #define YAW_P 3
 #define PITCHRATE_MAX radians(360)
 #define ROLLRATE_MAX radians(360)
-#define YAWRATE_MAX radians(360)
-#define MAX_TILT radians(30)
+#define YAWRATE_MAX radians(300)
+#define TILT_MAX radians(30)
 
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
@@ -50,8 +51,11 @@ 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();
@@ -65,38 +69,39 @@ void control() {
 }
 
 void interpretRC() {
-	armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5;
+	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
+
 
 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controls[RC_CHANNEL_MODE] < 0.25) {
+	if (controlMode < 0.25) {
 		mode = STAB;
-	} else if (controls[RC_CHANNEL_MODE] < 0.75) {
+	} else if (controlMode < 0.75) {
 		mode = STAB;
 	} else {
 		mode = STAB;
 	}
 
-	thrustTarget = controls[RC_CHANNEL_THROTTLE];
+	thrustTarget = controlThrottle;
 
 	if (mode == ACRO) {
 		yawMode = YAW_RATE;
-		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;
+		ratesTarget.x = controlRoll * ROLLRATE_MAX;
+		ratesTarget.y = controlPitch * PITCHRATE_MAX;
+		ratesTarget.z = -controlYaw * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
 
 	} else if (mode == STAB) {
-		yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
+		yawMode = controlYaw == 0 ? YAW : YAW_RATE;
 
-		attitudeTarget = Quaternion::fromEulerZYX(Vector(
-			controls[RC_CHANNEL_ROLL] * MAX_TILT,
-			-controls[RC_CHANNEL_PITCH] * MAX_TILT,
+		attitudeTarget = Quaternion::fromEuler(Vector(
+			controlRoll * TILT_MAX,
+			controlPitch * TILT_MAX,
 			attitudeTarget.getYaw()));
-		ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
+		ratesTarget.z = -controlYaw * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
 
 	} else if (mode == MANUAL) {
 		// passthrough mode
 		yawMode = YAW_RATE;
-		torqueTarget = Vector(controls[RC_CHANNEL_ROLL], -controls[RC_CHANNEL_PITCH], controls[RC_CHANNEL_YAW]) * 0.01;
+		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 
 	if (yawMode == YAW_RATE || !motorsActive()) {
@@ -113,17 +118,18 @@ void controlAttitude() {
 		return;
 	}
 
-	const Vector up(0, 0, -1);
-	Vector upActual = attitude.rotate(up);
-	Vector upTarget = attitudeTarget.rotate(up);
+	const Vector up(0, 0, 1);
+	Vector upActual = Quaternion::rotateVector(up, attitude);
+	Vector upTarget = Quaternion::rotateVector(up, attitudeTarget);
 
-	Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);
+	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
 	ratesTarget.x = rollPID.update(error.x, dt);
 	ratesTarget.y = pitchPID.update(error.y, dt);
 
 	if (yawMode == YAW) {
-		ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);
+		float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
+		ratesTarget.z = yawPID.update(yawError, dt);
 	}
 }
 
@@ -149,10 +155,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);
@@ -160,10 +166,6 @@ void controlTorque() {
 	motors[3] = constrain(motors[3], 0, 1);
 }
 
-bool motorsActive() {
-	return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
-}
-
 const char* getModeName() {
 	switch (mode) {
 		case MANUAL: return "MANUAL";

flix/estimate.ino

@@ -6,9 +6,9 @@
 #include "quaternion.h"
 #include "vector.h"
 #include "lpf.h"
+#include "util.h"
 
-#define ONE_G 9.807f
-#define WEIGHT_ACC 0.5f
+#define WEIGHT_ACC 0.003
 #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
 LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
@@ -16,7 +16,6 @@ LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
 void estimate() {
 	applyGyro();
 	applyAcc();
-	signalizeHorizontality();
 }
 
 void applyGyro() {
@@ -24,8 +23,7 @@ void applyGyro() {
 	rates = ratesFilter.update(gyro);
 
 	// apply rates to attitude
-	attitude *= Quaternion::fromAngularRates(rates * dt);
-	attitude.normalize();
+	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));
 }
 
 void applyAcc() {
@@ -36,15 +34,9 @@ void applyAcc() {
 	if (!landed) return;
 
 	// calculate accelerometer correction
-	Vector up = attitude.rotate(Vector(0, 0, -1));
-	Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;
+	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+	Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
 
 	// apply 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));
+	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
 }

flix/failsafe.ino

@@ -0,0 +1,26 @@
+// 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;
+}

flix/flix.ino

@@ -5,50 +5,34 @@
 
 #include "vector.h"
 #include "quaternion.h"
+#include "util.h"
 
 #define SERIAL_BAUDRATE 115200
-
-#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
+#define WIFI_ENABLED 1
 
 float t = NAN; // current step time, s
 float dt; // time delta from previous step, s
-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)
+float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]
 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 [-1..1]
+float motors[4]; // normalized motors thrust in range [0..1]
 
 void setup() {
 	Serial.begin(SERIAL_BAUDRATE);
-	Serial.println("Initializing flix");
+	Serial.println("Initializing flix\n");
 	disableBrownOut();
 	setupLED();
 	setupMotors();
 	setLED(true);
-#if WIFI_ENABLED == 1
+#if WIFI_ENABLED
 	setupWiFi();
 #endif
 	setupIMU();
 	setupRC();
-
 	setLED(false);
-	Serial.println("Initializing complete");
+	Serial.println("Initializing complete\n");
 }
 
 void loop() {
@@ -58,8 +42,8 @@ void loop() {
 	estimate();
 	control();
 	sendMotors();
-	parseInput();
-#if WIFI_ENABLED == 1
+	handleInput();
+#if WIFI_ENABLED
 	processMavlink();
 #endif
 	logData();

flix/imu.ino

@@ -5,14 +5,13 @@
 
 #include <SPI.h>
 #include <MPU9250.h>
-
-#define ONE_G 9.80665
-
-// NOTE: use 'ca' command to calibrate the accelerometer and put the values here
-Vector accBias(0, 0, 0);
-Vector accScale(1, 1, 1);
+#include "util.h"
 
 MPU9250 IMU(SPI);
+
+// NOTE: use 'ca' command to calibrate the accelerometer and put the values here
+Vector accBias;
+Vector accScale(1, 1, 1);
 Vector gyroBias;
 
 void setupIMU() {
@@ -24,14 +23,15 @@ void setupIMU() {
 			delay(1000);
 		}
 	}
+	configureIMU();
 	calibrateGyro();
 }
 
 void configureIMU() {
 	IMU.setAccelRange(IMU.ACCEL_RANGE_4G);
 	IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
-	IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ);
-	IMU.setSrd(0); // set sample rate to 1000 Hz
+	IMU.setDLPF(IMU.DLPF_MAX);
+	IMU.setRate(IMU.RATE_1KHZ_APPROX);
 }
 
 void readIMU() {
@@ -41,6 +41,16 @@ 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() {
@@ -56,36 +66,41 @@ void calibrateGyro() {
 	}
 	gyroBias = gyroBias / samples;
 
-	printIMUCal();
+	printIMUCalibration();
 	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("Place level [enter] "); Serial.readStringUntil('\n');
+	Serial.print("1/6 Place level [enter] ");
+	Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("Place nose up [enter] "); Serial.readStringUntil('\n');
+	Serial.print("2/6 Place nose up [enter] ");
+	Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("Place nose down [enter] "); Serial.readStringUntil('\n');
+	Serial.print("3/6 Place nose down [enter] ");
+	Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("Place on right side [enter] "); Serial.readStringUntil('\n');
+	Serial.print("4/6 Place on right side [enter] ");
+	Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("Place on left side [enter] "); Serial.readStringUntil('\n');
+	Serial.print("5/6 Place on left side [enter] ");
+	Serial.readStringUntil('\n');
 	calibrateAccelOnce();
-	Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n');
+	Serial.print("6/6 Place upside down [enter] ");
+	Serial.readStringUntil('\n');
 	calibrateAccelOnce();
 
-	printIMUCal();
+	printIMUCalibration();
+	Serial.print("✓ Calibration done!\n");
 	configureIMU();
 }
 
 void calibrateAccelOnce() {
-	const int samples = 100;
+	const int samples = 1000;
 	static Vector accMax(-INFINITY, -INFINITY, -INFINITY);
 	static Vector accMin(INFINITY, INFINITY, INFINITY);
 
@@ -106,16 +121,18 @@ void calibrateAccelOnce() {
 	if (acc.x < accMin.x) accMin.x = acc.x;
 	if (acc.y < accMin.y) accMin.y = acc.y;
 	if (acc.z < accMin.z) accMin.z = acc.z;
-	Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z);
-	Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z);
-	Serial.printf("min %f %f %f\n", accMin.x, accMin.y, accMin.z);
 	// Compute scale and bias
 	accScale = (accMax - accMin) / 2 / ONE_G;
 	accBias = (accMax + accMin) / 2;
 }
 
-void printIMUCal() {
+void printIMUCalibration() {
 	Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
 	Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
 	Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);
 }
+
+void printIMUInfo() {
+	Serial.printf("model: %s\n", IMU.getModel());
+	Serial.printf("who am I: 0x%02X\n", IMU.whoAmI());
+}

flix/led.ino

@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Main LED control
+// Board's LED control
 
 #define BLINK_PERIOD 500000
 
@@ -14,9 +14,10 @@ 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);
-}
-
-void blinkLED() {
-	setLED(micros() / BLINK_PERIOD % 2);
+	state = on;
 }

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.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][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][13] = thrustTarget;
 
 	logPointer++;

flix/lpf.h

@@ -22,7 +22,8 @@ public:
 			output = input;
 			initialized = true;
 		}
-		return output = output * (1 - alpha) + input * alpha;
+
+		return output += alpha * (input - output);
 	}
 
 	void setCutOffFrequency(float cutOffFreq, float dt) {

flix/mavlink.ino

@@ -3,7 +3,7 @@
 
 // MAVLink communication
 
-#if WIFI_ENABLED == 1
+#if WIFI_ENABLED
 
 #include <MAVLink.h>
 
@@ -13,6 +13,8 @@
 #define MAVLINK_CONTROL_SCALE 0.7f
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
 
+extern float controlTime;
+
 void processMavlink() {
 	sendMavlink();
 	receiveMavlink();
@@ -28,8 +30,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 : 0),
+		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),
 			0, MAV_STATE_STANDBY);
 		sendMessage(&msg);
 	}
@@ -39,13 +41,11 @@ 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);
+			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
 		sendMessage(&msg);
 
-		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);
+		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);
 		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,
-			gyro.x * 1000, gyro.y * 1000, gyro.z * 1000,
+			acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd
+			gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
 			0, 0, 0, 0);
 		sendMessage(&msg);
 	}
@@ -82,18 +82,20 @@ void receiveMavlink() {
 }
 
 void handleMavlink(const void *_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
+	const mavlink_message_t& msg = *(mavlink_message_t *)_msg;
 
-		if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
+	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;
 	}
 }
 

flix/motors.ino

@@ -1,69 +1,61 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Motors output control
-// Motor: 8520 3.7V
-// ESC: KINGDUO Micro Mini 4A 1S Brushed Esc 3.6-6V
+// Motors output control using MOSFETs
 
-#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
+#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;
 
 void setupMotors() {
 	Serial.println("Setup Motors");
 
-	// 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);
+	// 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);
 
 	sendMotors();
 	Serial.println("Motors initialized");
 }
 
-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);
+int getDutyCycle(float value) {
+	value = constrain(value, 0, 1);
+	float duty = mapff(value, 0, 1, 0, (1 << PWM_RESOLUTION) - 1);
+	return round(duty);
 }
 
 void sendMotors() {
-	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)));
+	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]));
 }
 
-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));
+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");
 }

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(float a, float b, float c, float angle) {
+	static Quaternion fromAxisAngle(const Vector& axis, float angle) {
 		float halfAngle = angle * 0.5;
 		float sin2 = sin(halfAngle);
 		float cos2 = cos(halfAngle);
-		float sinNorm = sin2 / sqrt(a * a + b * b + c * c);
-		return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);
+		float sinNorm = sin2 / axis.norm();
+		return Quaternion(cos2, axis.x * sinNorm, axis.y * sinNorm, axis.z * sinNorm);
 	}
 
-	static Quaternion fromAngularRates(const Vector& rates) {
-		if (rates.zero()) {
+	static Quaternion fromRotationVector(const Vector& rotation) {
+		if (rotation.zero()) {
 			return Quaternion();
 		}
-		return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
+		return Quaternion::fromAxisAngle(rotation, rotation.norm());
 	}
 
-	static Quaternion fromEulerZYX(const Vector& euler) {
+	static Quaternion fromEuler(const Vector& euler) {
 		float cx = cos(euler.x / 2);
 		float cy = cos(euler.y / 2);
 		float cz = cos(euler.z / 2);
@@ -60,14 +60,38 @@ public:
 		return ret;
 	}
 
-	void toAxisAngle(float& a, float& b, float& c, float& angle) {
-		angle = acos(w) * 2;
-		a = x / sin(angle / 2);
-		b = y / sin(angle / 2);
-		c = z / sin(angle / 2);
+	bool finite() const {
+		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
-	Vector toEulerZYX() 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;
+	}
+
+	void toAxisAngle(Vector& axis, float& angle) const {
+		angle = acos(w) * 2;
+		axis.x = x / sin(angle / 2);
+		axis.y = y / sin(angle / 2);
+		axis.z = 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 {
 		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87
 		Vector euler;
 		float sqx = x * x;
@@ -75,7 +99,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); /* normalization added from urdfom_headers */
+		float sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
 		if (sarg <= -0.99999) {
 			euler.x = 0;
 			euler.y = -0.5 * PI;
@@ -112,21 +136,12 @@ public:
 
 	void setYaw(float yaw) {
 		// TODO: optimize?
-		Vector euler = toEulerZYX();
+		Vector euler = toEuler();
 		euler.z = yaw;
-		(*this) = Quaternion::fromEulerZYX(euler);
+		(*this) = Quaternion::fromEuler(euler);
 	}
 
-	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) {
+	Quaternion operator * (const Quaternion& q) const {
 		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,
@@ -134,6 +149,14 @@ 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(
@@ -143,37 +166,39 @@ public:
 			-z * normSqInv);
 	}
 
-	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) {
+	Vector conjugate(const Vector& v) const {
 		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) {
+	Vector conjugateInversed(const Vector& v) const {
 		Quaternion qv(0, v.x, v.y, v.z);
 		Quaternion res = inversed() * qv * (*this);
 		return Vector(res.x, res.y, res.z);
 	}
 
-	// Rotate vector by quaternion
-	inline Vector rotate(const Vector& v) {
-		return conjugateInversed(v);
+	// 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;
 	}
 
-	inline bool finite() const {
-		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
+	// Rotate vector by quaternion
+	static Vector rotateVector(const Vector& v, const Quaternion& q) {
+		return q.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;
 	}
 
 	size_t printTo(Print& p) const {

flix/rc.ino

@@ -4,51 +4,77 @@
 // 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 channelNeutral[] = {995, 883, 200, 972, 512, 512};
-int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
+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};
 
-SBUS RC(Serial2);
+// Channels mapping:
+int rollChannel = 0;
+int pitchChannel = 1;
+int throttleChannel = 2;
+int yawChannel = 3;
+int armedChannel = 4;
+int modeChannel = 5;
 
 void setupRC() {
 	Serial.println("Setup RC");
 	RC.begin();
 }
 
-void readRC() {
+bool readRC() {
 	if (RC.read()) {
 		SBUSData data = RC.data();
-		memcpy(channels, data.ch, sizeof(channels)); // copy channels data
+		for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
 		normalizeRC();
+		controlTime = t;
+		return true;
 	}
+	return false;
 }
 
 void normalizeRC() {
-	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
-		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
+	float controls[16];
+	for (int i = 0; i < 16; i++) {
+		controls[i] = mapf(channels[i], channelZero[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 within 4 seconds");
+	Serial.println("Calibrate RC: move all sticks to maximum positions [4 sec]");
 	Serial.println("··o     ··o\n···     ···\n···     ···");
 	delay(4000);
-	for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated
-	for (int i = 0; i < RC_CHANNELS; i++) {
+	while (!readRC());
+	for (int i = 0; i < 16; i++) {
 		channelMax[i] = channels[i];
 	}
-	Serial.println("Calibrate RC: move all sticks to neutral positions within 4 seconds");
+	Serial.println("Calibrate RC: move all sticks to neutral positions [4 sec]");
 	Serial.println("···     ···\n···     ·o·\n·o·     ···");
 	delay(4000);
-	for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated
-	for (int i = 0; i < RC_CHANNELS; i++) {
-		channelNeutral[i] = channels[i];
+	while (!readRC());
+	for (int i = 0; i < 16; i++) {
+		channelZero[i] = channels[i];
 	}
-	printRCCal();
+	printRCCalibration();
 }
 
-void printRCCal() {
-	printArray(channelNeutral, RC_CHANNELS);
-	printArray(channelMax, RC_CHANNELS);
+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");
 }

flix/time.ino

@@ -3,6 +3,8 @@
 
 // Time related functions
 
+float loopRate; // Hz
+
 void step() {
 	float now = micros() / 1000000.0;
 	dt = now - t;
@@ -12,16 +14,16 @@ void step() {
 		dt = 0; // assume dt to be zero on first step and on reset
 	}
 
-	computeLoopFreq();
+	computeLoopRate();
 }
 
-void computeLoopFreq() {
+void computeLoopRate() {
 	static float windowStart = 0;
-	static uint32_t freq = 0;
-	freq++;
+	static uint32_t rate = 0;
+	rate++;
 	if (t - windowStart >= 1) { // 1 second window
-		loopFreq = freq;
+		loopRate = rate;
 		windowStart = t;
-		freq = 0;
+		rate = 0;
 	}
 }

flix/util.h

@@ -3,10 +3,14 @@
 
 // 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;
 }
@@ -15,14 +19,6 @@ 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);
@@ -34,17 +30,7 @@ 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() {
-	WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
+	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
 }

flix/vector.h

@@ -13,14 +13,18 @@ public:
 
 	Vector(float x, float y, float z): x(x), y(y), z(z) {};
 
-	float norm() const {
-		return sqrt(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);
+	}
+
 	void normalize() {
 		float n = norm();
 		x /= n;
@@ -28,6 +32,10 @@ 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);
 	}
@@ -44,6 +52,14 @@ 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);
@@ -54,18 +70,14 @@ public:
 		return Vector(x / b.x, y / b.y, z / b.z);
 	}
 
-	inline bool operator == (const Vector& b) const {
+	bool operator == (const Vector& b) const {
 		return x == b.x && y == b.y && z == b.z;
 	}
 
-	inline bool operator != (const Vector& b) const {
+	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;
 	}
@@ -74,18 +86,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 angleBetweenVectors(const Vector& a, const Vector& b) {
+	static float angleBetween(const Vector& a, const Vector& b) {
 		return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1));
 	}
 
-	static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) {
+	static Vector rotationVectorBetween(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 = angleBetweenVectors(a, b);
+		float angle = angleBetween(a, b);
 		return direction * angle;
 	}
 
@@ -96,3 +108,6 @@ 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; }

flix/wifi.ino

@@ -3,7 +3,7 @@
 
 // Wi-Fi support
 
-#if WIFI_ENABLED == 1
+#if WIFI_ENABLED
 
 #include <WiFi.h>
 #include <WiFiAP.h>
@@ -11,20 +11,19 @@
 
 #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_UDP_IP, WIFI_UDP_PORT);
+	udp.beginPacket(WiFi.softAPBroadcastIP(), WIFI_UDP_REMOTE_PORT);
 	udp.write(buf, len);
 	udp.endPacket();
 }

gazebo/Arduino.h

@@ -7,6 +7,7 @@
 
 #include <cmath>
 #include <string>
+#include <stdint.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <sys/poll.h>
@@ -98,7 +99,7 @@ public:
 class HardwareSerial: public Print {
 public:
 	void begin(unsigned long baud) {
-		// server is running in background by default, so doesn't have access to stdin
+		// server is running in background by default, so it 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
@@ -116,7 +117,7 @@ public:
 	int read() {
 		if (available()) {
 			char c;
-			::read(STDIN_FILENO, &c, 1); // use raw read to avoid C++ buffering
+			size_t res = ::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;
 		}
@@ -132,8 +133,12 @@ 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;
+	return __micros + __resetTime; // keep the time monotonic
 }

gazebo/CMakeLists.txt

@@ -15,6 +15,7 @@ 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)

gazebo/SBUS.h

@@ -12,12 +12,14 @@ 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 joystickGet(); };
+	bool read() { return joystickInit(); };
 	SBUSData data() {
 		SBUSData data;
-		for (uint8_t i = 0; i < 16; i++) {
-			data.ch[i] = channels[i];
+		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
 		}
 		return data;
 	};

gazebo/flix.h

@@ -10,50 +10,44 @@
 #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];
-int16_t channels[16]; // raw rc channels
-float controls[RC_CHANNELS];
+float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
 Vector acc;
 Vector gyro;
 Vector rates;
 Quaternion attitude;
 
 // declarations
-void computeLoopFreq();
+void computeLoopRate();
 void applyGyro();
 void applyAcc();
-void signalizeHorizontality();
 void control();
 void interpretRC();
 void controlAttitude();
 void controlRate();
 void controlTorque();
 void showTable();
+void sendMotors();
 bool motorsActive();
-void cliTestMotor(uint8_t n);
-void printRCCal();
+void doCommand(const String& command);
+void normalizeRC();
+void printRCCalibration();
 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 fullMotorTest(int n, bool reverse) { printf("Skip full motor test\n"); };
-void sendMotors() {};
-void printIMUCal() { printf("cal: N/A\n"); };
+void printIMUCalibration() { printf("cal: N/A\n"); };
+void printIMUInfo() {};

gazebo/flix.world

@@ -9,7 +9,7 @@
 		</scene>
 		<gui>
 			<camera name="user_camera">
-				<pose>-2 -0.3 1.5 0 0.5 0.1</pose>
+				<pose>-2.3 0 1.1 0 0.3 0</pose>
 			</camera>
 		</gui>
 		<physics type="ode">
@@ -23,7 +23,7 @@
 		</include>
 		<include>
 			<uri>model://flix</uri>
-			<pose>0 0 0.2 0 0 0</pose>
+			<pose>0 0 0.3 0 0 0</pose>
 		</include>
 	</world>
 </sdf>

gazebo/joystick.h

@@ -8,22 +8,26 @@
 #include <iostream>
 
 // simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
-const int channelNeutralOverride[] = {-258, -258, -27349, 0, -27349, 0};
-const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1};
+const int channelZeroOverride[] = {1500, 0, 1000, 1500, 1500, 1000};
+const int channelMaxOverride[] = {2000, 2000, 2000, 2000, 2000, 2000};
 
-#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
+// 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;
 
 SDL_Joystick *joystick;
-bool joystickInitialized = false, warnShown = false;
 
 void normalizeRC();
 
-void joystickInit() {
+bool joystickInit() {
+	static bool joystickInitialized = false;
+	static bool warnShown = false;
+	if (joystickInitialized) return true;
+
 	SDL_Init(SDL_INIT_JOYSTICK);
 	joystick = SDL_JoystickOpen(0);
 	if (joystick != NULL) {
@@ -34,23 +38,28 @@ void joystickInit() {
 		warnShown = true;
 	}
 
-	// apply calibration overrides
-	extern int channelNeutral[RC_CHANNELS];
-	extern int channelMax[RC_CHANNELS];
-	memcpy(channelNeutral, channelNeutralOverride, sizeof(channelNeutralOverride));
+	// apply overrides
+	extern int channelZero[16];
+	extern int channelMax[16];
+	memcpy(channelZero, channelZeroOverride, sizeof(channelZeroOverride));
 	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() {
-	if (!joystickInitialized) {
-		joystickInit();
-		return false;
-	}
-
+bool joystickGet(int16_t ch[16]) {
 	SDL_JoystickUpdate();
 
-	for (uint8_t i = 0; i < 8; i++) {
-		channels[i] = SDL_JoystickGetAxis(joystick, i);
+	for (uint8_t i = 0; i < 16; i++) {
+		ch[i] = SDL_JoystickGetAxis(joystick, i);
 	}
 	return true;
 }

gazebo/models/flix/flix.sdf

@@ -1,6 +1,7 @@
 <?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>
@@ -13,57 +14,58 @@
 			<collision name="collision">
 				<geometry>
 					<box>
-						<size>0.125711 0.125711 0.022</size>
+						<size>0.095 0.095 0.0276</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><!-- 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>
+						<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>
 					</angular_velocity>
 					<linear_acceleration>
-						<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>
+						<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>
 					</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>

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(bind(&ModelFlix::OnUpdate, this));
-		this->resetConnection = event::Events::ConnectWorldReset(bind(&ModelFlix::OnReset, this));
+		this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&ModelFlix::OnUpdate, this));
+		this->resetConnection = event::Events::ConnectWorldReset(std::bind(&ModelFlix::OnReset, this));
 		initNode();
 		Serial.begin(0);
 		gzmsg << "Flix plugin loaded" << endl;
@@ -56,6 +56,7 @@ public:
 
 	void OnReset() {
 		attitude = Quaternion(); // reset estimated attitude
+		__resetTime += __micros;
 		gzmsg << "Flix plugin reset" << endl;
 	}
 
@@ -63,22 +64,22 @@ public:
 		__micros = model->GetWorld()->SimTime().Double() * 1000000;
 		step();
 
-		// read imu
-		gyro = flu2frd(imu->AngularVelocity());
-		acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
+		// 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 rc
 		readRC();
-		controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
-		controls[RC_CHANNEL_ARMED] = 1; // armed
+		controlMode = 1; // 0 acro, 1 stab
+		controlArmed = 1; // armed
 
 		estimate();
 
 		// correct yaw to the actual yaw
-		attitude.setYaw(-this->model->WorldPose().Yaw());
+		attitude.setYaw(this->model->WorldPose().Yaw());
 
 		control();
-		parseInput();
+		handleInput();
 		processMavlink();
 
 		applyMotorForces();

gazebo/soc/rtc_cntl_reg.h

@@ -1 +1 @@
-// Dummy file to make it possible to compile simulator with util.ino
+// Dummy file to make it possible to compile simulator with Flix' util.h

gazebo/soc/soc.h

@@ -1,3 +1,4 @@
-// Dummy file to make it possible to compile simulator with util.ino
+// Dummy file to make it possible to compile simulator with Flix' util.h
 
 #define WRITE_PERI_REG(addr, val) {}
+#define REG_CLR_BIT(_r, _b) {}

gazebo/util.h

@@ -1,14 +0,0 @@
-#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());
-}

gazebo/wifi.h

@@ -11,29 +11,29 @@
 #include <sys/poll.h>
 #include <gazebo/gazebo.hh>
 
-#define WIFI_UDP_PORT_LOCAL 14580
-#define WIFI_UDP_PORT_REMOTE 14550
+#define WIFI_UDP_PORT 14580
+#define WIFI_UDP_REMOTE_PORT 14550
 
 int wifiSocket;
 
 void setupWiFi() {
 	wifiSocket = socket(AF_INET, SOCK_DGRAM, 0);
-	sockaddr_in addr;
+	sockaddr_in addr; // local address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_ANY;
-	addr.sin_port = htons(WIFI_UDP_PORT_LOCAL);
+	addr.sin_port = htons(WIFI_UDP_PORT);
 	bind(wifiSocket, (sockaddr *)&addr, sizeof(addr));
 	int broadcast = 1;
 	setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
-	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT_LOCAL << " (remote port " << WIFI_UDP_PORT_REMOTE << ")" << std::endl;
+	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
 	if (wifiSocket == 0) setupWiFi();
-	sockaddr_in addr;
+	sockaddr_in addr; // remote address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_BROADCAST; // send UDP broadcast
-	addr.sin_port = htons(WIFI_UDP_PORT_REMOTE);
+	addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
 	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
 }
 

tools/check_c_cpp_properties.py

@@ -0,0 +1,61 @@
+#!/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'])

tools/csv_to_mcap.py

@@ -0,0 +1,46 @@
+#!/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()

tools/csv_to_ulog/CMakeLists.txt

@@ -0,0 +1,23 @@
+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)

tools/csv_to_ulog/README.md

@@ -0,0 +1,20 @@
+# 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.

tools/csv_to_ulog/csv_to_ulog.cpp

@@ -0,0 +1,72 @@
+// 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);
+	}
+}

tools/requirements.txt

@@ -1,2 +1,3 @@
 docopt
 matplotlib
+mcap