Add level calibration

Parameters: CTL_FLT_MODE_0, CTL_FLT_MODE_1, CTL_FLT_MODE_2.
Also fix a bug with incorrect choosing the mode from controlMode.

.vscode/c_cpp_properties.json

@@ -6,19 +6,18 @@
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
 				"${workspaceFolder}/tools/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
+				"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
 				"~/Arduino/libraries/**",
 				"/usr/include/gazebo-11/",
 				"/usr/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -31,9 +30,10 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
+				"${workspaceFolder}/flix/parameters.ino",
+				"${workspaceFolder}/flix/safety.ino"
 			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -53,19 +53,18 @@
 			"name": "Mac",
 			"includePath": [
 				"${workspaceFolder}/flix",
-				"~/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",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
+				"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
 				"~/Documents/Arduino/libraries/**",
 				"/opt/homebrew/include/gazebo-11/",
 				"/opt/homebrew/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/flix.ino",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
@@ -78,9 +77,10 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
+				"${workspaceFolder}/flix/parameters.ino",
+				"${workspaceFolder}/flix/safety.ino"
 			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -103,17 +103,16 @@
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
 				"${workspaceFolder}/tools/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
+				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
 				"~/Documents/Arduino/libraries/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -126,9 +125,10 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
+				"${workspaceFolder}/flix/parameters.ino",
+				"${workspaceFolder}/flix/safety.ino"
 			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
+			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [

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.2.0 --config-file arduino-cli.yaml
+	arduino-cli core install esp32:esp32@3.3.6 --config-file arduino-cli.yaml
 	arduino-cli lib update-index
 	arduino-cli lib install "FlixPeriph"
-	arduino-cli lib install "MAVLink"@2.0.16
+	arduino-cli lib install "MAVLink"@2.0.25
 	touch .dependencies
 
 gazebo/build cmake: gazebo/CMakeLists.txt

README.md

@@ -138,10 +138,10 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir
 
   |Motor|Position|Direction|Prop type|Motor wires|GPIO|
   |-|-|-|-|-|-|
-  |Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
-  |Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
-  |Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
-  |Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
+  |Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*|
+  |Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*|
+  |Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*|
+  |Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*|
 
   Clockwise motors have blue & red wires and correspond to propeller type A (marked on the propeller).
   Counter-clockwise motors have black & white wires correspond to propeller type B.

docs/assembly.md

@@ -41,10 +41,10 @@ Motors connection table:
 
 |Motor|Position|Direction|Prop type|Motor wires|GPIO|
 |-|-|-|-|-|-|
-|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
-|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
-|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
-|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
+|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*|
+|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*|
+|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*|
+|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*|
 
 ## Motors tightening
 

docs/book/geometry.md

@@ -110,7 +110,7 @@ float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов)
 
 #### Скалярное произведение
 
-Скалярное произведение векторов (*dot product*) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
+Скалярное произведение векторов *(dot product)* — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
 
 В Flix используется статический метод `Vector::dot()`:
 
@@ -124,7 +124,7 @@ float dotProduct = Vector::dot(a, b); // 32
 
 #### Векторное произведение
 
-Векторное произведение (*cross product*) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`:
+Векторное произведение *(cross product)* позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`:
 
 ```cpp
 Vector a(1, 2, 3);
@@ -144,9 +144,9 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
 
 В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, строго говоря, не являются вектором):
 
-* Угол по крену (*roll*) — `vector.x`.
-* Угол по тангажу (*pitch*) — `vector.y`.
-* Угол по рысканию (*yaw*) — `vector.z`.
+* Угол по крену *(roll)* — `vector.x`.
+* Угол по тангажу *(pitch)* — `vector.y`.
+* Угол по рысканию *(yaw)* — `vector.z`.
 
 Особенности углов Эйлера:
 
@@ -162,8 +162,8 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
 
 Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами:
 
-* **Ось вращения** (*axis*) — единичный вектор, определяющий ось вращения.
-* **Угол поворота** (*angle* или *θ*) — угол, на который нужно повернуть объект вокруг этой оси.
+* **Ось вращения** *(axis)* — единичный вектор, определяющий ось вращения.
+* **Угол поворота** *(angle* или *θ)* — угол, на который нужно повернуть объект вокруг этой оси.
 
 В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах:
 
@@ -177,7 +177,7 @@ float angle = radians(45);
 
 ### Вектор вращения
 
-Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** (*rotation vector*). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
+Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** *(rotation vector)*. Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
 
 Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями.
 
@@ -198,7 +198,7 @@ Vector rotation = radians(45) * Vector(1, 2, 3);
 	<a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br>
 </div>
 
-Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения (*axis*) и угла поворота (*θ*) по формуле:
+Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения *(axis)* и угла поворота *(θ)* по формуле:
 
 \\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\]
 

docs/book/gyro.md

@@ -177,7 +177,7 @@ imu.setDLPF(imu.DLPF_MAX);
 
 ## Калибровка гироскопа
 
-Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения (*bias*) и случайный шум (*noise*):
+Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения *(bias)* и случайный шум *(noise)*:
 
 \\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]
 

docs/troubleshooting.md

@@ -13,10 +13,10 @@ Do the following:
 Do the following:
 
 * **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
-* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
+* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 startup output.
 * **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
 * **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
-* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
+* **Check if the console is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the console using QGroundControl *(Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console)*.
 * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
 * **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.
 * **Check the IMU is working**. Perform `imu` command and check its output:

docs/usage.md

@@ -20,10 +20,10 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
 
 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
 2. *Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).*
-3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
+3. Install ESP32 core, version 3.3.6. 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.
+   * `MAVLink`, version 2.0.25.
 5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE.
 6. 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.
 7. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
@@ -108,7 +108,7 @@ The drone is configured using parameters. To access and modify them, go to the Q
 
 <img src="img/parameters.png" width="400">
 
-You can also work with parameters using `p` command in the console.
+You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
 
 ### Define IMU orientation
 
@@ -134,6 +134,20 @@ Before flight you need to calibrate the accelerometer:
 1. Access the console using QGroundControl (recommended) or Serial Monitor.
 2. Type `ca` command there and follow the instructions.
 
+### Setup motors
+
+If using non-default motor pins, set the pin numbers using the parameters: `MOTOR_PIN_FL`, `MOTOR_PIN_FR`, `MOTOR_PIN_RL`, `MOTOR_PIN_RR` (front-left, front-right, rear-left, rear-right respectively).
+
+If using brushless motors and ESCs:
+
+1. Set the appropriate PWM using the parameters: `MOT_PWM_STOP`, `MOT_PWM_MIN`, and `MOT_PWM_MAX` (1000, 1000, and 2000 is typical).
+2. Decrease the PWM frequency using the `MOT_PWM_FREQ` parameter (400 is typical).
+
+Reboot the drone to apply the changes.
+
+> [!CAUTION]
+> **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
+
 ### Check everything works
 
 1. Check the IMU is working: perform `imu` command and check its output:
@@ -250,8 +264,8 @@ You can configure the Wi-Fi using parameters and console commands.
 The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the console:
 
 * `0` — Wi-Fi is disabled.
-* `1` — Access Point mode (*AP*) — the drone creates a Wi-Fi network.
-* `2` — Client mode (*STA*) — the drone connects to an existing Wi-Fi network.
+* `1` — Access Point mode *(AP)* — the drone creates a Wi-Fi network.
+* `2` — Client mode *(STA)* — the drone connects to an existing Wi-Fi network.
 * `3` — *ESP-NOW (not implemented yet)*.
 
 > [!WARNING]
@@ -271,6 +285,12 @@ ap my-flix-ssid mypassword123
 p WIFI_MODE 1
 ```
 
+Disabling Wi-Fi:
+
+```
+p WIFI_MODE 0
+```
+
 ## Flight log
 
 After the flight, you can download the flight log for analysis wirelessly. Use the following command on your computer for that:

docs/user.md

@@ -4,6 +4,17 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
+Author: **Arkadiy "Arky" Matsekh**, Foucault Dynamics, Gold Coast, Australia.<br>
+The drone was built for the University of Queensland industry-led Master's capstone project.
+
+**Flight video:**
+
+<a href="https://drive.google.com/file/d/1NNYSVXBY-w0JjCo07D8-PgnVq3ca9plj/view?usp=sharing"><img height=300 src="img/user/arkymatsekh/video.jpg"></a>
+
+<img src="img/user/arkymatsekh/1.jpg" height=150> <img src="img/user/arkymatsekh/2.jpg" height=150> <img src="img/user/arkymatsekh/3.jpg" height=150>
+
+---
+
 Author: [goldarte](https://t.me/goldarte).<br>
 
 <img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150>

flix/cli.ino

@@ -6,6 +6,7 @@
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
+#include "lpf.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern const int RAW, ACRO, STAB, AUTO;
@@ -14,6 +15,7 @@ extern uint16_t channels[16];
 extern float controlTime;
 extern int mode;
 extern bool armed;
+extern LowPassFilter<Vector> gyroBiasFilter;
 
 const char* motd =
 "\nWelcome to\n"
@@ -44,6 +46,7 @@ const char* motd =
 "log [dump] - print log header [and data]\n"
 "cr - calibrate RC\n"
 "ca - calibrate accel\n"
+"cl - calibrate level\n"
 "mfr, mfl, mrr, mrl - test motor (remove props)\n"
 "sys - show system info\n"
 "reset - reset drone's state\n"
@@ -92,7 +95,7 @@ void doCommand(String str, bool echo = false) {
 	} else if (command == "p") {
 		bool success = setParameter(arg0.c_str(), arg1.toFloat());
 		if (success) {
-			print("%s = %g\n", arg0.c_str(), arg1.toFloat());
+			print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
 		} else {
 			print("Parameter not found: %s\n", arg0.c_str());
 		}
@@ -149,6 +152,8 @@ void doCommand(String str, bool echo = false) {
 		calibrateRC();
 	} else if (command == "ca") {
 		calibrateAccel();
+	} else if (command == "cl") {
+		calibrateLevel();
 	} else if (command == "mfr") {
 		testMotor(MOTOR_FRONT_RIGHT);
 	} else if (command == "mfl") {
@@ -178,6 +183,7 @@ void doCommand(String str, bool echo = false) {
 #endif
 	} else if (command == "reset") {
 		attitude = Quaternion();
+		gyroBiasFilter.reset();
 	} else if (command == "reboot") {
 		ESP.restart();
 	} else {

flix/control.ino

@@ -52,6 +52,7 @@ PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
 PID yawPID(YAW_P, 0, 0);
 Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
 float tiltMax = TILT_MAX;
+int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
@@ -65,9 +66,9 @@ void control() {
 }
 
 void interpretControls() {
-	if (controlMode < 0.25) mode = STAB;
-	if (controlMode < 0.75) mode = STAB;
-	if (controlMode > 0.75) mode = STAB;
+	if (controlMode < 0.25) mode = flightModes[0];
+	else if (controlMode < 0.75) mode = flightModes[1];
+	else if (controlMode > 0.75) mode = flightModes[2];
 
 	if (mode == AUTO) return; // pilot is not effective in AUTO mode
 

flix/flix.ino

@@ -7,7 +7,6 @@
 #include "quaternion.h"
 #include "util.h"
 
-
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
 extern Vector gyro, acc;

flix/imu.ino

@@ -19,6 +19,8 @@ Vector acc; // accelerometer output, m/s/s
 Vector accBias;
 Vector accScale(1, 1, 1);
 
+LowPassFilter<Vector> gyroBiasFilter(0.001);
+
 void setupIMU() {
 	print("Setup IMU\n");
 	imu.begin();
@@ -50,8 +52,6 @@ void readIMU() {
 void calibrateGyroOnce() {
 	static Delay landedDelay(2);
 	if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
-
-	static LowPassFilter<Vector> gyroBiasFilter(0.001);
 	gyroBias = gyroBiasFilter.update(gyro);
 }
 
@@ -110,6 +110,14 @@ void calibrateAccelOnce() {
 	accBias = (accMax + accMin) / 2;
 }
 
+void calibrateLevel() {
+	print("Place perfectly level [1 sec]\n");
+	pause(1);
+	Quaternion correction = Quaternion::fromBetweenVectors(Quaternion::rotateVector(Vector(0, 0, 1), attitude), Vector(0, 0, 1));
+	imuRotation = Quaternion::rotate(correction, Quaternion::fromEuler(imuRotation)).toEuler();
+	print("✓ Done: %.3f %.3f %.3f\n", degrees(imuRotation.x), degrees(imuRotation.y), degrees(imuRotation.z));
+}
+
 void printIMUCalibration() {
 	print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
 	print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);

flix/lpf.h

@@ -14,15 +14,6 @@ public:
 	LowPassFilter(float alpha): alpha(alpha) {};
 
 	T update(const T input) {
-		if (alpha == 1) { // filter disabled
-			return input;
-		}
-
-		if (!initialized) {
-			output = input;
-			initialized = true;
-		}
-
 		return output += alpha * (input - output);
 	}
 
@@ -31,9 +22,6 @@ public:
 	}
 
 	void reset() {
-		initialized = false;
+		output = T(); // set to zero
 	}
-
-private:
-	bool initialized = false;
 };

flix/mavlink.ino

@@ -142,7 +142,7 @@ void handleMavlink(const void *_msg) {
 		// send ack
 		mavlink_message_t msg;
 		mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
-			m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
+			m.param_id, getParameter(name), MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
 		sendMessage(&msg);
 	}
 

flix/motors.ino

@@ -1,24 +1,19 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Motors output control using MOSFETs
-// In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
+// PWM control for motors
 
 #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
-#define PWM_STOP 0
-#define PWM_MIN 0
-#define PWM_MAX 1000000 / PWM_FREQUENCY
-
 float motors[4]; // normalized motor thrusts in range [0..1]
 
+int motorPins[4] = {12, 13, 14, 15}; // default pin numbers
+int pwmFrequency = 78000;
+int pwmResolution = 10;
+int pwmStop = 0;
+int pwmMin = 0;
+int pwmMax = -1; // -1 means duty cycle mode
+
 const int MOTOR_REAR_LEFT = 0;
 const int MOTOR_REAR_RIGHT = 1;
 const int MOTOR_FRONT_RIGHT = 2;
@@ -26,30 +21,30 @@ const int MOTOR_FRONT_LEFT = 3;
 
 void setupMotors() {
 	print("Setup Motors\n");
-
 	// 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);
-
+	for (int i = 0; i < 4; i++) {
+		ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
+	}
 	sendMotors();
 	print("Motors initialized\n");
 }
 
-int getDutyCycle(float value) {
-	value = constrain(value, 0, 1);
-	float pwm = mapf(value, 0, 1, PWM_MIN, PWM_MAX);
-	if (value == 0) pwm = PWM_STOP;
-	float duty = mapf(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
-	return round(duty);
+void sendMotors() {
+	for (int i = 0; i < 4; i++) {
+		ledcWrite(motorPins[i], getDutyCycle(motors[i]));
+	}
 }
 
-void sendMotors() {
-	ledcWrite(MOTOR_0_PIN, getDutyCycle(motors[0]));
-	ledcWrite(MOTOR_1_PIN, getDutyCycle(motors[1]));
-	ledcWrite(MOTOR_2_PIN, getDutyCycle(motors[2]));
-	ledcWrite(MOTOR_3_PIN, getDutyCycle(motors[3]));
+int getDutyCycle(float value) {
+	value = constrain(value, 0, 1);
+	if (pwmMax >= 0) { // pwm mode
+		float pwm = mapf(value, 0, 1, pwmMin, pwmMax);
+		if (value == 0) pwm = pwmStop;
+		float duty = mapf(pwm, 0, 1000000 / pwmFrequency, 0, (1 << pwmResolution) - 1);
+		return round(duty);
+	} else { // duty cycle mode
+		return round(value * ((1 << pwmResolution) - 1));
+	}
 }
 
 bool motorsActive() {

flix/parameters.ino

@@ -10,6 +10,7 @@ extern float channelZero[16];
 extern float channelMax[16];
 extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern int wifiMode, udpLocalPort, udpRemotePort;
+extern float rcLossTimeout, descendTime;
 
 Preferences storage;
 
@@ -48,6 +49,9 @@ Parameter parameters[] = {
 	{"CTL_R_RATE_MAX", &maxRate.x},
 	{"CTL_Y_RATE_MAX", &maxRate.z},
 	{"CTL_TILT_MAX", &tiltMax},
+	{"CTL_FLT_MODE_0", &flightModes[0]},
+	{"CTL_FLT_MODE_1", &flightModes[1]},
+	{"CTL_FLT_MODE_2", &flightModes[2]},
 	// imu
 	{"IMU_ROT_ROLL", &imuRotation.x},
 	{"IMU_ROT_PITCH", &imuRotation.y},
@@ -58,9 +62,20 @@ Parameter parameters[] = {
 	{"IMU_ACC_SCALE_X", &accScale.x},
 	{"IMU_ACC_SCALE_Y", &accScale.y},
 	{"IMU_ACC_SCALE_Z", &accScale.z},
+	{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
 	// estimate
 	{"EST_ACC_WEIGHT", &accWeight},
 	{"EST_RATES_LPF_A", &ratesFilter.alpha},
+	// motors
+	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT]},
+	{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT]},
+	{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT]},
+	{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT]},
+	{"MOT_PWM_FREQ", &pwmFrequency},
+	{"MOT_PWM_RES", &pwmResolution},
+	{"MOT_PWM_STOP", &pwmStop},
+	{"MOT_PWM_MIN", &pwmMin},
+	{"MOT_PWM_MAX", &pwmMax},
 	// rc
 	{"RC_ZERO_0", &channelZero[0]},
 	{"RC_ZERO_1", &channelZero[1]},
@@ -91,6 +106,9 @@ Parameter parameters[] = {
 	{"MAV_SYS_ID", &mavlinkSysId},
 	{"MAV_RATE_SLOW", &telemetrySlow.rate},
 	{"MAV_RATE_FAST", &telemetryFast.rate},
+	// safety
+	{"SF_RC_LOSS_TIME", &rcLossTimeout},
+	{"SF_DESCEND_TIME", &descendTime},
 };
 
 void setupParameters() {
@@ -121,7 +139,7 @@ float getParameter(int index) {
 
 float getParameter(const char *name) {
 	for (auto &parameter : parameters) {
-		if (strcmp(parameter.name, name) == 0) {
+		if (strcasecmp(parameter.name, name) == 0) {
 			return parameter.getValue();
 		}
 	}
@@ -130,7 +148,7 @@ float getParameter(const char *name) {
 
 bool setParameter(const char *name, const float value) {
 	for (auto &parameter : parameters) {
-		if (strcmp(parameter.name, name) == 0) {
+		if (strcasecmp(parameter.name, name) == 0) {
 			if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
 			parameter.setValue(value);
 			return true;

flix/rc.ino

@@ -13,10 +13,10 @@ float channelZero[16]; // calibration zero values
 float channelMax[16]; // calibration max values
 
 float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
-float controlMode = NAN; //
-float controlTime; // time of the last controls update (0 when no RC)
+float controlMode = NAN;
+float controlTime = NAN; // time of the last controls update
 
-// Channels mapping (using float to store in parameters):
+// Channels mapping (nan means not assigned):
 float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
 
 void setupRC() {

flix/safety.ino

@@ -3,12 +3,12 @@
 
 // Fail-safe functions
 
-#define RC_LOSS_TIMEOUT 1
-#define DESCEND_TIME 10
-
 extern float controlTime;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
+float rcLossTimeout = 1;
+float descendTime = 10;
+
 void failsafe() {
 	rcLossFailsafe();
 	autoFailsafe();
@@ -16,9 +16,8 @@ void failsafe() {
 
 // RC loss failsafe
 void rcLossFailsafe() {
-	if (controlTime == 0) return; // no RC at all
 	if (!armed) return;
-	if (t - controlTime > RC_LOSS_TIMEOUT) {
+	if (t - controlTime > rcLossTimeout) {
 		descend();
 	}
 }
@@ -27,7 +26,7 @@ void rcLossFailsafe() {
 void descend() {
 	mode = AUTO;
 	attitudeTarget = Quaternion();
-	thrustTarget -= dt / DESCEND_TIME;
+	thrustTarget -= dt / descendTime;
 	if (thrustTarget < 0) {
 		thrustTarget = 0;
 		armed = false;

flix/wifi.ino

@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Wi-Fi connectivity
+// Wi-Fi communication
 
 #include <WiFi.h>
 #include <WiFiAP.h>

gazebo/flix.h

@@ -9,6 +9,7 @@
 #include "quaternion.h"
 #include "Arduino.h"
 #include "wifi.h"
+#include "lpf.h"
 
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -19,6 +20,7 @@ extern float motors[4];
 
 Vector gyro, acc, imuRotation;
 Vector accBias, gyroBias, accScale(1, 1, 1);
+LowPassFilter<Vector> gyroBiasFilter(0);
 
 // declarations
 void step();
@@ -32,6 +34,7 @@ void controlRates();
 void controlTorque();
 const char* getModeName();
 void sendMotors();
+int getDutyCycle(float value);
 bool motorsActive();
 void testMotor(int n);
 void print(const char* format, ...);
@@ -68,6 +71,7 @@ void resetParameters();
 void setLED(bool on) {};
 void calibrateGyro() { print("Skip gyro calibrating\n"); };
 void calibrateAccel() { print("Skip accel calibrating\n"); };
+void calibrateLevel() { print("Skip level calibrating\n"); };
 void printIMUCalibration() { print("cal: N/A\n"); };
 void printIMUInfo() {};
 void printWiFiInfo() {};

gazebo/wifi.h

@@ -11,9 +11,10 @@
 #include <sys/poll.h>
 #include <gazebo/gazebo.hh>
 
-#define WIFI_UDP_PORT 14580
-#define WIFI_UDP_REMOTE_PORT 14550
-#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
+int wifiMode = 1; // mock
+int udpLocalPort = 14580;
+int udpRemotePort = 14550;
+const char *udpRemoteIP = "255.255.255.255";
 
 int wifiSocket;
 
@@ -22,22 +23,22 @@ void setupWiFi() {
 	sockaddr_in addr; // local address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_ANY;
-	addr.sin_port = htons(WIFI_UDP_PORT);
+	addr.sin_port = htons(udpLocalPort);
 	if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
-		gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT << std::endl;
+		gzerr << "Failed to bind WiFi UDP socket on port " << udpLocalPort << std::endl;
 		return;
 	}
 	int broadcast = 1;
 	setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
-	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
+	gzmsg << "WiFi UDP socket initialized on port " << udpLocalPort << " (remote port " << udpRemotePort << ")" << std::endl;
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
 	if (wifiSocket == 0) setupWiFi();
 	sockaddr_in addr; // remote address
 	addr.sin_family = AF_INET;
-	addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
-	addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
+	addr.sin_addr.s_addr = inet_addr(udpRemoteIP);
+	addr.sin_port = htons(udpRemotePort);
 	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
 }
 

tools/grab_log.py

@@ -13,7 +13,7 @@ lines = []
 
 print('Downloading log...')
 count = 0
-dev.write('log\n'.encode())
+dev.write('log dump\n'.encode())
 while True:
     line = dev.readline()
     if not line:

tools/log.py

@@ -43,6 +43,7 @@ records = [record for record in records if record[0] != 0]
 
 print(f'Received records: {len(records)}')
 
+os.makedirs(f'{DIR}/log', exist_ok=True)
 log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
 log.write(header.encode() + b'\n')
 for record in records:

tools/pyflix/README.md

@@ -92,17 +92,17 @@ Full list of events:
 |-----|-----------|----------------|
 |`connected`|Connected to the drone||
 |`disconnected`|Connection is lost||
-|`armed`|Armed state update|Armed state (*bool*)|
-|`mode`|Flight mode update|Flight mode (*str*)|
-|`landed`|Landed state update|Landed state (*bool*)|
+|`armed`|Armed state update|Armed state *(bool)*|
+|`mode`|Flight mode update|Flight mode *(str)*|
+|`landed`|Landed state update|Landed state *(bool)*|
 |`print`|The drone prints text to the console|Text|
-|`attitude`|Attitude update|Attitude quaternion (*list*)|
-|`attitude_euler`|Attitude update|Euler angles (*list*)|
-|`rates`|Angular rates update|Angular rates (*list*)|
-|`channels`|Raw RC channels update|Raw RC channels (*list*)|
-|`motors`|Motor outputs update|Motor outputs (*list*)|
-|`acc`|Accelerometer update|Accelerometer output (*list*)|
-|`gyro`|Gyroscope update|Gyroscope output (*list*)|
+|`attitude`|Attitude update|Attitude quaternion *(list)*|
+|`attitude_euler`|Attitude update|Euler angles *(list)*|
+|`rates`|Angular rates update|Angular rates *(list)*|
+|`channels`|Raw RC channels update|Raw RC channels *(list)*|
+|`motors`|Motor outputs update|Motor outputs *(list)*|
+|`acc`|Accelerometer update|Accelerometer output *(list)*|
+|`gyro`|Gyroscope update|Gyroscope output *(list)*|
 |`mavlink`|Received MAVLink message|Message object|
 |`mavlink.<message_name>`|Received specific MAVLink message|Message object|
 |`mavlink.<message_id>`|Received specific MAVLink message|Message object|
@@ -277,7 +277,3 @@ logger = logging.getLogger('flix')
 logger.setLevel(logging.DEBUG)  # be more verbose
 logger.setLevel(logging.WARNING)  # be less verbose
 ```
-
-## Stability
-
-The library is in development stage. The API is not stable.