Use dev version of FlixPeriph

.github/workflows/build.yml

@@ -28,6 +28,12 @@ jobs:
       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: 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

@@ -12,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
 
@@ -22,7 +25,7 @@ core .core:
 
 libs .libs:
 	arduino-cli lib update-index
-	arduino-cli lib install "FlixPeriph"
+	ARDUINO_LIBRARY_ENABLE_UNSAFE_INSTALL=1 arduino-cli lib install --git-url 'https://github.com/okalachev/flixperiph.git#dev'
 	arduino-cli lib install "MAVLink"@2.0.25
 	touch .libs
 

flix/cli.ino

@@ -193,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());
 	}
@@ -212,7 +212,7 @@ void handleInput() {
 		char c = Serial.read();
 		if (c == '\n' || c == '\r') {
 			doCommand(input);
-			input.clear();
+			input = "";
 		} else {
 			input += c;
 		}

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/mavlink.ino

@@ -291,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];
@@ -221,5 +221,5 @@ void printParameters(const char *filter) {
 
 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,7 +6,11 @@
 #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;
@@ -53,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:
@@ -63,6 +68,15 @@ public:
 		ESP_NOW_Serial_Class::onSent(true); // always report success to avoid resends
 	}
 };
+#endif
+
+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

@@ -170,6 +170,9 @@ 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; }
+void analogWrite(uint8_t pin, int value) {}
+void analogWriteResolution(uint8_t res) {}
+void analogWriteFrequency(uint32_t freq) {}
 
 unsigned long __micros;
 unsigned long __resetTime = 0;