Use dev version of FlixPeriph

These arguments are effectively immutable, so Sequence makes more sense - the user may use tuples in addition to lists.

.github/workflows/build.yml

@@ -27,7 +27,13 @@ jobs:
     - 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 FLAGS=-DFLIX2 EXTRA=--output-dir=flix/build/esp32.esp32.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: Install STM32 core
+      run: arduino-cli core install STMicroelectronics:stm32 --additional-urls https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
+    - name: Build firmware for STM32F4
+      run: make BOARD=STMicroelectronics:stm32:GenF4:usb=CDCgen EXTRA='--build-property compiler.cpp.extra_flags=-DENABLE_HWSERIAL1'
+    - name: Build firmware for STM32H7
+      run: make BOARD=STMicroelectronics:stm32:GenH7:usb=CDCgen EXTRA='--build-property compiler.cpp.extra_flags=-DENABLE_HWSERIAL1'
     - name: Upload binaries
       uses: actions/upload-artifact@v7
       with:

Makefile

@@ -1,14 +1,10 @@
 BOARD = esp32:esp32:esp32
 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*))
-VERSION = $(shell git describe --always --dirty)
 
 export ARDUINO_NETWORK_CONNECTION_TIMEOUT := 1h
 
 build: .core .libs
-	arduino-cli compile flix \
-		--fqbn $(BOARD) \
-		--build-property "build.core_debug_level=1" \
-		--build-property "compiler.cpp.extra_flags=-DVERSION=$(VERSION) $(FLAGS)" $(EXTRA)
+	arduino-cli compile flix --fqbn $(BOARD) --build-property "build.core_debug_level=1" $(EXTRA)
 
 upload: build
 	arduino-cli upload flix --fqbn $(BOARD) -p "$(PORT)"
@@ -16,6 +12,9 @@ upload: build
 erase:
 	arduino-cli burn-bootloader --fqbn $(BOARD) -p "$(PORT)" -P esptool
 
+erase:
+	arduino-cli burn-bootloader --fqbn $(BOARD) -p "$(PORT)" -P esptool
+
 monitor:
 	arduino-cli monitor -p "$(PORT)" -c baudrate=115200
 

docs/firmware.md

@@ -73,7 +73,7 @@ Parameters subsystem (`parameters.ino`) uses standard [Preferences.h](https://do
 
 To add a new parameter:
 
-1. Define a global variable for the parameter, three types are supported: `float`, `int`, and `bool`.
+1. Define a global variable for the parameter, two types are supported: `float` and `int`.
 2. Add an entry to the `parameters` array, with the parameter name, a pointer to the variable, and optionally a callback function to call when the parameter is changed.
 3. Everything else will be handled automatically.
 

flix/cli.ino

@@ -50,14 +50,8 @@ 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"
-"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"
+"mfr/mfl/mrr/mrl [<thrust>] - test motor (remove props)\n"
 "sys - show system info\n"
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
@@ -105,7 +99,7 @@ void doCommand(String str, bool echo = false) {
 		if (success) {
 			print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
 		} else {
-			print("Cannot set parameter: %s\n", arg0.c_str());
+			print("Parameter not found: %s\n", arg0.c_str());
 		}
 	} else if (command == "preset") {
 		resetParameters();
@@ -159,18 +153,9 @@ 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" && arg0 == "") {
-		printLogInfo();
-	} else if (command == "log" && arg1 != "") {
-		configLogThrottle(arg0.c_str(), arg1.toFloat());
-	} else if (command == "log" && arg0 == "header") {
+	} else if (command == "log") {
 		printLogHeader();
-	} 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());
+		if (arg0 == "dump") printLogData();
 	} else if (command == "cr") {
 		calibrateRC();
 	} else if (command == "ca") {
@@ -189,12 +174,7 @@ 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);
-#ifdef VERSION
-		print("Version: %s\n", STRINGIFY(VERSION));
-#endif
-		print("Build date: " __DATE__ " " __TIME__ "\n");
+		print("Firmware: " __DATE__ " " __TIME__ "\n");
 		// Print tasks table
 		print("Num  Task                MinSt  Prio  Core  CPU%%\n");
 		int taskCount = uxTaskGetNumberOfTasks();
@@ -213,7 +193,7 @@ void doCommand(String str, bool echo = false) {
 		attitude = Quaternion();
 		gyroBiasFilter.reset();
 	} else if (command == "reboot") {
-		ESP.restart();
+		reboot();
 	} else {
 		print("Invalid command: %s\n", command.c_str());
 	}
@@ -232,7 +212,7 @@ void handleInput() {
 		char c = Serial.read();
 		if (c == '\n' || c == '\r') {
 			doCommand(input);
-			input.clear();
+			input = "";
 		} else {
 			input += c;
 		}

flix/control.ino

@@ -16,7 +16,7 @@ bool armed = false;
 Quaternion attitudeTarget;
 Vector ratesTarget;
 Vector ratesExtra; // feedforward rates
-Vector torqueTarget; // 0 - no torque, 1 - maximum torque
+Vector torqueTarget;
 float thrustTarget;
 
 PID rollRatePID(0.05, 0.2, 0.001, 0.3, 0.2);

flix/estimate.ino

@@ -15,12 +15,6 @@ bool landed;
 float accWeight = 0.003;
 float levelWeight = 0.0002;
 LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
-NotchFilter<Vector> ratesNotch(382, 40);
-
-void setupEstimate() {
-	print("Setup estimation\n");
-	ratesNotch.reset();
-}
 
 void estimate() {
 	applyGyro();
@@ -31,7 +25,6 @@ void estimate() {
 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

@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Low pass and notch filters
+// Low pass filter implementation
 
 #pragma once
 
@@ -32,67 +32,3 @@ public:
 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,8 +26,6 @@ void setup() {
 	setupWiFi();
 	setupIMU();
 	setupRC();
-	setupEstimate();
-	setupLog();
 	setLED(false);
 	print("Initializing complete\n");
 }
@@ -42,6 +40,6 @@ void loop() {
 	handleInput();
 	processMavlink();
 	readVoltage();
-	loopLog();
+	logData();
 	syncParameters();
 }

flix/imu.ino

@@ -33,11 +33,23 @@ void setupIMU() {
 
 	if (imuBus == 0) {
 		// SPI connection
+#if defined(ESP32)
 		SPI.begin(imuSckPin, imuMisoPin, imuMosiPin);
+#elif defined(ARDUINO_ARCH_STM32)
+		SPI.setSCLK(imuSckPin);
+		SPI.setMOSI(imuMosiPin);
+		SPI.setMISO(imuMisoPin);
+		SPI.begin();
+#endif
 		imu = IMU::create(imuModel, SPI, imuCsPin, imuIntPin);
 	} else {
 		// I2C connection
+#if defined(ESP32)
 		Wire.setPins(imuSdaPin, imuSclPin);
+#elif defined(ARDUINO_ARCH_STM32)
+		Wire.setSDA(imuSdaPin);
+		Wire.setSCL(imuSclPin);
+#endif
 		imu = IMU::create(imuModel, Wire, imuIntPin);
 	}
 

flix/log.ino

@@ -1,272 +1,77 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Logging subsystem
+// In-RAM logging
 
 #include "vector.h"
 #include "util.h"
 
-int logMemory = 0; // 0 - RAM, 1 - PSRAM, -1 - disabled
-float logUsage = 0.5; // fraction of free memory to use for log
+#define LOG_RATE 100
+#define LOG_DURATION 10
+#define LOG_SIZE LOG_DURATION * LOG_RATE
 
-struct LogValue {
+Vector attitudeEuler;
+Vector attitudeTargetEuler;
+
+struct LogEntry {
 	const char *name;
-	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) {};
+	float *value;
 };
 
-struct LogTopic {
-	LogValue values[10];
-	int length = 0; // number of logged values
-	float throttle; // max update rate, Hz
-	float lastUpdate = -INFINITY;
-
-	LogTopic(float throttle, LogValue v0, LogValue v1 = {}, LogValue v2 = {}, LogValue v3 = {}, LogValue v4 = {}, LogValue v5 = {}, LogValue v6 = {}, LogValue v7 = {}, LogValue v8 = {}, LogValue v9 = {}) :
-		throttle(throttle), values{v0, v1, v2, v3, v4, v5, v6, v7, v8, v9} {
-			// Count logged values
-			for (auto& v : values) {
-				if (v.name == nullptr) break;
-				if (v.logged) length++;
-			}
-		};
-
-	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) {};
+LogEntry logEntries[] = {
+	{"t", &t},
+	{"rates.x", &rates.x},
+	{"rates.y", &rates.y},
+	{"rates.z", &rates.z},
+	{"ratesTarget.x", &ratesTarget.x},
+	{"ratesTarget.y", &ratesTarget.y},
+	{"ratesTarget.z", &ratesTarget.z},
+	{"attitude.x", &attitudeEuler.x},
+	{"attitude.y", &attitudeEuler.y},
+	{"attitude.z", &attitudeEuler.z},
+	{"attitudeTarget.x", &attitudeTargetEuler.x},
+	{"attitudeTarget.y", &attitudeTargetEuler.y},
+	{"attitudeTarget.z", &attitudeTargetEuler.z},
+	{"thrustTarget", &thrustTarget}
 };
 
-LogTopic logTopics[] = {
-	// time
-	LogTopic({"t", &t}), // must be the first topic
-	LogTopic(1, {"loopRate", &loopRate}),
+const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
+float logBuffer[LOG_SIZE][logColumns];
 
-	// imu
-	LogTopic(
-		{"gyro.x", &gyro.x},
-		{"gyro.y", &gyro.y},
-		{"gyro.z", &gyro.z}),
-
-	LogTopic(50,
-		{"acc.x", &acc.x},
-		{"acc.y", &acc.y},
-		{"acc.z", &acc.z}),
-
-	LogTopic(10,
-		{"gyroBias.x", &gyroBias.x},
-		{"gyroBias.y", &gyroBias.y},
-		{"gyroBias.z", &gyroBias.z}),
-
-	// estimation
-	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(); }}),
-
-	// 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 prepareLogData() {
+	attitudeEuler = attitude.toEuler();
+	attitudeTargetEuler = attitudeTarget.toEuler();
 }
 
-void loopLog() {
-	if (logBuffer == nullptr || !armed) return;
+void logData() {
+	if (!armed) return;
+	static int logPointer = 0;
+	static Rate period(LOG_RATE);
+	if (!period) return;
 
-	if (!logLength) resetLog(); // reset state on first log write
+	prepareLogData();
 
-	static Rate sync(2);
-	if (sync) {
-		const uint8_t marker[] = {0x1A, 0x91, 0x4F, 0xF6, 0x7F};
-		writeLog(&marker, sizeof(marker)); // write sync marker
+	for (int i = 0; i < logColumns; i++) {
+		logBuffer[logPointer][i] = *logEntries[i].value;
 	}
 
-	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
-		}
+	logPointer++;
+	if (logPointer >= LOG_SIZE) {
+		logPointer = 0;
 	}
 }
 
-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)");
-		}
+	for (int i = 0; i < logColumns; i++) {
+		print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n");
 	}
 }
 
-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 printLogData() {
+	for (int i = 0; i < LOG_SIZE; i++) {
+		if (logBuffer[i][0] == 0) continue; // skip empty records
+		for (int j = 0; j < logColumns; j++) {
+			print("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n");
 		}
 	}
 }
-
-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/mavlink.ino

@@ -16,7 +16,6 @@ Rate telemetryAttitude(20);
 Rate telemetryRC(10);
 Rate telemetryMotors(10);
 Rate telemetryIMU(15);
-Rate telemetryTopic(10);
 
 float mavlinkTime = NAN; // time of last received message
 String mavlinkPrintBuffer;
@@ -85,12 +84,6 @@ 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) {
@@ -99,26 +92,6 @@ 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);
@@ -223,24 +196,18 @@ void handleMavlink(const void *_msg) {
 		mavlink_msg_set_attitude_target_decode(&msg, &m);
 		if (m.target_system && m.target_system != mavlinkSysId) return;
 
-		if (!(m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE)) {
-			// Attitude control
-			attitudeTarget.w = m.q[0];
-			attitudeTarget.x = m.q[1];
-			attitudeTarget.y = -m.q[2]; // convert to flu
-			attitudeTarget.z = -m.q[3];
-			ratesExtra.x = m.body_roll_rate;
-			ratesExtra.y = -m.body_pitch_rate;
-			ratesExtra.z = -m.body_yaw_rate;
-		} else {
-			// Rates control
-			attitudeTarget.invalidate();
-			ratesTarget.x = m.body_roll_rate;
-			ratesTarget.y = -m.body_pitch_rate;
-			ratesTarget.z = -m.body_yaw_rate;
-		}
+		// copy attitude, rates and thrust targets
+		ratesTarget.x = m.body_roll_rate;
+		ratesTarget.y = -m.body_pitch_rate; // convert to flu
+		ratesTarget.z = -m.body_yaw_rate;
+		attitudeTarget.w = m.q[0];
+		attitudeTarget.x = m.q[1];
+		attitudeTarget.y = -m.q[2];
+		attitudeTarget.z = -m.q[3];
+		thrustTarget = m.thrust;
+		ratesExtra = Vector(0, 0, 0);
 
-		thrustTarget = valid(m.thrust) ? m.thrust : thrustTarget;
+		if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
 		armed = m.thrust > 0;
 	}
 
@@ -258,29 +225,18 @@ 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;
 
-		for (int i = 0; i < m.count; i += MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
-			int chunkSize = min(MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN, (int)(m.count - i));
-			mavlink_message_t response;
-			uint8_t data[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN];
-			readLog(data, m.ofs + i, chunkSize);
-			mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
-				m.id, m.ofs + i, chunkSize, data);
-			batchMessage(&response);
+		// Send all log records
+		for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
+			mavlink_message_t msg;
+			mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
+				sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
+			sendMessage(&msg);
 		}
-		flushBatchMessages();
 	}
 
 	// Handle commands
@@ -335,5 +291,5 @@ void sendMavlinkPrint() {
 			0, 0, strlen(data), (uint8_t *)data, 0, 0);
 		sendMessage(&msg);
 	}
-	mavlinkPrintBuffer.clear();
+	mavlinkPrintBuffer = "";
 }

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,18 +19,27 @@ 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,7 +3,7 @@
 
 // Parameters storage in flash memory
 
-#include <Preferences.h>
+#include "prefs.h"
 #include "util.h"
 
 extern int channelZero[16], channelMax[16];
@@ -20,11 +20,15 @@ Preferences storage;
 
 struct Parameter {
 	const char *name; // max length is 15
-	Value value; // pointer to the variable
+	bool integer;
+	union { float *f; int *i; }; // pointer to the variable
 	float initial; // default value
 	float cache; // what's stored in flash
 	void (*callback)(); // called after parameter change
-	Parameter(const char *name, Value value, void (*callback)() = nullptr) : name(name), value(value), callback(callback) {};
+	Parameter(const char *name, float *variable, void (*callback)() = nullptr) : name(name), integer(false), f(variable), callback(callback) {};
+	Parameter(const char *name, int *variable, void (*callback)() = nullptr) : name(name), integer(true), i(variable), callback(callback) {};
+	float getValue() const { return integer ? *i : *f; };
+	void setValue(const float value) { if (integer) *i = value; else *f = value; };
 };
 
 Parameter parameters[] = {
@@ -82,8 +86,6 @@ 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},
@@ -132,22 +134,6 @@ 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},
@@ -164,11 +150,11 @@ void setupParameters() {
 	storage.begin("flix");
 	// Read parameters from storage
 	for (auto &parameter : parameters) {
-		parameter.initial = parameter.value.get();
+		parameter.initial = parameter.getValue();
 		if (storage.isKey(parameter.name)) {
-			parameter.value.set(storage.getFloat(parameter.name));
+			parameter.setValue(storage.getFloat(parameter.name));
 		}
-		parameter.cache = parameter.value.get();
+		parameter.cache = parameter.getValue();
 	}
 }
 
@@ -183,13 +169,13 @@ const char *getParameterName(int index) {
 
 float getParameter(int index) {
 	if (index < 0 || index >= parametersCount()) return NAN;
-	return parameters[index].value.get();
+	return parameters[index].getValue();
 }
 
 float getParameter(const char *name) {
 	for (auto &parameter : parameters) {
 		if (strcasecmp(parameter.name, name) == 0) {
-			return parameter.value.get();
+			return parameter.getValue();
 		}
 	}
 	return NAN;
@@ -198,9 +184,10 @@ float getParameter(const char *name) {
 bool setParameter(const char *name, const float value) {
 	for (auto &parameter : parameters) {
 		if (strcasecmp(parameter.name, name) == 0) {
-			bool success = parameter.value.set(value);
+			if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
+			parameter.setValue(value);
 			if (parameter.callback) parameter.callback();
-			return success;
+			return true;
 		}
 	}
 	return false;
@@ -212,10 +199,10 @@ void syncParameters() {
 	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
 
 	for (auto &parameter : parameters) {
-		if (floatEquals(parameter.value.get(), parameter.cache)) continue; // no change
+		if (floatEquals(parameter.getValue(), parameter.cache)) continue; // no change
 
-		storage.putFloat(parameter.name, parameter.value.get());
-		parameter.cache = parameter.value.get(); // update cache
+		storage.putFloat(parameter.name, parameter.getValue());
+		parameter.cache = parameter.getValue(); // update cache
 	}
 }
 
@@ -224,15 +211,15 @@ void printParameters(const char *filter) {
 	for (auto &parameter : parameters) {
 		if (strncasecmp(parameter.name, filter, strlen(filter))) continue;
 
-		if (floatEquals(parameter.value.get(), parameter.initial)) { // parameter changed
-			print("%-15s  %-13g\n", parameter.name, parameter.value.get());
+		if (floatEquals(parameter.getValue(), parameter.initial)) { // parameter changed
+			print("%-15s  %-13g\n", parameter.name, parameter.getValue());
 		} else {
-			print("%-15s  %-13g  [%g]\n", parameter.name, parameter.value.get(), parameter.initial);
+			print("%-15s  %-13g  [%g]\n", parameter.name, parameter.getValue(), parameter.initial);
 		}
 	}
 }
 
 void resetParameters() {
 	storage.clear();
-	ESP.restart();
+	reboot();
 }

flix/power.ino

@@ -3,8 +3,10 @@
 
 // Power management
 
+#ifdef ESP32
 #include <soc/soc.h>
 #include <soc/rtc_cntl_reg.h>
+#endif
 #include "filter.h"
 #include "util.h"
 
@@ -14,8 +16,10 @@ 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
 	if (digitalPinToAnalogChannel(voltagePin) == -1) voltagePin = -1; // test ADC pin
+#endif
 }
 
 void readVoltage() {
@@ -24,6 +28,12 @@ void readVoltage() {
 	static Rate rate(10);
 	if (!rate) return;
 
-	float v = analogReadMilliVolts(voltagePin) * voltageScale / 1000.0f;
+	float v = 0;
+#if defined(ESP32)
+	v = analogReadMilliVolts(voltagePin) * voltageScale / 1000.0f;
+#elif defined(ARDUINO_ARCH_STM32)
+	v = analogRead(voltagePin) * voltageScale * 3.3f / 4095.0f;
+#endif
+
 	voltage = voltageFilter.update(v);
 }

flix/prefs.h

@@ -0,0 +1,280 @@
+#ifndef ARDUINO_ARCH_STM32
+#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 = nullptr) {
+		(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() {
+		return STORAGE_SIZE;
+	}
+
+	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]);
+		}
+		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,13 +6,15 @@
 #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;
 
-#define STRINGIFY(x) __STRINGIFY(x)
-
 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;
 }
@@ -55,6 +57,7 @@ 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:
@@ -65,50 +68,15 @@ 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;
-		}
-	};
-
-	bool set(float value) const {
-		switch (type) {
-			case FLOAT: *_float = value; break;
-			case INT: if (!isfinite(value)) return false; *_int = value; break;
-			case BOOL: *_bool = (value != 0); break;
-			default: return false;
-		}
-		return true;
-	}
-};
+void reboot() {
+#if defined(ESP32)
+	ESP.restart();
+#elif defined(ARDUINO_ARCH_STM32)
+	NVIC_SystemReset();
+#endif
+}
 
 // Rate limiter
 class Rate {

flix/wifi.ino

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

gazebo/Arduino.h

@@ -20,10 +20,6 @@
 #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; }
@@ -160,8 +156,6 @@ 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;
@@ -171,16 +165,14 @@ 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; }
+void analogWrite(uint8_t pin, int value) {}
+void analogWriteResolution(uint8_t res) {}
+void analogWriteFrequency(uint32_t freq) {}
 
 unsigned long __micros;
 unsigned long __resetTime = 0;

gazebo/flix.h

@@ -51,17 +51,8 @@ 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 printLogValues(const char *filter);
-void configLogThrottle(const char *name, float throttle);
-void exposeLogValue(const char *name);
+void printLogData();
 void processMavlink();
 void sendMavlink();
 void sendMessage(const void *msg);
@@ -92,8 +83,3 @@ 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

@@ -55,7 +55,6 @@ public:
 		initNode();
 		Serial.begin(0);
 		setupParameters();
-		setupLog();
 		rcRxPin = 1; // set rc pin to enable rc reading
 		gzmsg << "Flix plugin loaded" << endl;
 	}
@@ -89,7 +88,7 @@ public:
 
 		applyMotorForces();
 		publishTopics();
-		loopLog();
+		logData();
 		syncParameters();
 	}
 

tools/csv_to_mcap.py

@@ -9,7 +9,6 @@ Usage:
 import csv
 import json
 import docopt
-import math
 from mcap.writer import Writer
 
 args = docopt.docopt(__doc__)
@@ -40,15 +39,7 @@ channel_id = writer.register_channel(
 )
 
 for row in csv_reader:
-    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)}
+    data = {key: float(value) for key, value in zip(header, row)}
     timestamp = round(float(row[0]) * 1e9)
     writer.add_message(channel_id=channel_id, log_time=timestamp, data=json.dumps(data).encode(), publish_time=timestamp,)
 

tools/log_to_csv.py

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

tools/pyflix/flix.py

@@ -326,7 +326,7 @@ class Flix:
     def set_velocity(self, velocity: Sequence[float], yaw: Optional[float] = None):
         raise NotImplementedError('Velocity control is not implemented yet')
 
-    def set_attitude(self, attitude: Sequence[float], thrust: float, rates_extra: Sequence[float] = (0, 0, 0)):
+    def set_attitude(self, attitude: Sequence[float], thrust: float):
         if len(attitude) == 3:
             attitude = Quaternion([attitude[0], attitude[1], attitude[2]]).q  # type: ignore
         elif len(attitude) != 4:
@@ -334,11 +334,10 @@ class Flix:
         if not (0 <= thrust <= 1):
             raise ValueError('Thrust must be in range [0, 1]')
         attitude = self._flu_to_mavlink(attitude)
-        rates_extra = self._flu_to_mavlink(rates_extra)
         for _ in range(2): # duplicate to ensure delivery
             self.mavlink.set_attitude_target_send(0, self.system_id, 0, 0,
                 [attitude[0], attitude[1], attitude[2], attitude[3]],
-                rates_extra[0], rates_extra[1], rates_extra[2], thrust)
+                0, 0, 0, thrust)
 
     def set_rates(self, rates: Sequence[float], thrust: float):
         if len(rates) != 3: