Make some variables static

.gitignore

@@ -4,10 +4,9 @@ build/
 tools/log/
 tools/dist/
 *.egg-info/
-.core
+.dependencies
-.libs
 .vscode/*
-!.vscode/settings.default.json
+!.vscode/settings.json
 !.vscode/c_cpp_properties.json
 !.vscode/tasks.json
 !.vscode/launch.json

.vscode/c_cpp_properties.json

@@ -6,34 +6,20 @@
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
 				"${workspaceFolder}/tools/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
-				"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
+				"~/.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.3.10/cores/esp32/Arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_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",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino",
-				"${workspaceFolder}/flix/safety.ino"
 			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2601/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,34 +39,20 @@
 			"name": "Mac",
 			"includePath": [
 				"${workspaceFolder}/flix",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
+				"~/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.3.10/cores/esp32/Arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_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",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino",
-				"${workspaceFolder}/flix/safety.ino"
 			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2601/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,32 +75,18 @@
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
 				"${workspaceFolder}/tools/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/libraries/**",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.10/include/**",
+				"~/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.3.10/cores/esp32/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.10/variants/d1_mini32/pins_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",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino",
-				"${workspaceFolder}/flix/safety.ino"
 			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2601/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": [

.vscode/extensions.json

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

Makefile

@@ -1,32 +1,24 @@
-BOARD = esp32:esp32:d1_mini32:DebugLevel=error
+BOARD = esp32:esp32:d1_mini32
 PORT := $(strip $(wildcard /dev/serial/by-id/usb-Silicon_Labs_CP21* /dev/serial/by-id/usb-1a86_USB_Single_Serial_* /dev/cu.usbserial-* /dev/cu.usbmodem*))
 
-export ARDUINO_NETWORK_CONNECTION_TIMEOUT := 1h
+build: .dependencies
-
-build: .core .libs
 	arduino-cli compile --fqbn $(BOARD) flix
 
 upload: build
 	arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" flix
 
-erase:
-	arduino-cli burn-bootloader --fqbn $(BOARD) -p "$(PORT)" -P esptool
-
 monitor:
 	arduino-cli monitor -p "$(PORT)" -c baudrate=115200
 
-core .core:
+dependencies .dependencies:
-	arduino-cli core update-index --additional-urls https://espressif.github.io/arduino-esp32/package_esp32_index.json
+	arduino-cli core update-index --config-file arduino-cli.yaml
-	arduino-cli core install esp32:esp32@3.3.10 --additional-urls https://espressif.github.io/arduino-esp32/package_esp32_index.json
+	arduino-cli core install esp32:esp32@3.3.6 --config-file arduino-cli.yaml
-	touch .core
-
-libs .libs:
 	arduino-cli lib update-index
 	arduino-cli lib install "FlixPeriph"
 	arduino-cli lib install "MAVLink"@2.0.25
-	touch .libs
+	touch .dependencies
 
-upload_proxy: .core .libs
+upload_proxy: .dependencies
 	arduino-cli compile --fqbn $(BOARD) tools/espnow-proxy
 	arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" tools/espnow-proxy
 
@@ -34,7 +26,7 @@ gazebo/build cmake: gazebo/CMakeLists.txt
 	mkdir -p gazebo/build
 	cd gazebo/build && cmake ..
 
-build_simulator: .libs gazebo/build
+build_simulator: .dependencies gazebo/build
 	make -C gazebo/build
 
 simulator: build_simulator
@@ -49,6 +41,6 @@ plot:
 	plotjuggler -d $(shell ls -t tools/log/*.csv | head -n1)
 
 clean:
-	rm -rf gazebo/build flix/build flix/cache .core .libs
+	rm -rf gazebo/build flix/build flix/cache .dependencies
 
-.PHONY: build upload monitor core libs cmake build_simulator simulator log clean
+.PHONY: build upload monitor dependencies cmake build_simulator simulator log clean

README.md

@@ -47,14 +47,6 @@ See the [user builds gallery](docs/user.md):
 
 <a href="docs/user.md"><img src="docs/img/user/user.jpg" width=500></a>
 
-### PCB
-
-The official PCB *(Flix2)* is in development now. Follow the [project's channel](https://t.me/opensourcequadcopter) to track the progress.
-
-Outdoor flights demo video of the current prototype:
-
-<a href="https://youtu.be/KXlNmvUTi4g"><img width=300 src="https://i3.ytimg.com/vi/KXlNmvUTi4g/maxresdefault.jpg"></a>
-
 ## Simulation
 
 The simulator is implemented using Gazebo and runs the original Arduino code:
@@ -81,7 +73,7 @@ Additional articles:
 |-|-|:-:|:-:|
 |Microcontroller board|ESP32 Mini.<br>ESP32-S3/ESP32-C3 boards are also supported.|<img src="docs/img/esp32.jpg" width=100>|1|
 |IMU (and barometer¹) board|GY‑91, MPU-9265 (or other MPU‑9250/MPU‑6500 board)<br>ICM20948V2 (ICM‑20948)<br>GY-521 (MPU-6050)|<img src="docs/img/gy-91.jpg" width=90 align=center><br><img src="docs/img/icm-20948.jpg" width=100><br><img src="docs/img/gy-521.jpg" width=100>|1|
-|*Boost converter (optional, for more stable power supply)*|*5V output*|<img src="docs/img/buck-boost.jpg" width=100>|1|
+|Boost converter (optional, for more stable power supply)|5V output|<img src="docs/img/buck-boost.jpg" width=100>|1|
 |Motor|8520 3.7V brushed motor.<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).<br>Make sure the motor shaft diameter and propeller hole diameter match!|<img src="docs/img/motor.jpeg" width=100>|4|
 |Propeller|55 mm or 65 mm|<img src="docs/img/prop.jpg" width=100>|4|
 |MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|

arduino-cli.yaml

@@ -0,0 +1,5 @@
+board_manager:
+  additional_urls:
+    - https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json
+network:
+  connection_timeout: 1h

docs/firmware.md

@@ -79,9 +79,6 @@ To add a new parameter:
 
 See examples of adding new parameters in commits: [c434107](https://github.com/okalachev/flix/commit/c434107), [a687303](https://github.com/okalachev/flix/commit/a687303).
 
-> [!NOTE]
-> Since all the parameters are internally stored and passed as floats, the safe range for `int` parameters is -16777216 to 16777215.
-
 ## Adding a subsystem
 
 To add a new subsystem:

docs/troubleshooting.md

@@ -5,7 +5,7 @@
 Do the following:
 
 * **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#building-the-firmware).
-* **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU-9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
+* **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
 * **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
 
 ## The drone doesn't fly

docs/usage.md

@@ -20,14 +20,13 @@ 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.3.10. 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.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. Set *Tools* ⇒ *Core Debug Level* to *Error* to see the errors in the serial console. Set *Tools* ⇒ *USB CDC on Boot* to *Enabled* for ESP32-S3/ESP32-C3 boards.
+7. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
-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)
 
@@ -58,12 +57,6 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
    make upload monitor
    ```
 
-   For ESP32-S3/ESP32-C3 boards, set the appropriate [FQBN](https://docs.arduino.cc/arduino-cli/FAQ/#whats-the-fqbn-string) using `BOARD` parameter:
-
-   ```bash
-   make BOARD=esp32:esp32:esp32s3:DebugLevel=error,FlashSize=4M,CDCOnBoot=cdc upload
-   ```
-
 See other available Make commands in [Makefile](../Makefile).
 
 > [!TIP]
@@ -89,9 +82,6 @@ QGroundControl is a ground control station software that can be used to monitor
 3. Connect your computer or smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
 4. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
 
-> [!TIP]
-> If QGroundControl doesn't connect, try to disable the firewall and/or VPN on your computer, as they may block the connection.
-
 ### Access console
 
 The console is a command line interface (CLI) that allows to interact with the drone, change parameters, and perform various actions. There are two ways of accessing the console: using **serial port** or using **QGroundControl (wirelessly)**.
@@ -339,7 +329,7 @@ To setup ESP-NOW communication:
    espnow 7a:c8:e3:eb:bf:e9 &PiuSysxP9+$L&5E
    ```
 
-   Run this line as a console command on each drone you want to bind to this proxy board. [The maximum number](https://github.com/espressif/esp-idf/blob/e95cab4be8fd293e3f3323181e7a2280874da6f7/components/esp_wifi/include/esp_now.h#L32-L33) of simultaneously connected drones is 20 (unencrypted) io 6 (encrypted).
+   Run this line as a console command on each drone you want to bind to this proxy board.
 
 3. Set the `WIFI_MODE` parameter to `3` on the drone:
 

docs/user.md

@@ -4,36 +4,6 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
-Author: [Неруш Михаил](https://t.me/NerushMV).<br>
-Description: custom frame made of 4 mm plywood, 8520 brushed motors, 75 mm propellers, MPU-6500. FlySky FS-i6X with ESP32-based adapter for ESP-NOW communication (using PPM output).
-
-<img src="img/user/nerush/1.jpg" height=200> <img src="img/user/nerush/2.jpg" height=200>
-
-[Flight video](https://drive.google.com/file/d/1jRXeGx34lJpUfw0GKLQeIzkWZvooQJSE/view?usp=sharing).
-
----
-
-Author: [Konstantinos Paraskevas](https://github.com/Frapais).<br>
-Description: drone with a custom single-boarded airframe, extending the [Sprig-C3 module](https://github.com/Frapais/Sprig-C3).
-ESP32-C3 microcontroller, ICM-20948 IMU, on-board fuel-gauge, status LED indicator.<br>
-Repository with all the code and PCB sources: https://github.com/Frapais/Sprig-Drone.
-
-<img src="img/user/kostas/1.jpg" height=150> <img src="img/user/kostas/2.jpg" height=150>
-
-Detailed video about making the drone:
-
-<a href="https://youtu.be/82Q-uBq6s48"><img width=400 src="https://i3.ytimg.com/vi/82Q-uBq6s48/maxresdefault.jpg"></a>
-
----
-
-Author: [Awab Anas](http://t.me/AW_VENOM).<br>
-Description: ESP32 D1 Mini, MPU-6050, 8520 3.7V brushed motors, 55 mm propellers, battery li-po 1200 mAh, controlling via [Mavlink Joystick app](https://github.com/goldarte/mavlink-joystick/releases/latest).<br>
-[Flight validation](https://drive.google.com/file/d/12z0jfctZDBA6b5UKCG0Uje5rAxj6DhF-/view?usp=sharing).
-
-<img src="img/user/aw_venom/1.jpg" height=200>
-
----
-
 Author: [Ina Tix](https://t.me/ina_tix).<br>
 Description: XR2981 based DC-DC converter, ELRS MINI 2.4GHz RX SX1280 receiver (SBUS interface), Radiomaster TX12 remote control.<br>
 [Flight validation](https://drive.google.com/file/d/1yqkKNuz4R_yxGqUNQxVpixJbXqEEcUSj/view?usp=share_link).
@@ -87,17 +57,6 @@ Author: [goldarte](https://t.me/goldarte).<br>
 
 ---
 
-Author: [malagis](https://oshwhub.com/malagis).<br>
-
-A Chinese custom PCB version of Flix with a big community of users, lots of materials and modifications.
-
-Main project's page: https://oshwhub.com/malagis/esp32-mini-plane.<br>
-Video about the project: https://www.bilibili.com/video/BV14vyqBFEJn/.
-
-<img src="img/user/malagis/1.jpg" height=200> <img src="img/user/malagis/2.jpg" height=200> <img src="img/user/malagis/3.jpg" height=200>
-
----
-
 ## School 548 course
 
 Special course on quadcopter design and engineering took place in october-november 2025 in School 548, Moscow. The course included UAV control theory, electronics, drone assembly and setup practice, using the Flix project.

flix/cli.cpp

@@ -3,6 +3,8 @@
 
 // Implementation of command line interface
 
+#include <Arduino.h>
+#include "flix.h"
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
@@ -31,33 +33,33 @@ const char* motd =
 "Commands:\n\n"
 "help - show help\n"
 "p - show all parameters\n"
-"p <str> - show parameters starting with str\n"
+"p <name> - show parameter\n"
 "p <name> <value> - set parameter\n"
 "preset - reset parameters\n"
 "time - show time info\n"
+"ps - show pitch/roll/yaw\n"
+"psq - show attitude quaternion\n"
 "imu - show IMU data\n"
-"ca - calibrate accel\n"
-"st - show state estimation\n"
 "arm - arm the drone\n"
 "disarm - disarm the drone\n"
 "raw/stab/acro/auto - set mode\n"
 "rc - show RC data\n"
-"cr - calibrate RC\n"
 "pw - show power info\n"
 "wifi - show Wi-Fi info\n"
-"wifi ap/sta/espnow/off - set Wi-Fi mode\n"
+"ap <ssid> <password> - setup Wi-Fi access point\n"
-"ap <ssid> <password> - configure Wi-Fi access point\n"
+"sta <ssid> <password> - setup Wi-Fi client mode\n"
-"sta <ssid> <password> - configure Wi-Fi client mode\n"
+"espnow <mac> [<key>] - setup ESP-NOW peer\n"
-"espnow <mac> [<key>] - configure ESP-NOW peer\n"
 "mot - show motor output\n"
 "log [dump] - print log header [and data]\n"
+"cr - calibrate RC\n"
+"ca - calibrate accel\n"
 "mfr, mfl, mrr, mrl - test motor (remove props)\n"
 "sys - show system info\n"
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
 
 void print(const char* format, ...) {
-	char buf[3000];
+	char buf[1000];
 	va_list args;
 	va_start(args, format);
 	vsnprintf(buf, sizeof(buf), format, args);
@@ -76,7 +78,7 @@ void pause(float duration) {
 	}
 }
 
-void doCommand(String str, bool echo = false) {
+void doCommand(String str, bool echo) {
 	// parse command
 	String command, arg0, arg1;
 	splitString(str, command, arg0, arg1);
@@ -92,8 +94,10 @@ void doCommand(String str, bool echo = false) {
 	// execute command
 	if (command == "help" || command == "motd") {
 		print("%s\n", motd);
-	} else if (command == "p" && arg1 == "") {
+	} else if (command == "p" && arg0 == "") {
-		printParameters(arg0.c_str());
+		printParameters();
+	} else if (command == "p" && arg0 != "" && arg1 == "") {
+		print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
 	} else if (command == "p") {
 		bool success = setParameter(arg0.c_str(), arg1.toFloat());
 		if (success) {
@@ -107,15 +111,15 @@ void doCommand(String str, bool echo = false) {
 		print("Time: %f\n", t);
 		print("Loop rate: %.0f\n", loopRate);
 		print("dt: %f\n", dt);
+	} else if (command == "ps") {
+		Vector a = attitude.toEuler();
+		print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
+	} else if (command == "psq") {
+		print("qw: %f qx: %f qy: %f qz: %f\n", attitude.w, attitude.x, attitude.y, attitude.z);
 	} else if (command == "imu") {
 		printIMUInfo();
 		printIMUCalibration();
 		print("landed: %d\n", landed);
-	} else if (command == "st") {
-		print("rates: %g %g %g\n", rates.x, rates.y, rates.z);
-		print("attitude: %g %g %g %g\n", attitude.w, attitude.x, attitude.y, attitude.z);
-		print("roll: %g° pitch: %g° yaw: %g°\n", degrees(attitude.getRoll()), degrees(attitude.getPitch()), degrees(attitude.getYaw()));
-		print("landed: %d\n", landed);
 	} else if (command == "arm") {
 		armed = true;
 	} else if (command == "disarm") {
@@ -140,10 +144,8 @@ void doCommand(String str, bool echo = false) {
 		print("armed: %d\n", armed);
 	} else if (command == "pw") {
 		print("Voltage: %.1f V\n", voltage);
-	} else if (command == "wifi" && arg0 == "") {
-		printWiFiInfo();
 	} else if (command == "wifi") {
-		setWiFiMode(arg0);
+		printWiFiInfo();
 	} else if (command == "ap") {
 		configWiFi(W_AP, arg0.c_str(), arg1.c_str());
 	} else if (command == "sta") {
@@ -172,11 +174,10 @@ void doCommand(String str, bool echo = false) {
 #ifdef ESP32
 		print("Chip: %s\n", ESP.getChipModel());
 		print("Temperature: %.1f °C\n", temperatureRead());
-		print("Total RAM: %d KB\n", ESP.getHeapSize() / 1024);
+		print("Free heap: %d\n", ESP.getFreeHeap());
-		print("Free heap: %d KB\n", ESP.getFreeHeap() / 1024);
 		print("Firmware: " __DATE__ " " __TIME__ "\n");
 		// Print tasks table
-		print("Num  Task                MinSt  Prio  Core  CPU%%\n");
+		print("Num  Task                Stack  Prio  Core  CPU%%\n");
 		int taskCount = uxTaskGetNumberOfTasks();
 		TaskStatus_t *systemState = new TaskStatus_t[taskCount];
 		uint32_t totalRunTime;
@@ -210,7 +211,7 @@ void handleInput() {
 
 	while (Serial.available()) {
 		char c = Serial.read();
-		if (c == '\n' || c == '\r') {
+		if (c == '\n') {
 			doCommand(input);
 			input.clear();
 		} else {

flix/config.h

@@ -0,0 +1,55 @@
+// Wi-Fi
+#define WIFI_ENABLED 1
+#define WIFI_SSID "flix"
+#define WIFI_PASSWORD "flixwifi"
+#define WIFI_UDP_PORT 14550
+#define WIFI_UDP_REMOTE_PORT 14550
+#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
+
+// Motors
+#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
+
+// Control
+#define PITCHRATE_P 0.05
+#define PITCHRATE_I 0.2
+#define PITCHRATE_D 0.001
+#define PITCHRATE_I_LIM 0.3
+#define ROLLRATE_P PITCHRATE_P
+#define ROLLRATE_I PITCHRATE_I
+#define ROLLRATE_D PITCHRATE_D
+#define ROLLRATE_I_LIM PITCHRATE_I_LIM
+#define YAWRATE_P 0.3
+#define YAWRATE_I 0.0
+#define YAWRATE_D 0.0
+#define YAWRATE_I_LIM 0.3
+#define ROLL_P 6
+#define ROLL_I 0
+#define ROLL_D 0
+#define PITCH_P ROLL_P
+#define PITCH_I ROLL_I
+#define PITCH_D ROLL_D
+#define YAW_P 3
+#define PITCHRATE_MAX radians(360)
+#define ROLLRATE_MAX radians(360)
+#define YAWRATE_MAX radians(300)
+#define TILT_MAX radians(30)
+#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
+
+// Estimation
+#define WEIGHT_ACC 0.003
+#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
+
+// MAVLink
+#define SYSTEM_ID 1
+
+// Safety
+#define RC_LOSS_TIMEOUT 1
+#define DESCEND_TIME 10

flix/control.cpp

@@ -3,38 +3,16 @@
 
 // Flight control
 
+#include "config.h"
+#include "flix.h"
 #include "vector.h"
 #include "quaternion.h"
 #include "pid.h"
 #include "lpf.h"
 #include "util.h"
 
-#define PITCHRATE_P 0.05
+extern const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
-#define PITCHRATE_I 0.2
-#define PITCHRATE_D 0.001
-#define PITCHRATE_I_LIM 0.3
-#define ROLLRATE_P PITCHRATE_P
-#define ROLLRATE_I PITCHRATE_I
-#define ROLLRATE_D PITCHRATE_D
-#define ROLLRATE_I_LIM PITCHRATE_I_LIM
-#define YAWRATE_P 0.3
-#define YAWRATE_I 0.0
-#define YAWRATE_D 0.0
-#define YAWRATE_I_LIM 0.3
-#define ROLL_P 6
-#define ROLL_I 0
-#define ROLL_D 0
-#define PITCH_P ROLL_P
-#define PITCH_I ROLL_I
-#define PITCH_D ROLL_D
-#define YAW_P 3
-#define PITCHRATE_MAX radians(360)
-#define ROLLRATE_MAX radians(360)
-#define YAWRATE_MAX radians(300)
-#define TILT_MAX radians(30)
-#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
 int mode = STAB;
 bool armed = false;
 
@@ -149,7 +127,6 @@ void controlTorque() {
 	motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
 	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
 
-	// Prioritize angle control over thrust control
 	desaturate(motors[MOTOR_FRONT_LEFT], motors[MOTOR_FRONT_RIGHT], motors[MOTOR_REAR_LEFT], motors[MOTOR_REAR_RIGHT]);
 
 	motors[0] = constrain(motors[0], 0, 1);

flix/estimate.cpp

@@ -3,6 +3,8 @@
 
 // Attitude estimation using gyro and accelerometer
 
+#include "config.h"
+#include "flix.h"
 #include "quaternion.h"
 #include "vector.h"
 #include "lpf.h"
@@ -32,7 +34,8 @@ void applyGyro() {
 
 void applyAcc() {
 	// test should we apply accelerometer gravity correction
-	landed = !motorsActive() && abs(acc.norm() - ONE_G) < ONE_G * 0.1f;
+	float accNorm = acc.norm();
+	landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
 
 	if (!landed) return;
 
@@ -46,7 +49,6 @@ void applyAcc() {
 
 void applyLevel() {
 	if (landed) return;
-	if (thrustTarget < 0.1) return; // skip at idle thrust
 
 	// assume the pilot keeps the drone more or less level in flight
 	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);

flix/flix.h

@@ -0,0 +1,95 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// All-in-one header file
+
+#pragma once
+
+#include <Arduino.h>
+#include "vector.h"
+#include "quaternion.h"
+
+extern float t, dt;
+extern float loopRate;
+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
+extern Vector gyro, acc;
+extern Vector rates;
+extern Quaternion attitude;
+extern bool landed;
+extern int mode;
+extern bool armed;
+extern Quaternion attitudeTarget;
+extern Vector ratesTarget, ratesExtra, torqueTarget;
+extern float thrustTarget;
+extern float motors[4];
+
+void print(const char* format, ...);
+void pause(float duration);
+void doCommand(String str, bool echo = false);
+void handleInput();
+void control();
+void interpretControls();
+void controlAttitude();
+void controlRates();
+void controlTorque();
+void desaturate(float& a, float& b, float& c, float& d);
+const char *getModeName();
+void estimate();
+void applyGyro();
+void applyAcc();
+void applyLevel();
+void setupIMU();
+void configureIMU();
+void readIMU();
+void rotateIMU(Vector& data);
+void calibrateGyroOnce();
+void calibrateAccel();
+void calibrateAccelOnce();
+void printIMUCalibration();
+void printIMUInfo();
+void setupLED();
+void setLED(bool on);
+void blinkLED();
+void prepareLogData();
+void logData();
+void printLogHeader();
+void printLogData();
+void processMavlink();
+void sendMavlink();
+void sendMessage(const void *msg);
+void receiveMavlink();
+void printWiFiInfo();
+void configWiFi(int mode, const char *first, const char *second);
+void handleMavlink(const void *_msg);
+void mavlinkPrint(const char* str);
+void sendMavlinkPrint();
+void setupMotors();
+int getDutyCycle(float value);
+void sendMotors();
+bool motorsActive();
+void testMotor(int n);
+void setupParameters();
+int parametersCount();
+const char *getParameterName(int index);
+float getParameter(int index);
+float getParameter(const char *name);
+bool setParameter(const char *name, const float value);
+void syncParameters();
+void printParameters();
+void resetParameters();
+void setupRC();
+bool readRC();
+void normalizeRC();
+void calibrateRC();
+void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const char *str);
+void printRCCalibration();
+void setupPower();
+void failsafe();
+void rcLossFailsafe();
+void descend();
+void autoFailsafe();
+void step();
+void computeLoopRate();
+void setupWiFi();
+void sendWiFi(const uint8_t *buf, int len);
+int receiveWiFi(uint8_t *buf, int len);

flix/flix.ino

@@ -3,17 +3,11 @@
 
 // Main firmware file
 
+#include "config.h"
 #include "vector.h"
 #include "quaternion.h"
 #include "util.h"
-
+#include "flix.h"
-extern float t, dt;
-extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
-extern Vector gyro, acc;
-extern Vector rates;
-extern Quaternion attitude;
-extern bool landed;
-extern float motors[4];
 
 void setup() {
 	Serial.begin(115200);

flix/imu.cpp

@@ -40,12 +40,10 @@ void readIMU() {
 	imu.getGyro(gyro.x, gyro.y, gyro.z);
 	imu.getAccel(acc.x, acc.y, acc.z);
 	calibrateGyroOnce();
-
+	// apply scale and bias
-	// Apply scale and bias
 	acc = (acc - accBias) / accScale;
 	gyro = gyro - gyroBias;
-
+	// rotate to body frame
-	// Rotate to body frame
 	Quaternion rotation = Quaternion::fromEuler(imuRotation);
 	acc = Quaternion::rotateVector(acc, rotation.inversed());
 	gyro = Quaternion::rotateVector(gyro, rotation.inversed());
@@ -54,7 +52,6 @@ void readIMU() {
 void calibrateGyroOnce() {
 	static Delay landedDelay(2);
 	if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
-
 	gyroBias = gyroBiasFilter.update(gyro);
 }
 
@@ -108,7 +105,6 @@ 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;
-
 	// Compute scale and bias
 	accScale = (accMax - accMin) / 2 / ONE_G;
 	accBias = (accMax + accMin) / 2;
@@ -125,7 +121,6 @@ void printIMUInfo() {
 	print("model: %s\n", imu.getModel());
 	print("who am I: 0x%02X\n", imu.whoAmI());
 	print("rate: %.0f\n", loopRate);
-	print("temperature: %.1f °C\n", imu.getTemp());
 	print("gyro: %f %f %f\n", gyro.x, gyro.y, gyro.z);
 	print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
 	imu.waitForData();

flix/led.cpp

@@ -3,6 +3,8 @@
 
 // Board's LED control
 
+#include <Arduino.h>
+
 #define BLINK_PERIOD 500000
 
 #ifndef LED_BUILTIN

flix/log.cpp

@@ -3,6 +3,7 @@
 
 // In-RAM logging
 
+#include "flix.h"
 #include "vector.h"
 #include "util.h"
 

flix/lpf.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include <Arduino.h>
+
 template <typename T> // Using template to make the filter usable for scalar and vector values
 class LowPassFilter {
 public:

flix/mavlink.cpp

@@ -3,22 +3,22 @@
 
 // MAVLink communication
 
+#include <Arduino.h>
 #include <MAVLink.h>
+#include "config.h"
 #include "util.h"
 
+extern const int RAW, ACRO, STAB, AUTO;
 extern float controlTime;
 extern float voltage;
+extern uint16_t channels[16];
 
 int mavlinkSysId = 1;
-
+Rate telemetryFast(10);
 Rate telemetrySlow(2);
-Rate telemetryAttitude(20);
-Rate telemetryRC(10);
-Rate telemetryMotors(10);
-Rate telemetryIMU(15);
 
 bool mavlinkConnected = false;
-String mavlinkPrintBuffer;
+static String mavlinkPrintBuffer;
 
 void processMavlink() {
 	sendMavlink();
@@ -38,46 +38,36 @@ void sendMavlink() {
 			((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
 			mode, MAV_STATE_STANDBY);
 		sendMessage(&msg);
-	}
 
 		if (!mavlinkConnected) return; // send only heartbeat until connected
 
-	if (telemetrySlow) {
 		mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 			MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
 		sendMessage(&msg);
-	}
 
-	if (telemetrySlow && valid(voltage)) {
+		uint16_t voltages[] = {voltage * 1000, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX};
-		uint16_t voltages[] = {(uint16_t)(voltage * 1000), UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX};
 		uint16_t voltagesExt[] = {0, 0, 0, 0};
 		float remaining = constrain(mapf(voltage, 3.4, 4.2, 0, 1), 0, 1);
 		mavlink_msg_battery_status_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, MAV_BATTERY_FUNCTION_ALL,
 			MAV_BATTERY_TYPE_LIPO, INT16_MAX, voltages, -1, -1, -1, remaining * 100, 0, MAV_BATTERY_CHARGE_STATE_OK, voltagesExt, 0, 0);
-		sendMessage(&msg);
+		if (valid(voltage)) sendMessage(&msg);
 	}
 
-	if (telemetryAttitude) {
+	if (telemetryFast && mavlinkConnected) {
 		const float offset[] = {0, 0, 0, 0};
 		mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, offset); // convert to frd
 		sendMessage(&msg);
-	}
 
-	if (telemetryRC && channels[0]) { // 0 means no RC input
 		mavlink_msg_rc_channels_raw_pack(mavlinkSysId, 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);
+		if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
-	}
 
-	if (telemetryMotors) {
 		float controls[8];
 		memcpy(controls, motors, sizeof(motors));
 		mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
 		sendMessage(&msg);
-	}
 
-	if (telemetryIMU) {
 		mavlink_msg_scaled_imu_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time,
 			acc.x / ONE_G * 1000, -acc.y / ONE_G * 1000, -acc.z / ONE_G * 1000, // convert to frd
 			gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
@@ -95,13 +85,13 @@ void sendMessage(const void *msg) {
 void receiveMavlink() {
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 	int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);
+	if (len) mavlinkConnected = true;
 
 	// New packet, parse it
 	mavlink_message_t msg;
 	mavlink_status_t status;
 	for (int i = 0; i < len; i++) {
 		if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
-			mavlinkConnected = true;
 			handleMavlink(&msg);
 		}
 	}
@@ -225,6 +215,7 @@ void handleMavlink(const void *_msg) {
 		armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
 	}
 
+	/* TODO:
 	if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
 		mavlink_log_request_data_t m;
 		mavlink_msg_log_request_data_decode(&msg, &m);
@@ -238,6 +229,7 @@ void handleMavlink(const void *_msg) {
 			sendMessage(&msg);
 		}
 	}
+	*/
 
 	// Handle commands
 	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
@@ -255,7 +247,7 @@ void handleMavlink(const void *_msg) {
 		}
 
 		if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
-			if (m.param1 == 1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
+			if (m.param1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
 			accepted = true;
 			armed = m.param1 == 1;
 		}

flix/motors.cpp

@@ -3,6 +3,9 @@
 
 // PWM control for motors
 
+#include <Arduino.h>
+#include "config.h"
+#include "flix.h"
 #include "util.h"
 
 float motors[4]; // normalized motor thrusts in range [0..1]
@@ -14,13 +17,12 @@ int pwmStop = 0;
 int pwmMin = 0;
 int pwmMax = -1; // -1 means duty cycle mode
 
-const int MOTOR_REAR_LEFT = 0, MOTOR_REAR_RIGHT = 1, MOTOR_FRONT_RIGHT = 2, MOTOR_FRONT_LEFT = 3;
+extern const int MOTOR_REAR_LEFT = 0, MOTOR_REAR_RIGHT = 1, MOTOR_FRONT_RIGHT = 2, MOTOR_FRONT_LEFT = 3;
 
 void setupMotors() {
 	print("Setup Motors\n");
-	// Configure pins
+	// configure pins
 	for (int i = 0; i < 4; i++) {
-		if (motorPins[i] < 0) continue; // skip unassigned motors
 		ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
 		pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // when reconfiguring
 	}
@@ -30,14 +32,12 @@ void setupMotors() {
 
 void sendMotors() {
 	for (int i = 0; i < 4; i++) {
-		if (motorPins[i] < 0) continue; // skip unassigned motors
 		ledcWrite(motorPins[i], getDutyCycle(motors[i]));
 	}
 }
 
 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;

flix/parameters.cpp

@@ -4,15 +4,32 @@
 // Parameters storage in flash memory
 
 #include <Preferences.h>
+#include "flix.h"
+#include "pid.h"
+#include "lpf.h"
 #include "util.h"
 
-extern int channelZero[16], channelMax[16];
+extern float channelZero[16], channelMax[16];
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern float tiltMax;
+extern int flightModes[3];
+extern PID rollPID, pitchPID, yawPID;
+extern PID rollRatePID, pitchRatePID, yawRatePID;
+extern Vector maxRate;
+extern Vector imuRotation;
+extern Vector accBias, accScale;
+extern float accWeight, levelWeight;
+extern LowPassFilter<Vector> gyroBiasFilter, ratesFilter, voltageFilter;
 extern int rcRxPin, voltagePin;
+extern int motorPins[4];
+extern int pwmFrequency, pwmResolution, pwmStop, pwmMin, pwmMax;
 extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel;
+extern int mavlinkSysId;
+extern Rate telemetrySlow, telemetryFast;
 extern float rcLossTimeout, descendTime;
+extern int voltagePin;
 extern float voltageScale;
-extern LowPassFilter<float> voltageFilter;
+extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 
 Preferences storage;
 
@@ -20,7 +37,6 @@ struct Parameter {
 	const char *name; // max length is 15
 	bool integer;
 	union { float *f; int *i; }; // pointer to the variable
-	float inital; // default value
 	float cache; // what's stored in flash
 	void (*callback)(); // called after parameter change
 	Parameter(const char *name, float *variable, void (*callback)() = nullptr) : name(name), integer(false), f(variable), callback(callback) {};
@@ -29,7 +45,7 @@ struct Parameter {
 	void setValue(const float value) { if (integer) *i = value; else *f = value; };
 };
 
-Parameter parameters[] = {
+static Parameter parameters[] = {
 	// control
 	{"CTL_R_RATE_P", &rollRatePID.p},
 	{"CTL_R_RATE_I", &rollRatePID.i},
@@ -117,10 +133,7 @@ Parameter parameters[] = {
 	// mavlink
 	{"MAV_SYS_ID", &mavlinkSysId},
 	{"MAV_RATE_SLOW", &telemetrySlow.rate},
-	{"MAV_RATE_ATT", &telemetryAttitude.rate},
+	{"MAV_RATE_FAST", &telemetryFast.rate},
-	{"MAV_RATE_RC", &telemetryRC.rate},
-	{"MAV_RATE_MOT", &telemetryMotors.rate},
-	{"MAV_RATE_IMU", &telemetryIMU.rate},
 	// power
 	{"PWR_VOLT_PIN", &voltagePin, setupPower},
 	{"PWR_VOLT_SCALE", &voltageScale},
@@ -138,7 +151,6 @@ void setupParameters() {
 		if (!storage.isKey(parameter.name)) {
 			storage.putFloat(parameter.name, parameter.getValue()); // store default value
 		}
-		parameter.inital = parameter.getValue();
 		parameter.setValue(storage.getFloat(parameter.name, 0));
 		parameter.cache = parameter.getValue();
 	}
@@ -185,23 +197,17 @@ void syncParameters() {
 	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
 
 	for (auto &parameter : parameters) {
-		if (floatEquals(parameter.getValue(), parameter.cache)) continue; // no change
+		if (parameter.getValue() == parameter.cache) continue; // no change
+		if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NAN
 
 		storage.putFloat(parameter.name, parameter.getValue());
 		parameter.cache = parameter.getValue(); // update cache
 	}
 }
 
-void printParameters(const char *filter) {
+void printParameters() {
-	print("Name             Value          [Default]\n");
 	for (auto &parameter : parameters) {
-		if (strncasecmp(parameter.name, filter, strlen(filter))) continue;
+		print("%s = %g\n", parameter.name, parameter.getValue());
-
-		if (floatEquals(parameter.getValue(), parameter.inital)) { // parameter changed
-			print("%-15s  %-13g\n", parameter.name, parameter.getValue());
-		} else {
-			print("%-15s  %-13g  [%g]\n", parameter.name, parameter.getValue(), parameter.inital);
-		}
 	}
 }
 

flix/pid.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include "Arduino.h"
+#include "flix.h"
 #include "lpf.h"
 
 class PID {

flix/power.ino

@@ -20,7 +20,6 @@ void setupPower() {
 
 void readVoltage() {
 	if (voltagePin < 0) return;
-
 	static Rate rate(10);
 	if (!rate) return;
 

flix/quaternion.h

@@ -5,6 +5,7 @@
 
 #pragma once
 
+#include <Arduino.h>
 #include "vector.h"
 
 class Quaternion : public Printable {

flix/rc.cpp

@@ -6,7 +6,7 @@
 #include <SBUS.h>
 #include "util.h"
 
-SBUS rc(Serial1);
+static SBUS rc(Serial1);
 int rcRxPin = -1; // -1 means disabled
 
 uint16_t channels[16]; // raw rc channels
@@ -27,12 +27,14 @@ void setupRC() {
 
 bool readRC() {
 	if (rcRxPin < 0) return false;
-	if (!rc.read()) return false;
+	if (rc.read()) {
-
+		SBUSData data = rc.data();
-	rc.getChannels(channels);
+		for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
 		normalizeRC();
 		controlTime = t;
 		return true;
+	}
+	return false;
 }
 
 void normalizeRC() {
@@ -53,7 +55,6 @@ void calibrateRC() {
 		print("RC_RX_PIN = %d, set the RC pin!\n", rcRxPin);
 		return;
 	}
-
 	uint16_t zero[16]; // for zero positions
 	uint16_t center[16]; // for center positions
 	uint16_t _[16]; // for unused data

flix/safety.cpp

@@ -3,8 +3,12 @@
 
 // Fail-safe functions
 
+#include "config.h"
+#include "flix.h"
+#include "util.h"
+
 extern float controlTime;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw;
+extern const int AUTO, STAB;
 
 float rcLossTimeout = 1;
 float descendTime = 10;

flix/time.cpp

@@ -3,6 +3,9 @@
 
 // Time related functions
 
+#include "Arduino.h"
+#include "flix.h"
+
 float t = NAN; // current time, s
 float dt; // time delta with the previous step, s
 float loopRate; // Hz

flix/util.h

@@ -7,30 +7,24 @@
 
 #include <math.h>
 #include <ESP32_NOW_Serial.h>
+#include "flix.h"
 
 const float ONE_G = 9.80665;
-extern float t;
 
-float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
+inline float mapf(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;
 }
 
-bool invalid(float x) {
+inline bool invalid(float x) {
 	return !isfinite(x);
 }
 
-bool valid(float x) {
+inline bool valid(float x) {
 	return isfinite(x);
 }
 
-bool floatEquals(float a, float b, float epsilon = 0) {
-	if (isnan(a) && isnan(b)) return true;
-	if (a == b) return true;
-	return fabsf(a - b) <= epsilon;
-}
-
 // Wrap angle to [-PI, PI)
-float wrapAngle(float angle) {
+inline float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);
 	if (angle > PI) {
 		angle -= 2 * PI;
@@ -41,7 +35,7 @@ float wrapAngle(float angle) {
 }
 
 // Trim and split string by spaces
-void splitString(String& str, String& token0, String& token1, String& token2) {
+inline void splitString(String& str, String& token0, String& token1, String& token2) {
 	str.trim();
 	if (str.isEmpty()) return;
 	char chars[str.length() + 1];
@@ -53,14 +47,13 @@ void splitString(String& str, String& token0, String& token1, String& token2) {
 	if (token2.c_str() == NULL) token2 = "";
 }
 
-// Simplified ESP-NOW Serial without resends
+// Simplified ESP-NOW Serial without tx buffering and resends
 class ESPNOWSerial : public ESP_NOW_Serial_Class {
 public:
-	int lost = 0;
 	using ESP_NOW_Serial_Class::ESP_NOW_Serial_Class;
-	void onSent(bool success) override {
+	void onSent(bool success) override {} // disable resends
-		if (!success) lost++;
+	size_t write(const uint8_t *data, size_t len) override {
-		ESP_NOW_Serial_Class::onSent(true); // always report success to avoid resends
+		return ESP_NOW_Peer::send(data, len); // pure send without buffering
 	}
 };
 
@@ -72,9 +65,6 @@ public:
 	Rate(float rate) : rate(rate) {}
 
 	operator bool() {
-		if (t == last) {
-			return true; // the same step
-		}
 		if (t - last >= 1 / rate) {
 			last = t;
 			return true;

flix/vector.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include <Arduino.h>
+
 class Vector : public Printable {
 public:
 	float x, y, z;
@@ -136,5 +138,5 @@ public:
 	}
 };
 
-Vector operator * (const float a, const Vector& b) { return b * a; }
+inline Vector operator * (const float a, const Vector& b) { return b * a; }
-Vector operator + (const float a, const Vector& b) { return b + a; }
+inline Vector operator + (const float a, const Vector& b) { return b + a; }

flix/wifi.cpp

@@ -9,21 +9,24 @@
 #include <MacAddress.h>
 #include <ESP32_NOW_Serial.h>
 #include <Preferences.h>
+#include "config.h"
+#include "flix.h"
 #include "util.h"
 
 extern Preferences storage; // use the main preferences storage
+extern bool mavlinkConnected;
 
-const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
+extern const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
 int wifiMode = W_AP;
 
 int wifiLongRange = 0;
 int udpLocalPort = 14550;
 int udpRemotePort = 14550;
-IPAddress udpRemoteIP = "255.255.255.255";
+static IPAddress udpRemoteIP = "255.255.255.255";
-WiFiUDP udp;
+static WiFiUDP udp;
 
-ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
+static ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
-ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
+static ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
 int espnowChannel = 6;
 
 void setupWiFi() {
@@ -33,14 +36,10 @@ void setupWiFi() {
 	if (wifiMode == W_AP) {
 		WiFi.softAP(storage.getString("WIFI_AP_SSID", "flix").c_str(), storage.getString("WIFI_AP_PASS", "flixwifi").c_str());
 		udp.begin(udpLocalPort);
-	}
+	} else if (wifiMode == W_STA) {
-
-	if (wifiMode == W_STA) {
 		WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str());
 		udp.begin(udpLocalPort);
-	}
+	} else if (wifiMode == W_ESPNOW) {
-
-	if (wifiMode == W_ESPNOW) {
 		WiFi.mode(WIFI_AP);
 		WiFi.setChannel(espnowChannel);
 		espnow.addr(MacAddress(storage.getString("ESPNOW_PEER_MAC", "FF:FF:FF:FF:FF:FF").c_str()));
@@ -56,7 +55,6 @@ void setupWiFi() {
 void sendWiFi(const uint8_t *buf, int len) {
 	if (espnow) {
 		espnow.write(buf, len);
-
 		static Rate discovery(2);
 		if (discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
 		return;
@@ -90,7 +88,6 @@ void printWiFiInfo() {
 		print("Peer MAC: %s\n", MacAddress(espnow.addr()).toString().c_str());
 		print("Encrypted: %d\n", espnow.isEncrypted());
 		print("Channel: %d\n", espnow.getChannel());
-		print("Lost packets: %d\n", espnow.lost);
 	} else if (WiFi.getMode() == WIFI_MODE_AP) {
 		print("Mode: Access Point (AP)\n");
 		print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
@@ -133,20 +130,3 @@ void configWiFi(int mode, const char *first, const char *second) {
 	}
 	print("✓ Reboot to apply new settings\n");
 }
-
-void setWiFiMode(const String& mode) {
-	if (mode == "ap") {
-		wifiMode = W_AP;
-	} else if (mode == "sta") {
-		wifiMode = W_STA;
-	} else if (mode == "espnow") {
-		wifiMode = W_ESPNOW;
-	} else if (mode == "off") {
-		wifiMode = W_DISABLED;
-	} else {
-		print("Invalid Wi-Fi mode\n");
-		return;
-	}
-	static const char *modes[] = {"Disabled", "Access Point (AP)", "Client (STA)", "ESP-NOW"};
-	print("✓ Wi-Fi mode set to %s, reboot to apply\n", modes[wifiMode]);
-}

gazebo/CMakeLists.txt

@@ -10,9 +10,23 @@ list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
 
 set(FLIX_SOURCE_DIR ../flix)
 include_directories(${FLIX_SOURCE_DIR})
+set(FLIX_SOURCE_DIR ../flix)
+include_directories(${FLIX_SOURCE_DIR})
+set(FLIX_SOURCES
+  ${FLIX_SOURCE_DIR}/cli.cpp
+  ${FLIX_SOURCE_DIR}/control.cpp
+  ${FLIX_SOURCE_DIR}/estimate.cpp
+  ${FLIX_SOURCE_DIR}/safety.cpp
+  ${FLIX_SOURCE_DIR}/log.cpp
+  ${FLIX_SOURCE_DIR}/mavlink.cpp
+  ${FLIX_SOURCE_DIR}/motors.cpp
+  ${FLIX_SOURCE_DIR}/parameters.cpp
+  ${FLIX_SOURCE_DIR}/rc.cpp
+  ${FLIX_SOURCE_DIR}/time.cpp
+)
 
 set(CMAKE_BUILD_TYPE RelWithDebInfo)
-add_library(flix SHARED simulator.cpp)
+add_library(flix SHARED simulator.cpp ${FLIX_SOURCES})
 target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})
 target_include_directories(flix PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_compile_options(flix PRIVATE -Wno-address-of-packed-member) # disable unneeded mavlink warnings

gazebo/SBUS.h

@@ -15,11 +15,12 @@ public:
 	SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
 	void begin(int rxpin = -1, int txpin = -1, bool inv = true, bool fast = false) {};
 	bool read() { return joystickInit(); };
-	void getChannels(uint16_t (&channels)[16]) const {
+	SBUSData data() {
-		int16_t ch[16];
+		SBUSData data;
-		joystickGet(ch);
+		joystickGet(data.ch);
 		for (int i = 0; i < 16; i++) {
-			channels[i] = map(ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
+			data.ch[i] = map(data.ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
 		}
+		return data;
 	};
 };

gazebo/flix.h

@@ -68,7 +68,7 @@ const char *getParameterName(int index);
 float getParameter(int index);
 float getParameter(const char *name);
 bool setParameter(const char *name, const float value);
-void printParameters(const char *filter);
+void printParameters();
 void resetParameters();
 
 // mocks
@@ -79,4 +79,3 @@ void printIMUCalibration() { print("cal: N/A\n"); };
 void printIMUInfo() {};
 void printWiFiInfo() {};
 void configWiFi(bool, const char*, const char*) { print("Skip WiFi config\n"); };
-void setWiFiMode(const String& mode) { print("Skip WiFi mode set\n"); };

gazebo/simulator.cpp

@@ -18,19 +18,6 @@
 #include "Arduino.h"
 #include "flix.h"
 
-#include "cli.ino"
-#include "control.ino"
-#include "estimate.ino"
-#include "safety.ino"
-#include "log.ino"
-#include "lpf.h"
-#include "mavlink.ino"
-#include "motors.ino"
-#include "parameters.ino"
-#include "power.ino"
-#include "rc.ino"
-#include "time.ino"
-
 using ignition::math::Vector3d;
 using namespace gazebo;
 using namespace std;

gazebo/wifi.h

@@ -11,12 +11,7 @@
 #include <sys/poll.h>
 #include <gazebo/gazebo.hh>
 
-// Mocks
+int wifiMode = 1; // mock
-int wifiMode = 1;
-int wifiLongRange = 0;
-int espnowChannel = 6;
-const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
-
 int udpLocalPort = 14580;
 int udpRemotePort = 14550;
 const char *udpRemoteIP = "255.255.255.255";

tools/pyflix/README.md

@@ -28,8 +28,6 @@ from pyflix import Flix
 flix = Flix()  # create a Flix object and wait for connection
 ```
 
-If using ESP-NOW connection, specify the proxy device name in `FLIX_DEVICE` environment variable or pass it to the constructor: `Flix(device='/dev/cu.usbserial-0001')`.
-
 ### Telemetry
 
 Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware code:

tools/pyflix/flix.py

@@ -44,17 +44,12 @@ class Flix:
     _print_buffer: str = ''
     _modes = ['RAW', 'ACRO', 'STAB', 'AUTO']
 
-    def __init__(self, system_id: int=1, wait_connection: bool=True, device=os.getenv('FLIX_DEVICE')):
+    def __init__(self, system_id: int=1, wait_connection: bool=True):
         if not (0 <= system_id < 256):
             raise ValueError('system_id must be in range [0, 255]')
         self._setup_mavlink()
         self.system_id = system_id
         self._init_state()
-        if device is not None:
-            # User defined connection
-            logger.debug(f'Connecting to {device}')
-            self.connection: mavutil.mavfile = mavutil.mavlink_connection(device, source_system=255)  # type: ignore
-        else:
         try:
             # Direct connection
             logger.debug('Listening on port 14550')

tools/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "pyflix"
-version = "0.16"
+version = "0.15"
 description = "Python API for Flix drone"
 authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
 license = "MIT"