Position control draft

(Setting alpha = 1 efficiently disables the filter.)
Filtering does more harm than good in voltage monitoring.

.github/workflows/build.yml

@@ -10,33 +10,25 @@ on:
 jobs:
   build_linux:
     runs-on: ubuntu-latest
-    env:
-        ARDUINO_SKETCH_ALWAYS_EXPORT_BINARIES: 1
     steps:
     - uses: actions/checkout@v4
     - name: Install Arduino CLI
       run: curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh
-    - name: Build firmware for ESP32
+    - name: Build firmware
+      env:
+        ARDUINO_SKETCH_ALWAYS_EXPORT_BINARIES: 1
       run: make
-    - name: Build firmware for ESP32-C3
-      run: make BOARD=esp32:esp32:esp32c3
-    - name: Build firmware for ESP32-S3
-      run: make BOARD=esp32:esp32:esp32s3:CDCOnBoot=cdc
-    - name: Build firmware for ESP32-S3 with QSPI PSRAM
-      run: make BOARD=esp32:esp32:esp32s3:CDCOnBoot=cdc,PSRAM=enabled EXTRA=--output-dir=flix/build/esp32.esp32.esp32s3.qspi
-    - name: Build firmware for ESP32-S3 with OPI PSRAM
-      run: make BOARD=esp32:esp32:esp32s3:CDCOnBoot=cdc,PSRAM=opi EXTRA=--output-dir=flix/build/esp32.esp32.esp32s3.opi
-    - name: Build firmware for Flix2
-      run: make BOARD=esp32:esp32:esp32s3:FlashSize=4M,CDCOnBoot=cdc,PSRAM=opi EXTRA='--build-property "compiler.cpp.extra_flags=-DFLIX2" --output-dir=flix/build/esp32.esp32.flix2'
-    - name: Build firmware for STM32F4
-      run: |
-        arduino-cli core install STMicroelectronics:stm32 --additional-urls https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json  
-        make BOARD=STMicroelectronics:stm32:GenF4
     - name: Upload binaries
       uses: actions/upload-artifact@v4
       with:
         name: firmware-binary
         path: flix/build
+    - name: Build firmware for ESP32-C3
+      run: make BOARD=esp32:esp32:esp32c3
+    - name: Build firmware for ESP32-S3
+      run: make BOARD=esp32:esp32:esp32s3
+    - name: Build espnow-proxy
+      run: arduino-cli compile --fqbn esp32:esp32:esp32 tools/espnow-proxy
     - name: Check c_cpp_properties.json
       run: tools/check_c_cpp_properties.py
 
@@ -74,7 +66,7 @@ jobs:
         apt-get update
         DEBIAN_FRONTEND=noninteractive apt-get install -y curl wget build-essential cmake g++ pkg-config gnupg2 lsb-release sudo
     - name: Install Arduino CLI
-      run: curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh
+      uses: arduino/setup-arduino-cli@v1.1.1
     - uses: actions/checkout@v4
     - name: Install Gazebo
       run: |

.github/workflows/docs.yml

@@ -8,7 +8,6 @@ on:
 
 permissions:
   contents: read
-  actions: read
   pages: write
   id-token: write
 
@@ -31,38 +30,6 @@ jobs:
       run: cargo install mdbook --vers 0.4.43 --locked
     - name: Build book
       run: cd docs && mdbook build
-    - name: Wait for Build to complete
-      uses: lewagon/wait-on-check-action@v1.9.1
-      with:
-        ref: ${{ github.sha }}
-        check-name: build_linux
-        repo-token: ${{ secrets.GITHUB_TOKEN }}
-        wait-interval: 30
-    - name: Find firmware binaries
-      id: build_run
-      run: |
-        RUN_ID=$(gh api "repos/${{ github.repository }}/actions/workflows/build.yml/runs?head_sha=${{ github.sha }}&per_page=1" --jq '.workflow_runs[0].id')
-        echo "id=$RUN_ID" >> $GITHUB_OUTPUT
-      env:
-        GH_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-    - name: Download firmware binaries
-      uses: actions/download-artifact@v4
-      with:
-        github-token: ${{ secrets.GITHUB_TOKEN }}
-        repository: ${{ github.repository }}
-        run-id: ${{ steps.build_run.outputs.id }}
-        name: firmware-binary
-        path: docs/build
-    - name: Create shortcuts for firmware binaries
-      working-directory: docs/build
-      run: |
-        for FQBN in esp32.esp32.*; do
-          zip -r $FQBN.zip $FQBN
-          BOARD="${FQBN#esp32.esp32.}"
-          ln -s "$FQBN/flix.ino.merged.bin" "flix.$BOARD.merged.bin"
-          ln -s "$FQBN/flix.ino.bin" "flix.$BOARD.bin"
-          ln -s "$FQBN/flix.ino.bootloader.bin" "flix.$BOARD.bootloader.bin"
-        done
     - name: Upload artifact
       uses: actions/upload-pages-artifact@v3
       with:

Makefile

@@ -1,10 +1,10 @@
-BOARD = esp32:esp32:esp32
+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: .core .libs
-	arduino-cli compile --fqbn $(BOARD) --build-property "build.core_debug_level=1" flix $(EXTRA)
+	arduino-cli compile --fqbn $(BOARD) --build-property "build.core_debug_level=1" flix
 
 upload: build
 	arduino-cli upload --fqbn $(BOARD) -p "$(PORT)" flix

README.md

@@ -55,12 +55,6 @@ 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>
 
-### Position control
-
-The position control feature is in development. RoboCamp 2026 demo (using an overhead camera, [sources](https://github.com/xTimop/flix-poscontrol/compare/robolager2026...xTimop:flix-poscontrol:poscontrol)):
-
-<a href="https://youtu.be/369Xowm4HcU"><img width=300 src="https://i3.ytimg.com/vi/369Xowm4HcU/maxresdefault.jpg"></a>
-
 ## Simulation
 
 The simulator is implemented using Gazebo and runs the original Arduino code:

docs/usage.md

@@ -1,48 +1,20 @@
 # Usage: build, setup and flight
 
-To fly Flix quadcopter, you need to upload the firmware to the ESP32 board, and set up the drone for flight.
+To fly Flix quadcopter, you need to build the firmware, upload it to the ESP32 board, and set up the drone for flight.
 
-## Uploading the firmware
+To get the firmware sources, clone the repository using git:
-
-You can either use the **prebuilt binaries** or **build the firmware** from sources — this will let you modify the firmware and add new features.
-
-### Prebuilt binaries (the easiest way)
-
-1. Download the latest firmware file using the following links:
-
-   <!-- markdownlint-disable MD044 -->
-   |Type|Boards|Link|
-   |-|-|-|
-   |ESP32|DevKit, D1 Mini|[`quadcopter.dev/flix.esp32.merged.bin`](https://quadcopter.dev/flix.esp32.merged.bin)|
-   |ESP32-S3|Most S3 based|[`quadcopter.dev/flix.esp32s3.merged.bin`](https://quadcopter.dev/flix.esp32s3.merged.bin)|
-   |ESP32-S3 (2MB PSRAM)|S3 Super Mini, S3 Zero (2MB PSRAM)|[`quadcopter.dev/flix.esp32s3.qspi.merged.bin`](https://quadcopter.dev/flix.esp32s3.qspi.merged.bin)|
-   |ESP32-S3 (8/16MB PSRAM)|S3 Zero (8MB PSRAM)|[`quadcopter.dev/flix.esp32s3.opi.merged.bin`](https://quadcopter.dev/flix.esp32s3.opi.merged.bin)|
-   |ESP32-C3|C3 Super Mini|[`quadcopter.dev/flix.esp32c3.merged.bin`](https://quadcopter.dev/flix.esp32c3.merged.bin)|
-   |Flix2|Flix2 board|[`quadcopter.dev/flix.flix2.merged.bin`](https://quadcopter.dev/flix.flix2.merged.bin)|
-   <!-- markdownlint-enable MD044 -->
-
-2. Flash your ESP32 board using [ESP32 Web Flasher](https://www.espboards.dev/tools/program/):
-
-   <img src="img/web-flasher.png" width="400">
-
-   * Connect the board to your computer, press *Connect to ESP*, choose the serial port.
-   * Go to the *Flash* tab.
-   * Choose the downloaded firmware file, set *Flash address* to *0* (important).
-   * Click *Program* button and wait until the process is finished.
-
-### Building from sources (flexible)
-
-You can build and upload the firmware using either **Arduino IDE** (easier for beginners) or **command line**.
-
-Get the sources using git:
 
 ```bash
 git clone https://github.com/okalachev/flix.git && cd flix
 ```
 
-Beginners can [download the sources as a ZIP archive](https://github.com/okalachev/flix/archive/refs/heads/master.zip).
+Beginners can [download the source code as a ZIP archive](https://github.com/okalachev/flix/archive/refs/heads/master.zip).
 
-#### Arduino IDE (Windows, Linux, macOS)
+## Building the firmware
+
+You can build and upload the firmware using either **Arduino IDE** (easier for beginners) or **command line**.
+
+### Arduino IDE (Windows, Linux, macOS)
 
 <img src="img/arduino-ide.png" width="400" alt="Flix firmware open in Arduino IDE">
 
@@ -57,7 +29,7 @@ Beginners can [download the sources as a ZIP archive](https://github.com/okalach
 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.
 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)
+### Command line (Windows, Linux, macOS)
 
 1. [Install Arduino CLI](https://arduino.github.io/arduino-cli/installation/).
 
@@ -99,6 +71,32 @@ See other available Make commands in [Makefile](../Makefile).
 
 ## Before first flight
 
+### Choose the IMU model
+
+In case if using different IMU model than MPU9250, change `imu` variable declaration in the `imu.ino`:
+
+```cpp
+ICM20948 imu(SPI);  // For ICM-20948 via SPI
+// or
+MPU6050 imu(Wire);  // For MPU-6050 via I2C
+```
+
+If using non-default SPI pins, pass SCK, MISO, and MOSI pin numbers to `SPI.begin` call and SS pin to `imu` constructor like that:
+
+```cpp
+ICM20948(SPI, <SS>);
+// ...
+SPI.begin(<SCK>, <MISO>, <MOSI>);
+imu.begin();
+```
+
+If using non-default I2C pins, pass SDA and SCL pin numbers to `Wire.setPins` call like that:
+
+```cpp
+Wire.begin(<SDA>, <SCL>);
+imu.begin();
+```
+
 ### Connect using QGroundControl
 
 QGroundControl is a ground control station software that can be used to monitor and control the drone.
@@ -139,17 +137,6 @@ The drone is configured using parameters. To access and modify them, go to the Q
 
 You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
 
-### Configure the IMU
-
-1. Configure the following parameters for the IMU:
-   * `IMU_MODEL` — IMU model (1 for MPU-9250/MPU-6500, 2 for ICM-20948, 3 for MPU-6050, 4 for ICM-40609-D).
-   * `IMU_BUS` — communication bus (0 for SPI, 1 for I²C).
-   * `IMU_PIN_SCK`, `IMU_PIN_MISO`, `IMU_PIN_MOSI`, `IMU_PIN_CS` — SPI pin numbers.
-   * `IMU_PIN_SCL`, `IMU_PIN_SDA` — I²C pin numbers.
-   * `IMU_PIN_INT` — IMU data ready pin number (-1 if not used).
-2. Reboot the drone.
-3. Check the IMU is working using `imu` command in the console (should print `status: OK`).
-
 ### Define IMU orientation
 
 The IMU orientation (relative to the drone's axes) is defined using the parameters: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
@@ -178,9 +165,9 @@ Before flight you need to calibrate the accelerometer:
 
 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).
 
-#### Brushless motors
+Certain ESP32 models (such as ESP32-S3 and ESP32-C3) support a lower maximum PWM frequency; on these boards the parameter `MOT_PWM_FREQ` should be set to 38000 Hz.
 
-If using brushless motors with ESCs:
+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).
@@ -188,7 +175,7 @@ If using brushless motors with ESCs:
 > [!CAUTION]
 > **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
 
-### Battery voltage monitoring (optional)
+### Battery voltage monitoring
 
 ESP32 ADC can measure only up to 3.3 V, so you need to use a voltage divider to monitor the battery voltage. To enable voltage measurement, set the following parameters:
 
@@ -385,7 +372,7 @@ To setup ESP-NOW communication:
 5. Click *Save*, click *Connect*. QGroundControl should connect to the drone using ESP-NOW and begin showing the telemetry.
 
 > [!TIP]
-> Make sure Arduino IDE is not running when using ESP-NOW proxy board, as it may block the serial port.
+> Make sure Arduino IDE is not running when using ESP-NOW proxy board, as it may block it.
 
 ## Flight log
 

docs/user.md

@@ -4,15 +4,6 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
-Author: [Oleg1405](https://t.me/Oleg1405).<br>
-Description: ESP32 Mini, MPU-6500 IMU, boost converter, BT2.0 power connector, 65 mm props, BetaFPV ELRS Lite Receiver, Radiomaster Pocket + Mavlink Joystick (Android) control.
-
-<img src="img/user/oleg1405/1.jpg" height=300>
-
-[Flight video](https://www.youtube.com/shorts/rbXV4sHbpso).
-
----
-
 Author: Alican Erüst.<br>
 Description: QX95 mm frame, 55 mm propellers, 3.7 V 25C 1050 mAh LiPo battery, MPU6050 IMU, Logitech F310 gamepad controller, with a total quadcopter weight of 66 g.
 

flix/cli.ino

@@ -6,10 +6,10 @@
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
-#include "lpf.h"
+#include "filter.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern const int RAW, ACRO, STAB, AUTO;
+extern const int RAW, ACRO, STAB, AUTO, POS;
 extern const int W_AP, W_STA, W_ESPNOW;
 extern float t, dt, loopRate;
 extern uint16_t channels[16];
@@ -50,8 +50,14 @@ const char* motd =
 "sta <ssid> <password> - configure Wi-Fi client mode\n"
 "espnow <mac> [<key>] - configure ESP-NOW peer\n"
 "mot - show motor output\n"
-"log [dump] - print log header [and data]\n"
 "mfr/mfl/mrr/mrl [<thrust>] - test motor (remove props)\n"
+"log [dump] - print log header [and data]\n"
+"log - show log info\n"
+"log header - show log header\n"
+"log reset - reset log\n"
+"log <name> <rate> - setup log topic rate\n"
+"l <str> - show log values starting with str\n"
+"l expose <name> - expose log value to telemetry\n"
 "sys - show system info\n"
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
@@ -128,6 +134,8 @@ void doCommand(String str, bool echo = false) {
 		mode = ACRO;
 	} else if (command == "auto") {
 		mode = AUTO;
+	} else if (command == "pos") {
+		mode = POS;
 	} else if (command == "rc") {
 		print("channels: ");
 		for (int i = 0; i < 16; i++) {
@@ -153,9 +161,18 @@ void doCommand(String str, bool echo = false) {
 	} else if (command == "mot") {
 		print("front-right %g front-left %g rear-right %g rear-left %g\n",
 			motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
-	} else if (command == "log") {
+	} else if (command == "log" && arg0 == "") {
+		printLogInfo();
+	} else if (command == "log" && arg1 != "") {
+		configLogThrottle(arg0.c_str(), arg1.toFloat());
+	} else if (command == "log" && arg0 == "header") {
 		printLogHeader();
-		if (arg0 == "dump") printLogData();
+	} else if (command == "log" && arg0 == "reset") {
+		resetLog();
+	} else if (command == "l" && arg0 == "expose" && arg1 != "") {
+		exposeLogValue(arg1.c_str());
+	} else if (command == "l") {
+		printLogValues(arg0.c_str());
 	} else if (command == "cr") {
 		calibrateRC();
 	} else if (command == "ca") {
@@ -174,6 +191,8 @@ void doCommand(String str, bool echo = false) {
 		print("Temperature: %.1f °C\n", temperatureRead());
 		print("Total RAM: %d KB\n", ESP.getHeapSize() / 1024);
 		print("Free heap: %d KB\n", ESP.getFreeHeap() / 1024);
+		print("PSRAM: %d KB\n", ESP.getPsramSize() / 1024);
+		print("Free PSRAM: %d KB\n", ESP.getFreePsram() / 1024);
 		print("Firmware: " __DATE__ " " __TIME__ "\n");
 		// Print tasks table
 		print("Num  Task                MinSt  Prio  Core  CPU%%\n");

flix/config.h

@@ -1,27 +0,0 @@
-// Copyright (c) 2026 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-// Parameter defaults
-
-#pragma once
-
-void setDefaults() {
-	// Set defaults here
-
-	#if defined(CONFIG_IDF_TARGET_ESP32S3) || defined(CONFIG_IDF_TARGET_ESP32C3)
-		pwmFrequency = 38000;
-	#endif
-
-	#ifdef FLIX2
-		imuModel = 4; // ICM-40609-D
-		imuIntPin = 10;
-		imuCsPin = 14;
-
-		motorPins[MOTOR_REAR_LEFT] = 41;
-		motorPins[MOTOR_REAR_RIGHT] = 7;
-		motorPins[MOTOR_FRONT_RIGHT] = 18;
-		motorPins[MOTOR_FRONT_LEFT] = 38;
-
-		voltagePin = 3;
-	#endif
-}

flix/control.ino

@@ -6,10 +6,35 @@
 #include "vector.h"
 #include "quaternion.h"
 #include "pid.h"
-#include "lpf.h"
+#include "filter.h"
 #include "util.h"
 
-const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
+#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
+
+const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3, POS = 4;
 int mode = STAB;
 bool armed = false;
 
@@ -19,18 +44,19 @@ Vector ratesExtra; // feedforward rates
 Vector torqueTarget;
 float thrustTarget;
 
-PID rollRatePID(0.05, 0.2, 0.001, 0.3, 0.2);
+PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
-PID pitchRatePID(0.05, 0.2, 0.001, 0.3, 0.2);
+PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
-PID yawRatePID(0.3, 0, 0, 0.3);
+PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
-PID rollPID(6);
+PID rollPID(ROLL_P, ROLL_I, ROLL_D);
-PID pitchPID(6);
+PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
-PID yawPID(3);
+PID yawPID(YAW_P, 0, 0);
-Vector maxRate(radians(360), radians(360), radians(360));
+Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
-float tiltMax = radians(30);
+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;
+extern Vector position, positionTarget;
 
 void control() {
 	interpretControls();
@@ -54,7 +80,18 @@ void interpretControls() {
 
 	thrustTarget = controlThrottle;
 
-	if (mode == STAB) {
+	if (mode == POS) {
+		if (controlRoll != 0 || controlPitch != 0) {
+			positionTarget.x = NAN;
+			positionTarget.y = NAN;
+		} else if (invalid(positionTarget.x) || invalid(positionTarget.y)) {
+			// reset position target
+			positionTarget.x = position.x;
+			positionTarget.y = position.y;
+		}
+	}
+
+	if (mode == STAB || (mode == POS && invalid(positionTarget.x))) {
 		float yawTarget = attitudeTarget.getYaw();
 		if (!armed || invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target
 		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));

flix/estimate.ino

@@ -5,7 +5,7 @@
 
 #include "quaternion.h"
 #include "vector.h"
-#include "lpf.h"
+#include "filter.h"
 #include "util.h"
 
 Vector rates; // estimated angular rates, rad/s
@@ -15,16 +15,24 @@ bool landed;
 float accWeight = 0.003;
 float levelWeight = 0.0002;
 LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
+NotchFilter<Vector> ratesNotch(382, 0);
+
+void setupEstimate() {
+	print("Setup estimation\n");
+	ratesNotch.reset();
+}
 
 void estimate() {
 	applyGyro();
 	applyAcc();
 	applyLevel();
+	estimatePosition();
 }
 
 void applyGyro() {
 	// filter gyro to get angular rates
 	rates = ratesFilter.update(gyro);
+	rates = ratesNotch.update(rates);
 
 	// apply rates to attitude
 	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));

flix/filter.h

@@ -0,0 +1,98 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Low pass and notch filters
+
+#pragma once
+
+template <typename T> // Using template to make the filter usable for scalar and vector values
+class LowPassFilter {
+public:
+	float alpha; // smoothing constant, 1 means filter disabled
+	T output;
+
+	LowPassFilter(float alpha): alpha(alpha) {};
+
+	T update(const T input) {
+		if (!init) {
+			init = true;
+			return output = input;
+		}
+		return output += alpha * (input - output);
+	}
+
+	void setCutOffFrequency(float cutOffFreq, float dt) {
+		alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
+	}
+
+	void reset() {
+		init = false;
+	}
+
+private:
+	bool init = false;
+};
+
+template <typename T>
+class NotchFilter {
+public:
+	float frequency;
+	float bandwidth;
+	T output;
+
+	NotchFilter(float frequency, float bandwidth): frequency(frequency), bandwidth(bandwidth) {
+		reset();
+	};
+
+	T update(const T input) {
+		if (frequency <= 0 || bandwidth <= 0) return input;
+
+		if (!init) {
+			init = true;
+			x1 = x2 = input;
+			y1 = y2 = input;
+			return output = input;
+		}
+
+		output = b0 * input + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
+
+		x2 = x1;
+		x1 = input;
+		y2 = y1;
+		y1 = output;
+
+		return output;
+	}
+
+	void reset() {
+		const float dt = 0.001f;
+		float f = frequency;
+		float bw = bandwidth;
+		if (f < 0) f = 0;
+		if (bw < 1e-6f) bw = 1e-6f;
+
+		float q = f / bw;
+		if (q < 1e-3f) q = 1e-3f;
+
+		const float w0 = 2.0f * PI * f * dt;
+		const float c = cos(w0);
+		const float s = sin(w0);
+		const float alpha = s / (2.0f * q);
+
+		const float a0 = 1.0f + alpha;
+		const float invA0 = 1.0f / a0;
+
+		b0 = 1.0f * invA0;
+		b1 = -2.0f * c * invA0;
+		b2 = 1.0f * invA0;
+		a1 = -2.0f * c * invA0;
+		a2 = (1.0f - alpha) * invA0;
+
+		init = false;
+	}
+
+private:
+	float b0, b1, b2, a1, a2;
+	T x1, x2, y1, y2;
+	bool init = false;
+};

flix/flix.ino

@@ -26,6 +26,8 @@ void setup() {
 	setupWiFi();
 	setupIMU();
 	setupRC();
+	setupEstimate();
+	setupLog();
 	setLED(false);
 	print("Initializing complete\n");
 }
@@ -40,6 +42,6 @@ void loop() {
 	handleInput();
 	processMavlink();
 	readVoltage();
-	logData();
+	loopLog();
 	syncParameters();
 }

flix/imu.ino

@@ -4,17 +4,12 @@
 // Work with the IMU sensor
 
 #include <SPI.h>
-#include <Wire.h>
 #include <FlixPeriph.h>
 #include "vector.h"
-#include "lpf.h"
+#include "filter.h"
 #include "util.h"
 
-IMU *imu;
+MPU9250 imu(SPI);
-int imuModel = -1; // 1 - MPU9250, 2 - ICM20948, 3 - MPU6050, 4 - ICM40609D
-int imuBus = 0; // 0 - SPI, 1 - I2C
-int imuSckPin = SCK, imuMisoPin = MISO, imuMosiPin = MOSI, imuCsPin = SS, imuIntPin = -1;
-int imuSdaPin = SDA, imuSclPin = SCL;
 Vector imuRotation(0, 0, PI / 2); // imu orientation as Euler angles
 
 Vector gyro; // gyroscope output, rad/s
@@ -28,35 +23,22 @@ LowPassFilter<Vector> gyroBiasFilter(0.001);
 
 void setupIMU() {
 	print("Setup IMU\n");
-	free(imu);
+	imu.begin();
-	if (imuModel == 3) imuBus = 1; // MPU6050 is I2C only
-
-	if (imuBus == 0) {
-		// SPI connection
-		SPI.begin(imuSckPin, imuMisoPin, imuMosiPin);
-		imu = IMU::create(imuModel, SPI, imuCsPin, imuIntPin);
-	} else {
-		// I2C connection
-		Wire.setPins(imuSdaPin, imuSclPin);
-		imu = IMU::create(imuModel, Wire, imuIntPin);
-	}
-
-	imu->begin();
 	configureIMU();
 }
 
 void configureIMU() {
-	imu->setAccelRange(IMU::ACCEL_RANGE_4G);
+	imu.setAccelRange(imu.ACCEL_RANGE_4G);
-	imu->setGyroRange(IMU::GYRO_RANGE_2000DPS);
+	imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
-	imu->setDLPF(IMU::DLPF_MAX);
+	imu.setDLPF(imu.DLPF_MAX);
-	imu->setRate(IMU::RATE_1KHZ_APPROX);
+	imu.setRate(imu.RATE_1KHZ_APPROX);
-	imu->setupInterrupt();
+	imu.setupInterrupt();
 }
 
 void readIMU() {
-	imu->waitForData();
+	imu.waitForData();
-	imu->getGyro(gyro.x, gyro.y, gyro.z);
+	imu.getGyro(gyro.x, gyro.y, gyro.z);
-	imu->getAccel(acc.x, acc.y, acc.z);
+	imu.getAccel(acc.x, acc.y, acc.z);
 	calibrateGyroOnce();
 
 	// Apply scale and bias
@@ -78,7 +60,7 @@ void calibrateGyroOnce() {
 
 void calibrateAccel() {
 	print("Calibrating accelerometer\n");
-	imu->setAccelRange(IMU::ACCEL_RANGE_2G); // the most sensitive mode
+	imu.setAccelRange(imu.ACCEL_RANGE_2G); // the most sensitive mode
 
 	print("1/6 Place level [8 sec]\n");
 	pause(8);
@@ -112,9 +94,9 @@ void calibrateAccelOnce() {
 	// Compute the average of the accelerometer readings
 	acc = Vector(0, 0, 0);
 	for (int i = 0; i < samples; i++) {
-		imu->waitForData();
+		imu.waitForData();
 		Vector sample;
-		imu->getAccel(sample.x, sample.y, sample.z);
+		imu.getAccel(sample.x, sample.y, sample.z);
 		acc = acc + sample;
 	}
 	acc = acc / samples;
@@ -139,18 +121,17 @@ void printIMUCalibration() {
 }
 
 void printIMUInfo() {
-	imu->status() ? print("status: ERROR %d\n", imu->status()) : print("status: OK\n");
+	imu.status() ? print("status: ERROR %d\n", imu.status()) : print("status: OK\n");
-	print("model: %s\n", imu->getModel());
+	print("model: %s\n", imu.getModel());
-	print("who am I: 0x%02X\n", imu->whoAmI());
+	print("who am I: 0x%02X\n", imu.whoAmI());
 	print("rate: %.0f\n", loopRate);
-	print("interrupt mode: %s\n", imuIntPin != -1 ? "pin" : "timer");
+	print("temperature: %.1f °C\n", imu.getTemp());
-	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();
+	imu.waitForData();
 	Vector rawGyro, rawAcc;
-	imu->getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
+	imu.getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
-	imu->getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
+	imu.getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
 	print("raw gyro: %f %f %f\n", rawGyro.x, rawGyro.y, rawGyro.z);
 	print("raw acc: %f %f %f\n", rawAcc.x, rawAcc.y, rawAcc.z);
 }

flix/log.ino

@@ -1,77 +1,277 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// In-RAM logging
+// Logging subsystem
 
 #include "vector.h"
 #include "util.h"
 
-#define LOG_RATE 100
+int logMemory = -1; // 0 - RAM, 1 - PSRAM, -1 - disabled
-#define LOG_DURATION 10
+float logUsage = 0.5; // fraction of free memory to use for log
-#define LOG_SIZE LOG_DURATION * LOG_RATE
 
-Vector attitudeEuler;
+struct LogValue {
-Vector attitudeTargetEuler;
-
-struct LogEntry {
 	const char *name;
-	float *value;
+	Value value;
+	float lastValue = NAN;
+	bool logged = true; // if false, use only for triggering log update
+	LogValue() : name(nullptr), value() {}; // empty value constructor
+	template <typename T>
+	LogValue(const char *name, T value, bool logged = true) : name(name), value(value), logged(logged) {};
 };
 
-LogEntry logEntries[] = {
+struct LogTopic {
-	{"t", &t},
+	LogValue values[10];
-	{"rates.x", &rates.x},
+	int length = 0; // number of logged values
-	{"rates.y", &rates.y},
+	float throttle; // max update rate, Hz
-	{"rates.z", &rates.z},
+	float lastUpdate = -INFINITY;
-	{"ratesTarget.x", &ratesTarget.x},
+
-	{"ratesTarget.y", &ratesTarget.y},
+	LogTopic(float throttle, LogValue v0, LogValue v1 = {}, LogValue v2 = {}, LogValue v3 = {}, LogValue v4 = {}, LogValue v5 = {}, LogValue v6 = {}, LogValue v7 = {}, LogValue v8 = {}, LogValue v9 = {}) :
-	{"ratesTarget.z", &ratesTarget.z},
+		throttle(throttle), values{v0, v1, v2, v3, v4, v5, v6, v7, v8, v9} {
-	{"attitude.x", &attitudeEuler.x},
+			// Count logged values
-	{"attitude.y", &attitudeEuler.y},
+			for (auto& v : values) {
-	{"attitude.z", &attitudeEuler.z},
+				if (v.name == nullptr) break;
-	{"attitudeTarget.x", &attitudeTargetEuler.x},
+				if (v.logged) length++;
-	{"attitudeTarget.y", &attitudeTargetEuler.y},
+			}
-	{"attitudeTarget.z", &attitudeTargetEuler.z},
+		};
-	{"thrustTarget", &thrustTarget}
+
+	LogTopic(LogValue v0, LogValue v1 = {}, LogValue v2 = {}, LogValue v3 = {}, LogValue v4 = {}, LogValue v5 = {}, LogValue v6 = {}, LogValue v7 = {}, LogValue v8 = {}, LogValue v9 = {}) :
+		LogTopic(INFINITY, v0, v1, v2, v3, v4, v5, v6, v7, v8, v9) {};
 };
 
-const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
+LogTopic logTopics[] = {
-float logBuffer[LOG_SIZE][logColumns];
+	// time
+	LogTopic({"t", &t}), // must be the first topic
+	LogTopic(1, {"loopRate", &loopRate}),
 
-void prepareLogData() {
+	// imu
-	attitudeEuler = attitude.toEuler();
+	LogTopic(
-	attitudeTargetEuler = attitudeTarget.toEuler();
+		{"gyro.x", &gyro.x},
-}
+		{"gyro.y", &gyro.y},
+		{"gyro.z", &gyro.z}),
 
-void logData() {
+	LogTopic(50,
-	if (!armed) return;
+		{"acc.x", &acc.x},
-	static int logPointer = 0;
+		{"acc.y", &acc.y},
-	static Rate period(LOG_RATE);
+		{"acc.z", &acc.z}),
-	if (!period) return;
 
-	prepareLogData();
+	LogTopic(10,
+		{"gyroBias.x", &gyroBias.x},
+		{"gyroBias.y", &gyroBias.y},
+		{"gyroBias.z", &gyroBias.z}),
 
-	for (int i = 0; i < logColumns; i++) {
+	// estimation
-		logBuffer[logPointer][i] = *logEntries[i].value;
+	LogTopic(50,
-	}
+		{"rates.x", &rates.x},
+		{"rates.y", &rates.y},
+		{"rates.z", &rates.z},
+		{"attitude.roll", []() { return attitude.getRoll(); }},
+		{"attitude.pitch", []() { return attitude.getPitch(); }},
+		{"attitude.yaw", []() { return attitude.getYaw(); }}),
 
-	logPointer++;
+	LogTopic(50,
-	if (logPointer >= LOG_SIZE) {
+		{"position.x", &position.x},
-		logPointer = 0;
+		{"position.y", &position.y},
+		{"position.z", &position.z}),
+
+	// rc
+	LogTopic(10,
+		{"controlTime", &controlTime, false}, // trigger value
+		{"controlRoll", &controlRoll},
+		{"controlPitch", &controlPitch},
+		{"controlYaw", &controlYaw},
+		{"controlThrottle", &controlThrottle}),
+
+	// control
+	LogTopic({"armed", &armed}),
+	LogTopic({"mode", &mode}),
+
+	LogTopic(10,
+		{"ratesTarget.x", &ratesTarget.x},
+		{"ratesTarget.y", &ratesTarget.y},
+		{"ratesTarget.z", &ratesTarget.z},
+		{"attitudeTarget.roll", []() { return attitudeTarget.getRoll(); }},
+		{"attitudeTarget.pitch", []() { return attitudeTarget.getPitch(); }},
+		{"attitudeTarget.yaw", []() { return attitudeTarget.getYaw(); }},
+		{"thrustTarget", &thrustTarget}),
+
+	// motors
+	LogTopic(
+		{"motors[0]", &motors[0]},
+		{"motors[1]", &motors[1]},
+		{"motors[2]", &motors[2]},
+		{"motors[3]", &motors[3]}),
+
+	// misc
+	LogTopic(5,
+		{"voltage", &voltage},
+		{"temp", &temperatureRead},
+		{"imuTemp", []() { return imu.getTemp(); }}),
+};
+
+void *logBuffer; // buffer for log data
+size_t logCapacity;
+size_t logCursor = 0;
+size_t logLength = 0;
+LogValue *logExposed = nullptr; // log values exposed to telemetry
+
+void setupLog() {
+	print("Setup log\n");
+
+	free(logBuffer); // when reconfiguring
+	logBuffer = nullptr;
+	logCursor = 0;
+	logLength = 0;
+
+	if (logMemory == 0) {
+		logCapacity = ESP.getFreeHeap() * logUsage;
+		logBuffer = (uint8_t *)calloc(logCapacity, 1);
+	} else if (logMemory == 1) {
+		logCapacity = ESP.getFreePsram() * logUsage;
+		logBuffer = (uint8_t *)heap_caps_calloc(logCapacity, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
 	}
 }
 
-void printLogHeader() {
+void loopLog() {
-	for (int i = 0; i < logColumns; i++) {
+	if (logBuffer == nullptr || !armed) return;
-		print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n");
-	}
-}
 
-void printLogData() {
+	if (!logLength) resetLog(); // reset state on first log write
-	for (int i = 0; i < LOG_SIZE; i++) {
+
-		if (logBuffer[i][0] == 0) continue; // skip empty records
+	static Rate sync(2);
-		for (int j = 0; j < logColumns; j++) {
+	if (sync) {
-			print("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n");
+		const uint8_t marker[] = {0x1A, 0x91, 0x4F, 0xF6, 0x7F};
+		writeLog(&marker, sizeof(marker)); // write sync marker
+	}
+
+	for (uint8_t i = 0; i < sizeof(logTopics) / sizeof(logTopics[0]); i++) {
+		LogTopic& topic = logTopics[i];
+		if (t - topic.lastUpdate < 1 / topic.throttle) continue; // throttle topic
+		if (!isTopicUpdated(i)) continue; // skip if topic was't updated
+
+		topic.lastUpdate = t;
+		writeLog(&i, sizeof(i)); // write topic index
+
+		for (auto& value : topic.values) {
+			if (value.name == nullptr) break;
+			if (!value.logged) continue;
+			value.lastValue = value.value.get();
+			writeLog(&value.lastValue, sizeof(float)); // write value
 		}
 	}
 }
+
+void resetLog() {
+	for (auto& topic : logTopics) {
+		topic.lastUpdate = -INFINITY;
+		for (auto& value : topic.values) {
+			value.lastValue = NAN;
+		}
+	}
+	logCursor = 0;
+	logLength = 0;
+}
+
+void writeLog(const void *data, size_t size) {
+	size_t first = min(size, logCapacity - logCursor);
+	size_t second = size - first;
+	memcpy(logBuffer + logCursor, data, first);
+	logCursor = (logCursor + first) % logCapacity;
+	if (second > 0) {
+		memcpy(logBuffer + logCursor, data + first, second);
+		logCursor = (logCursor + second) % logCapacity;
+	}
+	logLength = min(logLength + size, logCapacity);
+}
+
+void readLog(void *data, size_t position, size_t size) {
+	if (logLength == logCapacity) {
+		position = (logCursor + position) % logCapacity;
+	}
+	size_t first = min(size, logCapacity - position);
+	size_t second = size - first;
+	memcpy(data, logBuffer + position, first);
+	if (second > 0) {
+		memcpy(data + first, logBuffer, second);
+	}
+}
+
+bool isTopicUpdated(const uint8_t topic) {
+	LogTopic& logTopic = logTopics[topic];
+	bool updated = false;
+
+	for (auto& value : logTopic.values) {
+		if (value.name == nullptr) break;
+		float v = value.value.get();
+		if (!floatEquals(value.lastValue, v)) {
+			value.lastValue = v;
+			updated = true;
+		}
+	}
+	return updated;
+}
+
+void printLogInfo() {
+	if (logMemory == -1) return print("Log: disabled\n");
+
+	print("Memory: %s\n", logMemory == 0 ? "RAM" : "PSRAM");
+	print("Usage: %.f%%\n", logUsage * 100);
+	print("Capacity: %u bytes\n", (unsigned)logCapacity);
+	print("Used: %u bytes\n", (unsigned)logLength);
+	print("Estimated duration: %d seconds\n", estimateLogDuration());
+}
+
+int estimateLogDuration() {
+	float bandwidth = 0;
+	for (LogTopic& topic : logTopics) {
+		float rate = isinf(topic.throttle) ? loopRate : topic.throttle;
+		bandwidth += rate * topic.length * sizeof(float);
+	}
+	return logCapacity / bandwidth;
+}
+
+void printLogHeader() {
+	int i = 0;
+	for (auto& topic : logTopics) {
+		print("Topic #%d (%g Hz):\n", i++, topic.throttle);
+		for (auto& value : topic.values) {
+			if (value.name == nullptr) break;
+			print("  %s%s\n", value.name, value.logged ?"" : " (not logged)");
+		}
+	}
+}
+
+void printLogValues(const char *filter) {
+	for (LogTopic& topic : logTopics) {
+		for (LogValue& value : topic.values) {
+			if (value.name == nullptr) break;
+			if (strncasecmp(value.name, filter, strlen(filter))) continue;
+			print("%s = %g\n", value.name, value.value.get());
+		}
+	}
+}
+
+void configLogThrottle(const char *name, float throttle) {
+	for (LogTopic& topic : logTopics) {
+		for (LogValue& value : topic.values) {
+			if (value.name == nullptr) break;
+			if (strcasecmp(value.name, name) != 0) continue;
+			topic.throttle = throttle;
+			print("Log throttle for %s set to %.1f Hz\n", name, throttle);
+			return;
+		}
+	}
+	print("Log value not found: %s\n", name);
+}
+
+void exposeLogValue(const char *name) {
+	for (int i = 0; i < sizeof(logTopics) / sizeof(logTopics[0]); i++) {
+		LogTopic& topic = logTopics[i];
+		for (LogValue& value : topic.values) {
+			if (value.name == nullptr) break;
+			if (strcasecmp(value.name, name) != 0) continue;
+			logExposed = &value;
+			print("Log value %s exposed\n", name);
+			return;
+		}
+	}
+	print("Log value not found: %s\n", name);
+}

flix/lpf.h

@@ -1,34 +0,0 @@
-// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-// Low pass filter implementation
-
-#pragma once
-
-template <typename T> // Using template to make the filter usable for scalar and vector values
-class LowPassFilter {
-public:
-	float alpha; // smoothing constant, 1 means filter disabled
-	T output;
-
-	LowPassFilter(float alpha): alpha(alpha) {};
-
-	T update(const T input) {
-		if (!init) {
-			init = true;
-			return output = input;
-		}
-		return output += alpha * (input - output);
-	}
-
-	void setCutOffFrequency(float cutOffFreq, float dt) {
-		alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
-	}
-
-	void reset() {
-		init = false;
-	}
-
-private:
-	bool init = false;
-};

flix/mavlink.ino

@@ -16,8 +16,9 @@ Rate telemetryAttitude(20);
 Rate telemetryRC(10);
 Rate telemetryMotors(10);
 Rate telemetryIMU(15);
+Rate telemetryTopic(10);
 
-float mavlinkTime = NAN; // time of last received message
+bool mavlinkConnected = false;
 String mavlinkPrintBuffer;
 
 void processMavlink() {
@@ -40,7 +41,7 @@ void sendMavlink() {
 		sendMessage(&msg);
 	}
 
-	if (!valid(mavlinkTime)) return; // send only heartbeat until connected
+	if (!mavlinkConnected) return; // send only heartbeat until connected
 
 	if (telemetrySlow) {
 		mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
@@ -84,6 +85,12 @@ void sendMavlink() {
 			0, 0, 0, 0);
 		sendMessage(&msg);
 	}
+
+	if (telemetryTopic && logExposed != nullptr) {
+		mavlink_msg_named_value_float_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
+			time, logExposed->name, logExposed->value.get());
+		sendMessage(&msg);
+	}
 }
 
 void sendMessage(const void *msg) {
@@ -92,6 +99,26 @@ void sendMessage(const void *msg) {
 	sendWiFi(buf, len);
 }
 
+static uint8_t mavlinkBatch[ESP_NOW_MAX_DATA_LEN_V2];
+static int mavlinkBatchSize = 0;
+
+void batchMessage(const void *msg) {
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+	int len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
+
+	if (mavlinkBatchSize + len > sizeof(mavlinkBatch)) {
+		sendWiFi(mavlinkBatch, mavlinkBatchSize);
+		mavlinkBatchSize = 0;
+	}
+	memcpy(mavlinkBatch + mavlinkBatchSize, buf, len);
+	mavlinkBatchSize += len;
+}
+
+void flushBatchMessages() {
+	sendWiFi(mavlinkBatch, mavlinkBatchSize);
+	mavlinkBatchSize = 0;
+}
+
 void receiveMavlink() {
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 	int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);
@@ -101,7 +128,7 @@ void receiveMavlink() {
 	mavlink_status_t status;
 	for (int i = 0; i < len; i++) {
 		if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
-			mavlinkTime = t;
+			mavlinkConnected = true;
 			handleMavlink(&msg);
 		}
 	}
@@ -169,6 +196,13 @@ void handleMavlink(const void *_msg) {
 		sendMessage(&msg);
 	}
 
+	if (msg.msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
+		mavlink_vision_position_estimate_t m;
+		mavlink_msg_vision_position_estimate_decode(&msg, &m);
+		Vector pos(m.x, m.y, m.z);
+		correctPosition(pos);
+	}
+
 	if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
 		mavlink_mission_request_list_t m;
 		mavlink_msg_mission_request_list_decode(&msg, &m);
@@ -196,18 +230,24 @@ void handleMavlink(const void *_msg) {
 		mavlink_msg_set_attitude_target_decode(&msg, &m);
 		if (m.target_system && m.target_system != mavlinkSysId) return;
 
-		// copy attitude, rates and thrust targets
+		if (!(m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE)) {
-		ratesTarget.x = m.body_roll_rate;
+			// Attitude control
-		ratesTarget.y = -m.body_pitch_rate; // convert to flu
+			attitudeTarget.w = m.q[0];
-		ratesTarget.z = -m.body_yaw_rate;
+			attitudeTarget.x = m.q[1];
-		attitudeTarget.w = m.q[0];
+			attitudeTarget.y = -m.q[2];
-		attitudeTarget.x = m.q[1];
+			attitudeTarget.z = -m.q[3];
-		attitudeTarget.y = -m.q[2];
+			ratesExtra.x = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE ? 0 : m.body_roll_rate;
-		attitudeTarget.z = -m.q[3];
+			ratesExtra.y = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE ? 0 : -m.body_pitch_rate; // convert to flu
-		thrustTarget = m.thrust;
+			ratesExtra.z = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE ? 0 : -m.body_yaw_rate;
-		ratesExtra = Vector(0, 0, 0);
+		} else {
+			// Rates control
+			attitudeTarget.invalidate();
+			ratesTarget.x = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE ? ratesTarget.x : m.body_roll_rate;
+			ratesTarget.y = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE ? ratesTarget.y : -m.body_pitch_rate;
+			ratesTarget.z = m.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE ? ratesTarget.z : -m.body_yaw_rate;
+		}
 
-		if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
+		thrustTarget = valid(m.thrust) ? m.thrust : thrustTarget;
 		armed = m.thrust > 0;
 	}
 
@@ -225,18 +265,29 @@ void handleMavlink(const void *_msg) {
 		armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
 	}
 
+	if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_LIST) {
+		const uint32_t qgcEpoch = 1262304000; // qgc accepts only timestamps after 2010-01-01
+		mavlink_message_t response;
+		mavlink_msg_log_entry_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
+			0, 1, 0, qgcEpoch + t * 60, logLength); // put fake unique date to make qgc happy with saving logs
+		sendMessage(&response);
+	}
+
 	if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
 		mavlink_log_request_data_t m;
 		mavlink_msg_log_request_data_decode(&msg, &m);
 		if (m.target_system && m.target_system != mavlinkSysId) return;
 
-		// Send all log records
+		for (int i = 0; i < m.count; i += MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
-		for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
+			int chunkSize = min(MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN, (int)(m.count - i));
-			mavlink_message_t msg;
+			mavlink_message_t response;
-			mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
+			uint8_t data[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN];
-				sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
+			readLog(data, m.ofs + i, chunkSize);
-			sendMessage(&msg);
+			mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
+				m.id, m.ofs + i, chunkSize, data);
+			batchMessage(&response);
 		}
+		flushBatchMessages();
 	}
 
 	// Handle commands

flix/motors.ino

@@ -7,7 +7,7 @@
 
 float motors[4]; // normalized motor thrusts in range [0..1]
 
-int motorPins[4] = {12, 13, 14, 15}; // default pin numbers
+int motorPins[4] = {-1, -1, -1, -1}; // default pin numbers
 int pwmFrequency = 78000;
 int pwmResolution = 10;
 int pwmStop = 0;
@@ -19,27 +19,18 @@ const int MOTOR_REAR_LEFT = 0, MOTOR_REAR_RIGHT = 1, MOTOR_FRONT_RIGHT = 2, MOTO
 void setupMotors() {
 	print("Setup motors\n");
 	// Configure pins
-#ifdef ESP32
 	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
 	}
-#else
-	analogWriteResolution(pwmResolution);
-	analogWriteFrequency(pwmFrequency);
-#endif
 	sendMotors();
 }
 
 void sendMotors() {
 	for (int i = 0; i < 4; i++) {
 		if (motorPins[i] < 0) continue; // skip unassigned motors
-#ifdef ESP32
 		ledcWrite(motorPins[i], getDutyCycle(motors[i]));
-#else
-		analogWrite(motorPins[i], getDutyCycle(motors[i]));
-#endif
 	}
 }
 

flix/parameters.ino

@@ -3,18 +3,18 @@
 
 // Parameters storage in flash memory
 
-#include "prefs.h"
+#include <Preferences.h>
 #include "util.h"
+#include "pid.h"
 
 extern int channelZero[16], channelMax[16];
 extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern int rcRxPin, voltagePin;
-extern int wifiMode, wifiLongRange, wifiBroadcast, udpLocalPort, udpRemotePort, espnowChannel;
+extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel;
 extern float rcLossTimeout, descendTime, disarmTilt;
 extern float voltageScale;
 extern LowPassFilter<float> voltageFilter;
-
+extern PID posX_PID, posY_PID;
-#include "config.h"
 
 Preferences storage;
 
@@ -46,7 +46,6 @@ Parameter parameters[] = {
 	{"CTL_Y_RATE_P", &yawRatePID.p},
 	{"CTL_Y_RATE_I", &yawRatePID.i},
 	{"CTL_Y_RATE_D", &yawRatePID.d},
-	{"CTL_Y_RATE_WU", &yawRatePID.windup},
 	{"CTL_Y_RATE_D_A", &yawRatePID.lpf.alpha},
 	{"CTL_R_P", &rollPID.p},
 	{"CTL_R_I", &rollPID.i},
@@ -62,16 +61,13 @@ Parameter parameters[] = {
 	{"CTL_FLT_MODE_0", &flightModes[0]},
 	{"CTL_FLT_MODE_1", &flightModes[1]},
 	{"CTL_FLT_MODE_2", &flightModes[2]},
+	// position
+	{"POS_XY_P", &posX_PID.p, setupPosition},
+	{"POS_XY_I", &posX_PID.i, setupPosition},
+	{"POS_XY_D", &posX_PID.d, setupPosition},
+	{"POS_XY_WU", &posX_PID.windup, setupPosition},
+	{"POS_XY_D_A", &posX_PID.lpf.alpha, setupPosition},
 	// imu
-	{"IMU_MODEL", &imuModel},
-	{"IMU_BUS", &imuBus},
-	{"IMU_PIN_SCK", &imuSckPin},
-	{"IMU_PIN_MISO", &imuMisoPin},
-	{"IMU_PIN_MOSI", &imuMosiPin},
-	{"IMU_PIN_CS", &imuCsPin},
-	{"IMU_PIN_SDA", &imuSdaPin},
-	{"IMU_PIN_SCL", &imuSclPin},
-	{"IMU_PIN_INT", &imuIntPin},
 	{"IMU_ROT_ROLL", &imuRotation.x},
 	{"IMU_ROT_PITCH", &imuRotation.y},
 	{"IMU_ROT_YAW", &imuRotation.z},
@@ -86,6 +82,8 @@ Parameter parameters[] = {
 	{"EST_ACC_WEIGHT", &accWeight},
 	{"EST_LVL_WEIGHT", &levelWeight},
 	{"EST_RATES_LPF_A", &ratesFilter.alpha},
+	{"EST_RATES_NF_F", &ratesNotch.frequency, setupEstimate},
+	{"EST_RATES_NF_BW", &ratesNotch.bandwidth, setupEstimate},
 	// motors
 	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},
 	{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},
@@ -124,7 +122,6 @@ Parameter parameters[] = {
 	{"WIFI_PORT_LOC", &udpLocalPort},
 	{"WIFI_PORT_REM", &udpRemotePort},
 	{"WIFI_LONG_RANGE", &wifiLongRange},
-	{"WIFI_BROADCAST", &wifiBroadcast},
 	// espnow
 	{"ESPNOW_CHANNEL", &espnowChannel},
 	// mavlink
@@ -134,6 +131,22 @@ Parameter parameters[] = {
 	{"MAV_RATE_RC", &telemetryRC.rate},
 	{"MAV_RATE_MOT", &telemetryMotors.rate},
 	{"MAV_RATE_IMU", &telemetryIMU.rate},
+	{"MAV_RATE_TOPIC", &telemetryTopic.rate},
+	// log
+	{"LOG_MEMORY", &logMemory, setupLog},
+	{"LOG_USAGE", &logUsage, setupLog},
+	{"LOG_RATE_000", &logTopics[0].throttle},
+	{"LOG_RATE_001", &logTopics[1].throttle},
+	{"LOG_RATE_002", &logTopics[2].throttle},
+	{"LOG_RATE_003", &logTopics[3].throttle},
+	{"LOG_RATE_004", &logTopics[4].throttle},
+	{"LOG_RATE_005", &logTopics[5].throttle},
+	{"LOG_RATE_006", &logTopics[6].throttle},
+	{"LOG_RATE_007", &logTopics[7].throttle},
+	{"LOG_RATE_008", &logTopics[8].throttle},
+	{"LOG_RATE_009", &logTopics[9].throttle},
+	{"LOG_RATE_010", &logTopics[10].throttle},
+	{"LOG_RATE_011", &logTopics[11].throttle},
 	// power
 	{"PWR_VOLT_PIN", &voltagePin, setupPower},
 	{"PWR_VOLT_SCALE", &voltageScale},
@@ -141,12 +154,11 @@ Parameter parameters[] = {
 	// safety
 	{"SF_RC_LOSS_TIME", &rcLossTimeout},
 	{"SF_DESCEND_TIME", &descendTime},
-	{"SF_DISARM_TILT", &disarmTilt},
+	{"SF_DISARM_TILT", &disarmTilt}
 };
 
 void setupParameters() {
 	print("Setup parameters\n");
-	setDefaults();
 	storage.begin("flix");
 	// Read parameters from storage
 	for (auto &parameter : parameters) {

flix/pid.h

@@ -5,7 +5,7 @@
 
 #pragma once
 
-#include "lpf.h"
+#include "filter.h"
 
 class PID {
 public:
@@ -18,7 +18,7 @@ public:
 
 	LowPassFilter<float> lpf; // low pass filter for derivative term
 
-	PID(float p, float i = 0, float d = 0, float windup = 0, float dAlpha = 1, float dtMax = 0.1) :
+	PID(float p, float i, float d, float windup = 0, float dAlpha = 1, float dtMax = 0.1) :
 		p(p), i(i), d(d), windup(windup), lpf(dAlpha), dtMax(dtMax) {}
 
 	float update(float error) {

flix/position.ino

@@ -0,0 +1,95 @@
+#include "vector.h"
+#include "quaternion.h"
+#include "pid.h"
+#include "util.h"
+
+float mass = 0.065;
+float motorThrust = 0.031 * ONE_G;
+float positionWeight = 0.9;
+float velocityWeight = 0.9;
+
+PID velX_PID(1.0, 0.0, 0.02);
+PID velY_PID(1.0, 0.0, 0.02);
+PID velZ_PID(1.0, 0.0, 0.02);
+
+// PID posX_PID(1.0, 0.0, 0.0);
+// PID posY_PID(1.0, 0.0, 0.0);
+// PID posZ_PID(1.0, 0.0, 0.0);
+
+PID posX_PID(0.1, 0.0, 0.0);
+PID posY_PID(0.1, 0.0, 0.0);
+PID posZ_PID(0.1, 0.0, 0.0);
+
+Vector acceleration;
+Vector velocity;
+Vector position;
+Vector velocityTarget(NAN, NAN, NAN);
+Vector positionTarget(NAN, NAN, NAN);
+
+extern Quaternion attitudeTarget;
+extern float thrustTarget;
+
+void setupPosition() {
+	// TODO: posY_PID = posX_PID;
+	// posX_PID.copy(posY_PID);
+	posY_PID.p = posX_PID.p;
+	posY_PID.i = posX_PID.i;
+	posY_PID.d = posX_PID.d;
+}
+
+void estimatePosition() {
+	acceleration = Quaternion::rotateVector(acceleration, attitude) + Vector(0, 0, -ONE_G);
+	velocity += acceleration * dt;
+	position += velocity * dt;
+}
+
+void correctPosition(Vector& pos) {
+	// position += (pos - position) * positionWeight;
+	position = pos;
+	// positionUpdated = true;
+
+	if (invalid(positionTarget.x) || invalid(positionTarget.y)) return;
+
+	attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x));
+	attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y));
+}
+
+void correctVelocity(Vector& vel) {
+	velocity += (vel - velocity) * velocityWeight;
+}
+
+void controlVelocity() {
+	Vector accelerationTarget(
+		velX_PID.update(velocityTarget.x - velocity.x),
+		velY_PID.update(velocityTarget.y - velocity.y),
+		velZ_PID.update(velocityTarget.z - velocity.z)
+	);
+
+	Vector thrustVector = (accelerationTarget + Vector(0, 0, ONE_G)) * mass;
+
+	const Vector up(0, 0, 1);
+
+	attitudeTarget = Quaternion::fromBetweenVectors(up, thrustVector);
+	float maxThrust = motorThrust * 4; // 4 motors
+	thrustTarget = thrustVector.norm() / maxThrust;
+}
+
+void controlPosition() {
+	if (positionTarget.invalid()) return;
+
+	velocityTarget = Vector(
+		posX_PID.update(positionTarget.x - position.x),
+		posY_PID.update(positionTarget.y - position.y),
+		posZ_PID.update(positionTarget.z - position.z)
+	);
+
+	controlVelocity();
+}
+
+void controlPositionSimple() {
+	if (positionTarget.invalid()) return;
+
+	// straight attitude target control
+	attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x));
+	attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y));
+}

flix/power.ino

@@ -3,11 +3,9 @@
 
 // Power management
 
-#ifdef ESP32
 #include <soc/soc.h>
 #include <soc/rtc_cntl_reg.h>
-#endif
+#include "filter.h"
-#include "lpf.h"
 #include "util.h"
 
 float voltage = NAN;
@@ -16,9 +14,7 @@ int voltagePin = -1;
 float voltageScale = 2;
 
 void setupPower() {
-#ifdef ESP32
 	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA); // disable reset on low voltage
-#endif
 	if (digitalPinToAnalogChannel(voltagePin) == -1) voltagePin = -1; // test ADC pin
 }
 

flix/prefs.h

@@ -1,283 +0,0 @@
-#ifdef ESP32
-#include <Preferences.h>
-#else
-
-#include <Arduino.h>
-#include <EEPROM.h>
-#include <string.h>
-
-class Preferences {
-public:
-	Preferences() = default;
-	~Preferences() = default;
-
-	bool begin(const char *name, bool readOnly = false, const char *partition_label = NULL) {
-		(void)name;
-		(void)readOnly;
-		(void)partition_label;
-		load();
-		started = true;
-		return true;
-	}
-
-	void end() {
-		started = false;
-	}
-
-	bool clear() {
-		if (!started) return false;
-		for (auto &entry : entries) {
-			entry = Entry();
-		}
-		return save();
-	}
-
-	size_t putFloat(const char *key, float value) {
-		if (!started) return 0;
-		int index = ensureKey(key);
-		if (index < 0) return 0;
-		entries[index].hasFloat = true;
-		entries[index].hasString = false;
-		entries[index].stringValue = "";
-		entries[index].floatValue = value;
-		if (!save()) return 0;
-		return sizeof(float);
-	}
-
-	float getFloat(const char *key, float defaultValue = NAN) {
-		if (!started) return defaultValue;
-		int index = findKey(key);
-		if (index < 0 || !entries[index].hasFloat) return defaultValue;
-		return entries[index].floatValue;
-	}
-
-	size_t putString(const char *key, const char *value) {
-		if (!started) return 0;
-		int index = ensureKey(key);
-		if (index < 0) return 0;
-		if (!value) value = "";
-		if (strlen(value) > MAX_STRING_LEN) return 0;
-
-		entries[index].hasString = true;
-		entries[index].hasFloat = false;
-		entries[index].stringValue = value;
-		if (!save()) return 0;
-		return entries[index].stringValue.length();
-	}
-
-	size_t putString(const char *key, String value) {
-		return putString(key, value.c_str());
-	}
-
-	String getString(const char *key, String defaultValue = String()) {
-		if (!started) return defaultValue;
-		int index = findKey(key);
-		if (index < 0 || !entries[index].hasString) return defaultValue;
-		return entries[index].stringValue;
-	}
-
-	bool isKey(const char *key) {
-		if (!started) return false;
-		return findKey(key) >= 0;
-	}
-
-private:
-	static const int MAX_ENTRIES = 128;
-	static const int MAX_KEY_LEN = 15;
-	static const int MAX_STRING_LEN = 95;
-	static const uint32_t MAGIC = 0x46504B56; // "VKPF"
-	static const uint8_t VERSION = 1;
-	static const uint8_t TYPE_FLOAT = 1;
-	static const uint8_t TYPE_STRING = 2;
-	static const int STORAGE_SIZE = 4096;
-
-	struct Entry {
-		bool used = false;
-		char key[MAX_KEY_LEN + 1] = {};
-		bool hasFloat = false;
-		float floatValue = NAN;
-		bool hasString = false;
-		String stringValue;
-	};
-
-	bool started = false;
-	Entry entries[MAX_ENTRIES];
-
-	static void writeU16(uint8_t *dst, uint16_t value) {
-		dst[0] = static_cast<uint8_t>(value & 0xFF);
-		dst[1] = static_cast<uint8_t>((value >> 8) & 0xFF);
-	}
-
-	static void writeU32(uint8_t *dst, uint32_t value) {
-		dst[0] = static_cast<uint8_t>(value & 0xFF);
-		dst[1] = static_cast<uint8_t>((value >> 8) & 0xFF);
-		dst[2] = static_cast<uint8_t>((value >> 16) & 0xFF);
-		dst[3] = static_cast<uint8_t>((value >> 24) & 0xFF);
-	}
-
-	static uint16_t readU16(const uint8_t *src) {
-		return static_cast<uint16_t>(src[0]) |
-			(static_cast<uint16_t>(src[1]) << 8);
-	}
-
-	static uint32_t readU32(const uint8_t *src) {
-		return static_cast<uint32_t>(src[0]) |
-			(static_cast<uint32_t>(src[1]) << 8) |
-			(static_cast<uint32_t>(src[2]) << 16) |
-			(static_cast<uint32_t>(src[3]) << 24);
-	}
-
-	int availableStorageSize() {
-		int n = STORAGE_SIZE;
-		if (EEPROM.length() > 0 && EEPROM.length() < n) n = EEPROM.length();
-		return n;
-	}
-
-	bool save() {
-		const int storageSize = availableStorageSize();
-		if (storageSize < 16) return false;
-
-		uint8_t buffer[STORAGE_SIZE] = {};
-		int pos = 0;
-		writeU32(buffer + pos, MAGIC);
-		pos += 4;
-		buffer[pos++] = VERSION;
-		buffer[pos++] = 0;
-		int lengthPos = pos;
-		pos += 2;
-
-		for (int i = 0; i < MAX_ENTRIES; i++) {
-			if (!entries[i].used) continue;
-			if (!entries[i].hasFloat && !entries[i].hasString) continue;
-
-			const uint8_t keyLen = static_cast<uint8_t>(strnlen(entries[i].key, MAX_KEY_LEN));
-			if (keyLen == 0) continue;
-
-			if (entries[i].hasFloat) {
-				const int recordSize = 3 + keyLen + 4;
-				if (pos + recordSize > storageSize) return false;
-
-				buffer[pos++] = TYPE_FLOAT;
-				buffer[pos++] = keyLen;
-				buffer[pos++] = 4;
-				memcpy(buffer + pos, entries[i].key, keyLen);
-				pos += keyLen;
-				float value = entries[i].floatValue;
-				memcpy(buffer + pos, &value, sizeof(value));
-				pos += sizeof(value);
-			} else if (entries[i].hasString) {
-				const uint8_t valueLen = static_cast<uint8_t>(entries[i].stringValue.length());
-				const int recordSize = 3 + keyLen + valueLen;
-				if (pos + recordSize > storageSize) return false;
-
-				buffer[pos++] = TYPE_STRING;
-				buffer[pos++] = keyLen;
-				buffer[pos++] = valueLen;
-				memcpy(buffer + pos, entries[i].key, keyLen);
-				pos += keyLen;
-				if (valueLen > 0) {
-					memcpy(buffer + pos, entries[i].stringValue.c_str(), valueLen);
-					pos += valueLen;
-				}
-			}
-		}
-
-		writeU16(buffer + lengthPos, static_cast<uint16_t>(pos));
-
-		for (int i = 0; i < storageSize; i++) {
-			EEPROM.write(i, buffer[i]);
-		}
-		EEPROM.commit();
-		return true;
-	}
-
-	void load() {
-		const int storageSize = availableStorageSize();
-		if (storageSize < 16) return;
-
-		for (auto &entry : entries) entry = Entry();
-
-		uint8_t buffer[STORAGE_SIZE] = {};
-		for (int i = 0; i < storageSize; i++) {
-			buffer[i] = EEPROM.read(i);
-		}
-
-		int pos = 0;
-		if (readU32(buffer + pos) != MAGIC) return;
-		pos += 4;
-		if (buffer[pos++] != VERSION) return;
-		pos++; // flags
-
-		const uint16_t totalLen = readU16(buffer + pos);
-		pos += 2;
-		if (totalLen < pos || totalLen > storageSize) return;
-
-		while (pos + 3 <= totalLen) {
-			const uint8_t type = buffer[pos++];
-			const uint8_t keyLen = buffer[pos++];
-			const uint8_t valueLen = buffer[pos++];
-			if (keyLen == 0 || keyLen > MAX_KEY_LEN) return;
-			if (pos + keyLen + valueLen > totalLen) return;
-
-			char key[MAX_KEY_LEN + 1] = {};
-			memcpy(key, buffer + pos, keyLen);
-			key[keyLen] = '\0';
-			pos += keyLen;
-
-			int index = ensureKey(key);
-			if (index < 0) return;
-
-			if (type == TYPE_FLOAT && valueLen == 4) {
-				float value = NAN;
-				memcpy(&value, buffer + pos, sizeof(value));
-				entries[index].hasFloat = true;
-				entries[index].hasString = false;
-				entries[index].stringValue = "";
-				entries[index].floatValue = value;
-			} else if (type == TYPE_STRING && valueLen <= MAX_STRING_LEN) {
-				char value[MAX_STRING_LEN + 1] = {};
-				if (valueLen > 0) memcpy(value, buffer + pos, valueLen);
-				value[valueLen] = '\0';
-				entries[index].hasString = true;
-				entries[index].hasFloat = false;
-				entries[index].stringValue = value;
-			}
-
-			pos += valueLen;
-		}
-	}
-
-	int findKey(const char *key) {
-		if (!key) return -1;
-		for (int i = 0; i < MAX_ENTRIES; i++) {
-			if (!entries[i].used) continue;
-			if (strncmp(entries[i].key, key, MAX_KEY_LEN + 1) == 0) return i;
-		}
-		return -1;
-	}
-
-	int ensureKey(const char *key) {
-		if (!key) return -1;
-		if (strlen(key) > MAX_KEY_LEN) return -1;
-
-		int index = findKey(key);
-		if (index >= 0) return index;
-
-		for (int i = 0; i < MAX_ENTRIES; i++) {
-			if (entries[i].used) continue;
-			entries[i].used = true;
-			entries[i].hasFloat = false;
-			entries[i].hasString = false;
-			entries[i].floatValue = NAN;
-			entries[i].stringValue = "";
-			strncpy(entries[i].key, key, MAX_KEY_LEN);
-			entries[i].key[MAX_KEY_LEN] = '\0';
-			return i;
-		}
-
-		return -1;
-	}
-};
-
-
-#endif

flix/util.h

@@ -6,11 +6,7 @@
 #pragma once
 
 #include <math.h>
-#ifdef ESP32
-#include <soc/soc.h>
-#include <soc/rtc_cntl_reg.h>
 #include <ESP32_NOW_Serial.h>
-#endif
 
 const float ONE_G = 9.80665;
 extern float t;
@@ -57,7 +53,6 @@ void splitString(String& str, String& token0, String& token1, String& token2) {
 	if (token2.c_str() == NULL) token2 = "";
 }
 
-#ifdef ESP32
 // Simplified ESP-NOW Serial without resends
 class ESPNOWSerial : public ESP_NOW_Serial_Class {
 public:
@@ -68,13 +63,55 @@ public:
 		ESP_NOW_Serial_Class::onSent(true); // always report success to avoid resends
 	}
 };
-#endif
+
+// Simple variant type for logging and parameters
+struct Value {
+	enum { EMPTY, FLOAT, INT, BOOL, FLOAT_FN, INT_FN, BOOL_FN } type;
+	union {
+		void *pointer;
+		float *_float;
+		int *_int;
+		bool *_bool;
+		float (*floatFn)();
+		int (*intFn)();
+		bool (*boolFn)();
+	};
+
+	Value() : type(EMPTY), pointer(nullptr) {};
+	Value(float *pt) : type(FLOAT), _float(pt) {};
+	Value(int *pt) : type(INT), _int(pt) {};
+	Value(bool *pt) : type(BOOL), _bool(pt) {};
+	Value(float (*fn)()) : type(FLOAT_FN), floatFn(fn) {};
+	Value(int (*fn)()) : type(INT_FN), intFn(fn) {};
+	Value(bool (*fn)()) : type(BOOL_FN), boolFn(fn) {};
+
+	float get() const {
+		switch (type) {
+			case FLOAT: return *_float;
+			case INT: return *_int;
+			case BOOL: return *_bool ? 1 : 0;
+			case FLOAT_FN: return floatFn();
+			case INT_FN: return intFn();
+			case BOOL_FN: return boolFn() ? 1 : 0;
+			default: return NAN;
+		}
+	};
+
+	void set(float value) const {
+		switch (type) {
+			case FLOAT: *_float = value; break;
+			case INT: *_int = value; break;
+			case BOOL: *_bool = (value != 0); break;
+			default: break;
+		}
+	};
+};
 
 // Rate limiter
 class Rate {
 public:
 	float rate;
-	float last = -INFINITY;
+	float last = 0;
 	Rate(float rate) : rate(rate) {}
 
 	operator bool() {

flix/wifi.ino

@@ -3,13 +3,13 @@
 
 // Wi-Fi and ESP-NOW communication
 
-// #include <WiFi.h>
+#include <WiFi.h>
-// #include <WiFiAP.h>
+#include <WiFiAP.h>
-// #include <WiFiUdp.h>
+#include <WiFiUdp.h>
-// #include <MacAddress.h>
+#include <MacAddress.h>
-// #include <ESP32_NOW_Serial.h>
+#include <ESP32_NOW_Serial.h>
-// #include "prefs.h"
+#include <Preferences.h>
-// #include "util.h"
+#include "util.h"
 
 extern Preferences storage; // use the main preferences storage
 
@@ -17,124 +17,121 @@ const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
 int wifiMode = W_AP;
 
 int wifiLongRange = 0;
-int wifiBroadcast = 0; // 0 - broadcast until connected, 1 - always broadcast
 int udpLocalPort = 14550;
 int udpRemotePort = 14550;
-// IPAddress udpRemoteIP = "255.255.255.255";
+IPAddress udpRemoteIP = "255.255.255.255";
-// WiFiUDP udp;
+WiFiUDP udp;
 
-// ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
+ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
-// ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
+ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
 int espnowChannel = 6;
 
 void setupWiFi() {
-	// print("Setup Wi-Fi\n");
+	print("Setup Wi-Fi\n");
-	// WiFi.enableLongRange(wifiLongRange);
+	WiFi.enableLongRange(wifiLongRange);
 
-	// if (wifiMode == W_AP) {
+	if (wifiMode == W_AP) {
-	// 	WiFi.softAP(storage.getString("WIFI_AP_SSID", "flix").c_str(), storage.getString("WIFI_AP_PASS", "flixwifi").c_str());
+		WiFi.softAP(storage.getString("WIFI_AP_SSID", "flix").c_str(), storage.getString("WIFI_AP_PASS", "flixwifi").c_str());
-	// 	udp.begin(udpLocalPort);
+		udp.begin(udpLocalPort);
-	// }
+	}
 
-	// if (wifiMode == W_STA) {
+	if (wifiMode == W_STA) {
-	// 	WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str());
+		WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str());
-	// 	udp.begin(udpLocalPort);
+		udp.begin(udpLocalPort);
-	// }
+	}
 
-	// if (wifiMode == W_ESPNOW) {
+	if (wifiMode == W_ESPNOW) {
-	// 	WiFi.mode(WIFI_AP);
+		WiFi.mode(WIFI_AP);
-	// 	WiFi.setChannel(espnowChannel);
+		WiFi.setChannel(espnowChannel);
-	// 	espnow.addr(MacAddress(storage.getString("ESPNOW_PEER_MAC", "FF:FF:FF:FF:FF:FF").c_str()));
+		espnow.addr(MacAddress(storage.getString("ESPNOW_PEER_MAC", "FF:FF:FF:FF:FF:FF").c_str()));
-	// 	String key = storage.getString("ESPNOW_PEER_KEY", "");
+		String key = storage.getString("ESPNOW_PEER_KEY", "");
-	// 	espnow.setKey(key.isEmpty() ? nullptr : (const uint8_t *)key.c_str());
+		espnow.setKey(key.isEmpty() ? nullptr : (const uint8_t *)key.c_str());
-	// 	espnow.begin();
+		espnow.begin();
-	// 	espnowBroadcast.begin();
+		espnowBroadcast.begin();
-	// }
+	}
 
-	// WiFi.setSleep(false); // disable power save
+	WiFi.setSleep(false); // disable power save
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
-	// if (espnow) {
+	if (espnow) {
-	// 	espnow.write(buf, len);
+		espnow.write(buf, len);
 
-	// 	static Rate discovery(2);
+		static Rate discovery(2);
-	// 	if (espnow.isEncrypted() && discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
+		if (espnow.isEncrypted() && discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
-	// 	return;
+		return;
-	// }
+	}
 
-	// if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return;
+	if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return;
 
-	// bool broadcast = wifiBroadcast || !(t - mavlinkTime < 5); // broadcast if lost connection
+	udp.beginPacket(udpRemoteIP, udpRemotePort);
-	// udp.beginPacket(broadcast ? IPAddress(255, 255, 255, 255) : udpRemoteIP, udpRemotePort);
+	udp.write(buf, len);
-	// udp.write(buf, len);
+	udp.endPacket();
-	// udp.endPacket();
 }
 
 int receiveWiFi(uint8_t *buf, int len) {
-	// if (espnow) {
+	if (espnow) {
-	// 	return espnow.read(buf, len);
+		return espnow.read(buf, len);
-	// }
+	}
 
-	// if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return 0;
+	if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return 0;
 
-	// udp.parsePacket();
+	udp.parsePacket();
-	// if (udp.remoteIP()) udpRemoteIP = udp.remoteIP();
+	if (udp.remoteIP()) udpRemoteIP = udp.remoteIP();
-	// return udp.read(buf, len);
+	return udp.read(buf, len);
-	return 0;
 }
 
 void printWiFiInfo() {
-	// if (espnow) {
+	if (espnow) {
-	// 	print("Mode: ESP-NOW\n");
+		print("Mode: ESP-NOW\n");
-	// 	print("ESP-NOW version: %d\n", ESP_NOW.getVersion());
+		print("ESP-NOW version: %d\n", ESP_NOW.getVersion());
-	// 	print("Max packet size: %d\n", ESP_NOW.getMaxDataLen());
+		print("Max packet size: %d\n", ESP_NOW.getMaxDataLen());
-	// 	print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
+		print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
-	// 	print("Peer MAC: %s\n", MacAddress(espnow.addr()).toString().c_str());
+		print("Peer MAC: %s\n", MacAddress(espnow.addr()).toString().c_str());
-	// 	print("Encrypted: %d\n", espnow.isEncrypted());
+		print("Encrypted: %d\n", espnow.isEncrypted());
-	// 	print("Channel: %d\n", espnow.getChannel());
+		print("Channel: %d\n", WiFi.channel());
-	// 	print("Lost packets: %d\n", espnow.lost);
+		print("Lost packets: %d\n", espnow.lost);
-	// } else if (WiFi.getMode() == WIFI_MODE_AP) {
+	} else if (WiFi.getMode() == WIFI_MODE_AP) {
-	// 	print("Mode: Access Point (AP)\n");
+		print("Mode: Access Point (AP)\n");
-	// 	print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
+		print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
-	// 	print("SSID: %s\n", WiFi.softAPSSID().c_str());
+		print("SSID: %s\n", WiFi.softAPSSID().c_str());
-	// 	print("Password: ***\n");
+		print("Password: ***\n");
-	// 	print("Channel: %d\n", WiFi.channel());
+		print("Channel: %d\n", WiFi.channel());
-	// 	print("Clients: %d\n", WiFi.softAPgetStationNum());
+		print("Clients: %d\n", WiFi.softAPgetStationNum());
-	// 	print("IP: %s\n", WiFi.softAPIP().toString().c_str());
+		print("IP: %s\n", WiFi.softAPIP().toString().c_str());
-	// 	print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
+		print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
-	// } else if (WiFi.getMode() == WIFI_MODE_STA) {
+	} else if (WiFi.getMode() == WIFI_MODE_STA) {
-	// 	print("Mode: Client (STA)\n");
+		print("Mode: Client (STA)\n");
-	// 	print("Connected: %d\n", WiFi.isConnected());
+		print("Connected: %d\n", WiFi.isConnected());
-	// 	print("MAC: %s\n", WiFi.macAddress().c_str());
+		print("MAC: %s\n", WiFi.macAddress().c_str());
-	// 	print("SSID: %s\n", WiFi.SSID().c_str());
+		print("SSID: %s\n", WiFi.SSID().c_str());
-	// 	print("Password: ***\n");
+		print("Password: ***\n");
-	// 	print("Channel: %d\n", WiFi.channel());
+		print("Channel: %d\n", WiFi.channel());
-	// 	print("RSSI: %d dBm\n", WiFi.RSSI());
+		print("RSSI: %d dBm\n", WiFi.RSSI());
-	// 	print("IP: %s\n", WiFi.localIP().toString().c_str());
+		print("IP: %s\n", WiFi.localIP().toString().c_str());
-	// 	print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
+		print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
-	// } else {
+	} else {
-	// 	print("Mode: Disabled\n");
+		print("Mode: Disabled\n");
-	// }
+	}
-	// print("MAVLink connected: %d\n", valid(mavlinkTime));
+	print("MAVLink connected: %d\n", mavlinkConnected);
 }
 
 void configWiFi(int mode, const char *first, const char *second) {
-	// MacAddress mac;
+	MacAddress mac;
-	// if (mode == W_AP && strlen(first) > 0 && strlen(second) >= 8) {
+	if (mode == W_AP && strlen(first) > 0 && strlen(second) >= 8) {
-	// 	storage.putString("WIFI_AP_SSID", first);
+		storage.putString("WIFI_AP_SSID", first);
-	// 	storage.putString("WIFI_AP_PASS", second);
+		storage.putString("WIFI_AP_PASS", second);
-	// } else if (mode == W_STA && strlen(first) > 0 && strlen(second) >= 8) {
+	} else if (mode == W_STA && strlen(first) > 0 && strlen(second) >= 8) {
-	// 	storage.putString("WIFI_STA_SSID", first);
+		storage.putString("WIFI_STA_SSID", first);
-	// 	storage.putString("WIFI_STA_PASS", second);
+		storage.putString("WIFI_STA_PASS", second);
-	// } else if (mode == W_ESPNOW && mac.fromString(first)) {
+	} else if (mode == W_ESPNOW && mac.fromString(first)) {
-	// 	storage.putString("ESPNOW_PEER_MAC", first);
+		storage.putString("ESPNOW_PEER_MAC", first);
-	// 	storage.putString("ESPNOW_PEER_KEY", strlen(second) == ESP_NOW_KEY_LEN ? second : "");
+		storage.putString("ESPNOW_PEER_KEY", strlen(second) == ESP_NOW_KEY_LEN ? second : "");
-	// } else {
+	} else {
-	// 	print("Invalid configuration\n");
+		print("Invalid configuration\n");
-	// 	return;
+		return;
-	// }
+	}
-	// print("✓ Reboot to apply new settings\n");
+	print("✓ Reboot to apply new settings\n");
 }
 
 void setWiFiMode(const String& mode) {

gazebo/Arduino.h

@@ -20,6 +20,10 @@
 #define radians(deg) ((deg)*DEG_TO_RAD)
 #define degrees(rad) ((rad)*RAD_TO_DEG)
 
+#define MALLOC_CAP_SPIRAM (1<<10)
+#define MALLOC_CAP_8BIT (1<<2)
+#define ESP_NOW_MAX_DATA_LEN_V2 1470
+
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 template<typename T> T max(T a, T b) { return a > b ? a : b; }
 template<typename T> T min(T a, T b) { return a < b ? a : b; }
@@ -156,6 +160,8 @@ HardwareSerial Serial, Serial1, Serial2;
 class EspClass {
 public:
 	void restart() { Serial.println("Ignore reboot in simulation"); }
+	uint32_t getFreeHeap() { return 300 * 1024; } // assume 300 KB free heap
+	uint32_t getFreePsram() { return 8 * 1024 * 1024; } // assume 8 MB free PSRAM
 } ESP;
 
 unsigned long __delayTime = 0;
@@ -165,11 +171,16 @@ void delay(uint32_t ms) {
 	__delayTime += ms * 1000;
 }
 
+void *heap_caps_calloc(size_t n, size_t size, uint32_t caps) {
+	return calloc(n, size);
+}
+
 bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
 bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
 uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution) { return freq; }
 int8_t digitalPinToAnalogChannel(uint8_t pin) { return -1; }
 uint32_t analogReadMilliVolts(uint8_t pin) { return 0; }
+float temperatureRead() { return 0; }
 
 unsigned long __micros;
 unsigned long __resetTime = 0;

gazebo/flix.h

@@ -9,7 +9,7 @@
 #include "quaternion.h"
 #include "Arduino.h"
 #include "wifi.h"
-#include "lpf.h"
+#include "filter.h"
 
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -21,9 +21,6 @@ extern float motors[4];
 Vector gyro, acc, imuRotation;
 Vector accBias, gyroBias, accScale(1, 1, 1);
 LowPassFilter<Vector> gyroBiasFilter(0);
-int imuModel = 1, imuBus = 0;
-int imuSckPin = 0, imuMisoPin = 0, imuMosiPin = 0, imuCsPin = -1, imuIntPin = -1;
-int imuSdaPin = 0, imuSclPin = 0;
 
 // declarations
 void step();
@@ -51,8 +48,17 @@ void normalizeRC();
 void calibrateRC();
 void calibrateRCChannel(int*, uint16_t[16], uint16_t[16], const char*);
 void printRCCalibration();
+void loopLog();
+void resetLog();
+void writeLog(const void *data, size_t size);
+void readLog(void *data, size_t position, size_t size);
+bool isTopicUpdated(const uint8_t topic);
+void printLogInfo();
+int estimateLogDuration();
 void printLogHeader();
-void printLogData();
+void printLogValues(const char *filter);
+void configLogThrottle(const char *name, float throttle);
+void exposeLogValue(const char *name);
 void processMavlink();
 void sendMavlink();
 void sendMessage(const void *msg);
@@ -83,3 +89,8 @@ 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"); };
+
+class IMU {
+public:
+	float getTemp() { return 0; }
+} imu;

gazebo/simulator.cpp

@@ -23,7 +23,7 @@
 #include "estimate.ino"
 #include "safety.ino"
 #include "log.ino"
-#include "lpf.h"
+#include "filter.h"
 #include "mavlink.ino"
 #include "motors.ino"
 #include "parameters.ino"
@@ -55,6 +55,7 @@ public:
 		initNode();
 		Serial.begin(0);
 		setupParameters();
+		setupLog();
 		rcRxPin = 1; // set rc pin to enable rc reading
 		gzmsg << "Flix plugin loaded" << endl;
 	}
@@ -88,7 +89,7 @@ public:
 
 		applyMotorForces();
 		publishTopics();
-		logData();
+		loopLog();
 		syncParameters();
 	}
 

tools/csv_to_mcap.py

@@ -9,6 +9,7 @@ Usage:
 import csv
 import json
 import docopt
+import math
 from mcap.writer import Writer
 
 args = docopt.docopt(__doc__)
@@ -39,7 +40,15 @@ channel_id = writer.register_channel(
 )
 
 for row in csv_reader:
-    data = {key: float(value) for key, value in zip(header, row)}
+    if row[0] == '': continue
+    data = {}
+    for key, value in zip(header, row):
+        if value == '' or math.isnan(float(value)):
+            data[key] = None
+        else:
+            data[key] = float(value)
+
+    data = {key: float(value) if value != '' else None for key, value in zip(header, row)}
     timestamp = round(float(row[0]) * 1e9)
     writer.add_message(channel_id=channel_id, log_time=timestamp, data=json.dumps(data).encode(), publish_time=timestamp,)
 

tools/espnow-proxy/espnow-proxy.ino

@@ -11,18 +11,22 @@
 #include <Preferences.h>
 #include "../../flix/util.h"
 
-const int CHANNEL = 6;
+const bool DISABLE_SWARM = true;
+const int CHANNEL = -1; // -1 means auto search
 char key[ESP_NOW_KEY_LEN + 1] = {0}; // with trailing null
 
 Preferences storage;
 
 std::vector<ESPNOWSerial *> peers;
+bool stop = false;
 
 void onNewPeer(const esp_now_recv_info_t *info, const uint8_t *data, int len, void *arg) {
 	if (len != 4 || memcmp(data, "flix", 4) != 0) return; // check if discovery message
 
+	if (stop) return;
+
 	Serial.printf("New peer: " MACSTR "\n", MAC2STR(info->src_addr));
-	ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, CHANNEL, WIFI_IF_AP);
+	ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, WiFi.channel(), WIFI_IF_STA);
 	link->begin();
 	link->setKey((const uint8_t *)key);
 	peers.push_back(link);
@@ -30,9 +34,8 @@ void onNewPeer(const esp_now_recv_info_t *info, const uint8_t *data, int len, vo
 
 void setup() {
 	Serial.begin(115200);
-	WiFi.mode(WIFI_AP);
+	WiFi.mode(WIFI_STA);
 	WiFi.setSleep(false);
-	WiFi.setChannel(CHANNEL);
 
 	ESP_NOW.onNewPeer(onNewPeer, NULL);
 	ESP_NOW.begin();
@@ -43,12 +46,6 @@ void setup() {
 		storage.putString("key", key);
 	}
 	strcpy(key, storage.getString("key").c_str());
-
-	// Discover the first peer
-	while (peers.empty()) {
-		Serial.printf("espnow %s %s\n", WiFi.softAPmacAddress().c_str(), key);
-		delay(500);
-	}
 }
 
 void generateRandomKey() {
@@ -61,6 +58,20 @@ void generateRandomKey() {
 void loop() {
 	uint8_t buf[5000];
 
+	static int channelIndex = 0;
+	static const int channels[] = {6, 11, 1, 2, 6, 11, 3, 4, 5, 6, 1, 11, 8, 6, 9, 10, 11, 6, 1, 12, 13}; // 6, 1 and 11 are most common
+
+	static unsigned long last = 0;
+	if (!stop && millis() - last > 500) {
+		// Change search channel
+		last = millis();
+		channelIndex = (channelIndex + 1) % (sizeof(channels) / sizeof(channels[0]));
+		int channel = CHANNEL < 0 ? channels[channelIndex] : CHANNEL;
+		Serial.printf("Run on Flix: espnow %s %s\n", WiFi.STA.macAddress().c_str(), key);
+		Serial.printf("Searching channel %d\n", channel);
+		WiFi.setChannel(channel);
+	}
+
 	// Send from Serial to ESP-NOW
 	while (Serial.available() > 0) {
 		int b = Serial.read();
@@ -81,6 +92,15 @@ void loop() {
 	// Send from ESP-NOW to Serial
 	for (ESPNOWSerial *link : peers) {
 		int len = link->read(buf, sizeof(buf));
+		if (!stop) {
+			for (int i = 0; i < len; i++) {
+				if (buf[i] == MAVLINK_STX) {
+					// Got MAVLink message, stop discovery
+					Serial.printf("Received MAVLink from " MACSTR "\n", MAC2STR(link->addr()));
+					if (DISABLE_SWARM) stop = true;
+				}
+			}
+		}
 		if (len > 0) {
 			Serial.write(buf, len);
 		}

tools/log_to_csv.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+
+"""Convert flight from Flix format to CSV
+
+Usage:
+    log_to_csv.py <input_file>
+"""
+
+import os
+from pyflix import Flix
+import docopt
+import struct
+import csv
+
+DIR = os.path.dirname(os.path.realpath(__file__))
+HEADER_FILE = os.path.join(DIR, 'log/log_header.txt')
+
+# Read log header
+try:
+    # read from file
+    header = open(HEADER_FILE, 'r').read()
+except FileNotFoundError:
+    flix = Flix()
+    header = flix.cli('log header')  # receive the log schema
+    open(HEADER_FILE, 'w').write(header)  # save to file
+
+# Parse log header
+topics = []
+for line in header.splitlines():
+    if not line.startswith('  '):
+        topics.append([])
+    elif 'not logged' not in line:
+        topics[-1].append(line.strip())
+
+# Read log file
+args = docopt.docopt(__doc__)
+input_file = args['<input_file>']
+outfile_file = input_file + '.csv'
+data = open(input_file, 'rb').read()
+
+# Search for sync marker
+SYNC_MARKER = bytes.fromhex('1A 91 4F F6 7F')
+sync_offset = data.find(SYNC_MARKER)
+if sync_offset == -1:
+    raise ValueError('Sync marker not found in log file')
+data = data[sync_offset + len(SYNC_MARKER):]
+data = data.replace(SYNC_MARKER, b'')  # remove all other sync markers
+
+header_row = [f'{value}' for topic in topics for value in topic]
+rows = []
+
+offset = 0
+while offset < len(data):
+    try:
+        topic = struct.unpack_from('B', data, offset)[0]
+        offset += 1
+
+        if topic >= len(topics):
+            raise ValueError(f'Invalid topic {topic} at offset {offset}')
+
+        if topic == 0 or not rows:
+            rows.append({})
+
+        for name in topics[topic]:
+            value = struct.unpack_from('<f', data, offset)[0]
+            offset += 4
+            rows[-1][name] = value
+    except struct.error as e:
+        break
+
+# Write CSV file
+with open(outfile_file, 'w', newline='') as f:
+    writer = csv.DictWriter(f, fieldnames=header_row, extrasaction='ignore')
+    writer.writeheader()
+    rows = filter(lambda row: float(row.get('t', 0)), rows)
+    writer.writerows(rows)