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.

.github/workflows/build.yml

@@ -15,6 +15,8 @@ jobs:
       run: curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh
     - name: Build firmware
       run: make
+    - name: Build firmware without Wi-Fi
+      run: sed -i 's/^#define WIFI_ENABLED 1$/#define WIFI_ENABLED 0/' flix/flix.ino && make
     - name: Check c_cpp_properties.json
       run: tools/check_c_cpp_properties.py
 
@@ -43,7 +45,7 @@ jobs:
       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
@@ -54,28 +56,28 @@ jobs:
       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@v4
-    - name: Clean up python binaries # Workaround for https://github.com/actions/setup-python/issues/577
-      run: |
-        rm -f /usr/local/bin/2to3*
-        rm -f /usr/local/bin/idle3*
-        rm -f /usr/local/bin/pydoc3*
-        rm -f /usr/local/bin/python3*
-        rm -f /usr/local/bin/python3*-config
-    - name: Install Gazebo
-      run: brew update && brew tap osrf/simulation && brew install gazebo11
-    - name: Install SDL2
-      run: brew install sdl2
-    - name: Build simulator
-      run: make build_simulator
+#  build_simulator_macos:
+#    runs-on: macos-latest
+#    steps:
+#    - name: Install Arduino CLI
+#      run: brew install arduino-cli
+#    - uses: actions/checkout@v4
+#    - name: Clean up python binaries # Workaround for https://github.com/actions/setup-python/issues/577
+#      run: |
+#        rm -f /usr/local/bin/2to3*
+#        rm -f /usr/local/bin/idle3*
+#        rm -f /usr/local/bin/pydoc3*
+#        rm -f /usr/local/bin/python3*
+#        rm -f /usr/local/bin/python3*-config
+#    - name: Install Gazebo
+#      run: brew update && brew tap osrf/simulation && brew install gazebo11
+#    - name: Install SDL2
+#      run: brew install sdl2
+#    - name: Build simulator
+#      run: make build_simulator

.vscode/c_cpp_properties.json

@@ -5,18 +5,18 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.3/cores/esp32",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.3/libraries/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.3/variants/d1_mini32",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-dc859c1e67/esp32/**",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-dc859c1e67/esp32/dio_qspi/include",
+				"~/.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.0.3/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.3/variants/d1_mini32/pins_arduino.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",
@@ -28,10 +28,9 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino"
 			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -51,19 +50,19 @@
 			"name": "Mac",
 			"includePath": [
 				"${workspaceFolder}/flix",
-				"${workspaceFolder}/gazebo",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.3/cores/esp32",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.3/libraries/**",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.3/variants/d1_mini32",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-dc859c1e67/esp32/include/**",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-dc859c1e67/esp32/dio_qspi/include",
+				// "${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.0.3/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.3/variants/d1_mini32/pins_arduino.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",
@@ -75,10 +74,9 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino"
 			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -100,17 +98,17 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.3/cores/esp32",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.3/libraries/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.3/variants/d1_mini32",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-dc859c1e67/esp32/**",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-dc859c1e67/esp32/dio_qspi/include",
+				"~/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.0.3/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.3/variants/d1_mini32/pins_arduino.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",
@@ -122,10 +120,9 @@
 				"${workspaceFolder}/flix/motors.ino",
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/util.ino",
 				"${workspaceFolder}/flix/wifi.ino"
 			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++.exe",
+			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [

.vscode/extensions.json

@@ -2,7 +2,6 @@
 	// See https://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
 	"recommendations": [
 		"ms-vscode.cpptools",
-		"twxs.cmake",
 		"ms-vscode.cmake-tools",
 		"ms-python.python"
 	],

Makefile

@@ -13,10 +13,10 @@ monitor:
 
 dependencies .dependencies:
 	arduino-cli core update-index --config-file arduino-cli.yaml
-	arduino-cli core install esp32:esp32@3.0.3 --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.10
+	arduino-cli lib install "MAVLink"@2.0.16
 	touch .dependencies
 
 gazebo/build cmake: gazebo/CMakeLists.txt

README.md

@@ -1,144 +1,7 @@
 # Flix
 
-**Flix** (*flight + X*) — making an open source ESP32-based quadcopter from scratch.
+Minimal **Flix** quadcopter firmware implementation for the [book](https://quadcopter.dev).
 
-<table>
-  <tr>
-    <td align=center><strong>Version 1</strong> (3D-printed frame)</td>
-    <td align=center><strong>Version 0</strong></td>
-  </tr>
-  <tr>
-    <td><img src="docs/img/flix1.jpg" width=500 alt="Flix quadcopter"></td>
-    <td><img src="docs/img/flix.jpg" width=500 alt="Flix quadcopter"></td>
-  </tr>
-</table>
+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](docs/log.md).
-* Command line interface through USB port.
-* Wi-Fi support.
-* MAVLink support.
-* Control using mobile phone (with QGroundControl app).
-* Completely 3D-printed frame.
-* *Textbook and videos for students on writing a flight controller¹.*
-* *Position control and autonomous flights using external camera¹*.
-* [Building and running instructions](docs/build.md).
-
-*¹ — planned.*
-
-## It actually flies
-
-See detailed demo video (for version 0): https://youtu.be/8GzzIQ3C6DQ.
-
-<a href="https://youtu.be/8GzzIQ3C6DQ"><img width=500 src="https://i3.ytimg.com/vi/8GzzIQ3C6DQ/maxresdefault.jpg"></a>
-
-Version 1 test flight: https://t.me/opensourcequadcopter/42.
-
-<a href="https://t.me/opensourcequadcopter/42"><img width=500 src="docs/img/flight-video.jpg"></a>
-
-## Simulation
-
-The simulator is implemented using Gazebo and runs the original Arduino code:
-
-<img src="docs/img/simulator.png" width=500 alt="Flix simulator">
-
-See [instructions on running the simulation](docs/build.md).
-
-## Components (version 1)
-
-|Type|Part|Image|Quantity|
-|-|-|:-:|:-:|
-|Microcontroller board|ESP32 Mini|<img src="docs/img/esp32.jpg" width=100>|1|
-|IMU and barometer² board|GY-91 (or other MPU-9250 board)|<img src="docs/img/gy-91.jpg" width=100>|1|
-|Motor|8520 3.7V brushed motor (**shaft 0.8mm!**)|<img src="docs/img/motor.jpeg" width=100>|4|
-|Propeller|Hubsan 55 mm|<img src="docs/img/prop.jpg" width=100>|4|
-|MOSFET (transistor)|100N03A or [compatible](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|
-|Pull-down resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|4|
-|3.7V Li-Po battery|LW 952540 (or any compatible by the size)|<img src="docs/img/battery.jpg" width=100>|1|
-|Li-Po Battery charger|Any|<img src="docs/img/charger.jpg" width=100>|1|
-|Screws for IMU board mounting|M3x5|<img src="docs/img/screw-m3.jpg" width=100>|2|
-|Screws for frame assembly|M1.4x5|<img src="docs/img/screw-m1.4.jpg" height=30 align=center>|4|
-|Frame bottom part|3D printed: [`flix-frame.stl`](docs/assets/flix-frame.stl)|<img src="docs/img/frame1.jpg" width=100>|1|
-|Frame top part|3D printed: [`esp32-holder.stl`](docs/assets/esp32-holder.stl)|<img src="docs/img/esp32-holder.jpg" width=100>|1|
-|Washer for IMU board mounting|3D printed: [`washer-m3.stl`](docs/assets/washer-m3.stl)|<img src="docs/img/washer-m3.jpg" width=100>|1|
-|*RC transmitter (optional)*|*KINGKONG TINY X8 or other³*|<img src="docs/img/tx.jpg" width=100>|1|
-|*RC receiver (optional)*|*DF500 or other³*|<img src="docs/img/rx.jpg" width=100>|1|
-|Wires|28 AWG recommended|<img src="docs/img/wire-28awg.jpg" width=100>||
-|Tape, double-sided tape||||
-
-*² — barometer is not used for now.*<br>
-*³ — you may use any transmitter-receiver pair with SBUS interface.*
-
-Tools required for assembly:
-
-* 3D printer.
-* Soldering iron.
-* Solder wire (with flux).
-* Screwdrivers.
-* Multimeter.
-
-Feel free to modify the design and or code, and create your own improved versions of Flix! Send your results to the [official Telegram chat](https://t.me/opensourcequadcopterchat), or directly to the author ([E-mail](mailto:okalachev@gmail.com), [Telegram](https://t.me/okalachev)).
-
-## Schematics (version 1)
-
-### Simplified connection diagram
-
-<img src="docs/img/schematics1.svg" width=800 alt="Flix version 1 schematics">
-
-Motor connection scheme:
-
-<img src="docs/img/mosfet-connection.png" height=400 alt="MOSFET connection scheme">
-
-Complete diagram is Work-in-Progress.
-
-### Notes
-
-* Power ESP32 Mini with Li-Po battery using VCC (+) and GND (-) pins.
-* Connect the GY-91 board to the ESP32 Mini using VSPI, power it using 3.3V and GND pins:
-
-  |GY-91 pin|ESP32 pin|
-  |-|-|
-  |GND|GND|
-  |3.3V|3.3V|
-  |SCK|SVP (GPIO18)|
-  |MOSI|GPIO23|
-  |MISO|GPIO19|
-  |NCS|GPIO5|
-
-* Solder pull-down resistors to the MOSFETs.
-* Connect the motors to the ESP32 Mini using MOSFETs, by following scheme:
-
-  |Motor|Position|Direction|Wires|GPIO|
-  |-|-|-|-|-|
-  |Motor 0|Rear left|Counter-clockwise|Black & White|GPIO12|
-  |Motor 1|Rear right|Clockwise|Blue & Red|GPIO13|
-  |Motor 2|Front right|Counter-clockwise|Black & White|GPIO14|
-  |Motor 3|Front left|Clockwise|Blue & Red|GPIO15|
-
-  Counter-clockwise motors have black and white wires and clockwise motors have blue and red wires.
-
-* Optionally connect the RC receiver to the ESP32's UART2:
-
-  |Receiver pin|ESP32 pin|
-  |-|-|
-  |GND|GND|
-  |VIN|VC (or 3.3V depending on the receiver)|
-  |Signal|GPIO4⁴|
-
-*⁴ — UART2 RX pin was [changed](https://docs.espressif.com/projects/arduino-esp32/en/latest/migration_guides/2.x_to_3.0.html#id14) to GPIO4 in Arduino ESP32 core 3.0.*
-
-## Version 0
-
-See the information on the obsolete version 0 in the [corresponding article](docs/version0.md).
-
-## Materials
-
-Subscribe to the Telegram channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter.
-
-Join the official Telegram chat: https://t.me/opensourcequadcopterchat.
-
-Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/.
+<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,6 +11,8 @@ 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
@@ -104,13 +106,15 @@ cd flix
 ### Arduino IDE (Windows, Linux, macOS)
 
 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
-2. Install ESP32 core, version 3.0.3 (version 2.x is not supported). See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
-3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
-   * `FlixPeriph`.
-   * `MAVLink`, version 2.0.10.
-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)
 
@@ -163,6 +167,9 @@ Before flight using remote control, you need to calibrate it:
 
 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.

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/version0.md

@@ -1,30 +0,0 @@
-# Flix version 0
-
-Flix version 0 (obsolete):
-
-<img src="img/flix.jpg" width=500 alt="Flix quadcopter">
-
-## Components list
-
-|Type|Part|Image|Quantity|
-|-|-|-|-|
-|Microcontroller board|ESP32 Mini|<img src="img/esp32.jpg" width=100>|1|
-|IMU and barometer² board|GY-91 (or other MPU-9250 board)|<img src="img/gy-91.jpg" width=100>|1|
-|Quadcopter frame|K100|<img src="img/frame.jpg" width=100>|1|
-|Motor|8520 3.7V brushed motor (**shaft 0.8mm!**)|<img src="img/motor.jpeg" width=100>|4|
-|Propeller|Hubsan 55 mm|<img src="img/prop.jpg" width=100>|4|
-|Motor ESC|2.7A 1S Dual Way Micro Brush ESC|<img src="img/esc.jpg" width=100>|4|
-|RC transmitter|KINGKONG TINY X8|<img src="img/tx.jpg" width=100>|1|
-|RC receiver|DF500 (SBUS)|<img src="img/rx.jpg" width=100>|1|
-|~~SBUS inverter~~*||<img src="img/inv.jpg" width=100>|~~1~~|
-|Battery|3.7 Li-Po 850 MaH 60C|||
-|Battery charger||<img src="img/charger.jpg" width=100>|1|
-|Wires, connectors, tape, ...|||
-
-*\* — not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).*
-
-## Schematics
-
-<img src="img/schematics.svg" width=800 alt="Flix schematics">
-
-You can also check a user contributed [variant of complete circuit diagram](https://miro.com/app/board/uXjVN-dTjoo=/) of the drone.

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());
+		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;
-		ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
+		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; // positive yaw stick means clockwise rotation in FLU
+		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()) {
@@ -114,10 +119,10 @@ void controlAttitude() {
 	}
 
 	const Vector up(0, 0, 1);
-	Vector upActual = attitude.rotate(up);
-	Vector upTarget = attitudeTarget.rotate(up);
+	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);
@@ -161,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 1
 
-#define RC_CHANNELS 16
-#define RC_CHANNEL_ROLL 0
-#define RC_CHANNEL_PITCH 1
-#define RC_CHANNEL_THROTTLE 2
-#define RC_CHANNEL_YAW 3
-#define RC_CHANNEL_ARMED 4
-#define RC_CHANNEL_MODE 5
-
-#define MOTOR_REAR_LEFT 0
-#define MOTOR_REAR_RIGHT 1
-#define MOTOR_FRONT_RIGHT 2
-#define MOTOR_FRONT_LEFT 3
-
 float t = NAN; // current step time, s
 float dt; // time delta from previous step, s
-float loopFreq; // loop frequency, Hz
-int16_t channels[RC_CHANNELS]; // 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

@@ -2,20 +2,16 @@
 // Repository: https://github.com/okalachev/flix
 
 // Work with the IMU sensor
-// IMU is oriented FLU (front-left-up) style.
-// In case of FRD (front-right-down) orientation of the IMU, use this code:
-// https://gist.github.com/okalachev/713db47e31bce643dbbc9539d166ce98.
 
 #include <SPI.h>
 #include <MPU9250.h>
-
-#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() {
@@ -27,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() {
@@ -44,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() {
@@ -59,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);
 
@@ -109,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);
 	}
@@ -38,15 +40,12 @@ void sendMavlink() {
 		lastFast = t;
 
 		const float zeroQuat[] = {0, 0, 0, 0};
-		Quaternion attitudeFRD = FLU2FRD(attitude); // MAVLink uses FRD coordinate system
 		mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
-			time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.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];
@@ -55,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);
 	}
@@ -83,24 +82,21 @@ 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;
 	}
 }
 
-// Convert Forward-Left-Up to Forward-Right-Down quaternion
-inline Quaternion FLU2FRD(const Quaternion &q) {
-	return Quaternion(q.w, q.x, -q.y, -q.z);
-}
-
 #endif

flix/motors.ino

@@ -2,16 +2,22 @@
 // Repository: https://github.com/okalachev/flix
 
 // Motors output control using MOSFETs
-// In case of using ESC, use this version of the code: https://gist.github.com/okalachev/8871d3a94b6b6c0a298f41a4edd34c61.
-// Motor: 8520 3.7V
+
+#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 200
-#define PWM_RESOLUTION 8
+#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");
@@ -26,25 +32,30 @@ void setupMotors() {
 	Serial.println("Motors initialized");
 }
 
-uint8_t signalToDutyCycle(float control) {
-	float duty = mapff(control, 0, 1, 0, (1 << PWM_RESOLUTION) - 1);
-	return round(constrain(duty, 0, (1 << PWM_RESOLUTION) - 1));
+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(MOTOR_0_PIN, signalToDutyCycle(motors[0]));
-	ledcWrite(MOTOR_1_PIN, signalToDutyCycle(motors[1]));
-	ledcWrite(MOTOR_2_PIN, signalToDutyCycle(motors[2]));
-	ledcWrite(MOTOR_3_PIN, signalToDutyCycle(motors[3]));
+	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) {
-	printf("Full test for motor %d\n", n);
-	for (float signal = 0; signal <= 1; signal += 0.1) {
-		printf("Motor %d: %f\n", n, signal);
-		ledcWrite(n, signalToDutyCycle(signal));
-		delay(3000);
-	}
-	printf("Motor %d: %f\n", n, 0);
-	ledcWrite(n, signalToDutyCycle(0));
+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;
@@ -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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+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, 16, 17); // NOTE: remove pin numbers (16, 17) if you use the new default pins for Serial2 (4, 25)
+// 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

@@ -99,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
@@ -133,6 +133,9 @@ void delay(uint32_t ms) {
 	std::this_thread::sleep_for(std::chrono::milliseconds(ms));
 }
 
+bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
+bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
+
 unsigned long __micros;
 unsigned long __resetTime = 0;
 

gazebo/SBUS.h

@@ -14,11 +14,12 @@ 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,51 +10,44 @@
 #include "Arduino.h"
 #include "wifi.h"
 
-#define RC_CHANNELS 16
-
-#define MOTOR_REAR_LEFT 0
-#define MOTOR_FRONT_LEFT 3
-#define MOTOR_FRONT_RIGHT 2
-#define MOTOR_REAR_RIGHT 1
-
 #define WIFI_ENABLED 1
 
 float t = NAN;
 float dt;
-float 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);
-inline Quaternion FLU2FRD(const Quaternion &q);
+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) { 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/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>
@@ -23,38 +24,14 @@
 				<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>
@@ -90,6 +67,5 @@
 				<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,14 +17,15 @@
 
 #include "Arduino.h"
 #include "flix.h"
-#include "util.ino"
 #include "rc.ino"
 #include "time.ino"
+#include "motors.ino"
 #include "estimate.ino"
 #include "control.ino"
 #include "log.ino"
 #include "cli.ino"
 #include "mavlink.ino"
+#include "failsafe.ino"
 #include "lpf.h"
 
 using ignition::math::Vector3d;
@@ -69,8 +70,8 @@ public:
 
 		// 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();
 
@@ -78,7 +79,7 @@ public:
 		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/wifi.h

@@ -11,8 +11,8 @@
 #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;
 
@@ -21,11 +21,11 @@ void setupWiFi() {
 	sockaddr_in addr; // local address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_ANY;
-	addr.sin_port = htons(WIFI_UDP_PORT_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) {
@@ -33,7 +33,7 @@ void sendWiFi(const uint8_t *buf, int len) {
 	sockaddr_in addr; // remote address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_BROADCAST; // send UDP broadcast
-	addr.sin_port = htons(WIFI_UDP_PORT_REMOTE);
+	addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
 	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
 }