add Serial Buffer to limit the data written in 1 cycle

Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 6 to 7.
- [Release notes](https://github.com/actions/upload-artifact/releases)
- [Commits](https://github.com/actions/upload-artifact/compare/v6...v7)

---
updated-dependencies:
- dependency-name: actions/upload-artifact
  dependency-version: '7'
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

.github/workflows/actions.yml

@@ -14,7 +14,7 @@ jobs:
   format:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v5
+      - uses: actions/checkout@v6
       - uses: jidicula/clang-format-action@v4.14.0
         with:
           clang-format-version: "17"
@@ -33,8 +33,8 @@ jobs:
     runs-on: ubuntu-latest
 
     steps:
-      - uses: actions/checkout@v5
-      - uses: actions/cache@v4
+      - uses: actions/checkout@v6
+      - uses: actions/cache@v5
         with:
           path: |
             ~/.cache/pip
@@ -58,7 +58,7 @@ jobs:
         run: python ./ci/build.py
 
       - name: Upload binaries
-        uses: actions/upload-artifact@v5
+        uses: actions/upload-artifact@v7
         with:
           name: binaries
           path: ./build/*.bin

board-defaults.json

@@ -307,7 +307,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false
@@ -346,7 +346,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false
@@ -385,7 +385,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false
@@ -424,7 +424,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false
@@ -463,7 +463,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false
@@ -502,7 +502,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false
@@ -541,7 +541,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false
@@ -580,7 +580,7 @@
 				}
 			},
 			"flashingRules": {
-				"applicationOffset": 0,
+				"applicationOffset": 65536,
 				"needBootPress": false,
 				"needManualReboot": false,
 				"shouldOnlyUseDefaults": false

board-defaults.schema.json

@@ -27,6 +27,7 @@
 				"IMU_BMI160",
 				"IMU_ICM20948",
 				"IMU_ICM42688",
+				"IMU_BMI270",
 				"IMU_LSM6DS3TRC",
 				"IMU_LSM6DSV",
 				"IMU_LSM6DSO",

platformio.ini

@@ -72,21 +72,25 @@ build_unflags = -Os -std=gnu++11 -std=gnu++17
 platform = espressif8266 @ 4.2.1
 board = esp12e
 custom_slime_board = BOARD_WEMOSD1MINI
+upload_speed = 921600
 
 [env:BOARD_NODEMCU]
 platform = espressif8266 @ 4.2.1
 board = esp12e
 custom_slime_board = BOARD_NODEMCU
+upload_speed = 921600
 
 [env:BOARD_TTGO_TBASE]
 platform = espressif8266 @ 4.2.1
 board = esp12e
 custom_slime_board = BOARD_TTGO_TBASE
+upload_speed = 921600
 
 [env:BOARD_WEMOSWROOM02]
 platform = espressif8266 @ 4.2.1
 board = esp12e
 custom_slime_board = BOARD_WEMOSWROOM02
+upload_speed = 921600
 
 [env:BOARD_WROOM32]
 platform = espressif32 @ 6.7.0
@@ -101,6 +105,7 @@ platform = espressif8266 @ 4.2.1
 board = esp01_1m
 board_build.arduino.ldscript = "eagle.flash.1m64.ld"
 custom_slime_board = BOARD_ESP01
+upload_speed = 921600
 
 [env:BOARD_LOLIN_C3_MINI]
 platform = espressif32 @ 6.7.0

src/configuration/Configuration.cpp

@@ -25,13 +25,18 @@
 
 #include <LittleFS.h>
 
+#include <cstdint>
+#include <cstring>
+
 #include "../FSHelper.h"
 #include "consts.h"
+#include "sensors/SensorToggles.h"
 #include "utils.h"
 
 #define DIR_CALIBRATIONS "/calibrations"
 #define DIR_TEMPERATURE_CALIBRATIONS "/tempcalibrations"
-#define DIR_TOGGLES "/toggles"
+#define DIR_TOGGLES_OLD "/toggles"
+#define DIR_TOGGLES "/sensortoggles"
 
 namespace SlimeVR::Configuration {
 void Configuration::setup() {
@@ -118,13 +123,21 @@ void Configuration::save() {
 		file.write((uint8_t*)&config, sizeof(SensorConfig));
 		file.close();
 
-		sprintf(path, DIR_TOGGLES "/%zu", i);
+		if (i < m_SensorToggles.size()) {
+			sprintf(path, DIR_TOGGLES "/%zu", i);
 
-		m_Logger.trace("Saving sensor toggle state for %d", i);
+			m_Logger.trace("Saving sensor toggle state for %d", i);
 
-		file = LittleFS.open(path, "w");
-		file.write((uint8_t*)&m_SensorToggles[i], sizeof(SensorToggleState));
-		file.close();
+			file = LittleFS.open(path, "w");
+			auto toggleValues = m_SensorToggles[i].getValues();
+			file.write((uint8_t*)&toggleValues, sizeof(SensorToggleValues));
+			file.close();
+		} else {
+			m_Logger.trace(
+				"Skipping saving toggles for sensor %d, no toggles present",
+				i
+			);
+		}
 	}
 
 	{
@@ -133,6 +146,18 @@ void Configuration::save() {
 		file.close();
 	}
 
+	// Clean up old toggles directory
+	if (LittleFS.exists(DIR_TOGGLES_OLD)) {
+		char path[17] = DIR_TOGGLES_OLD;
+		char* end = path + strlen(DIR_TOGGLES_OLD);
+		Utils::forEachFile(DIR_TOGGLES_OLD, [&](SlimeVR::Utils::File file) {
+			sprintf(end, "/%s", file.name());
+			LittleFS.remove(path);
+			file.close();
+		});
+		LittleFS.rmdir(DIR_TOGGLES_OLD);
+	}
+
 	m_Logger.debug("Saved configuration");
 }
 
@@ -226,14 +251,34 @@ void Configuration::loadSensors() {
 		setSensor(sensorId, sensorConfig);
 	});
 
+	if (LittleFS.exists(DIR_TOGGLES_OLD)) {
+		SlimeVR::Utils::forEachFile(DIR_TOGGLES_OLD, [&](SlimeVR::Utils::File f) {
+			SensorToggleValues values;
+			// Migration for pre 0.7.0 togglestate, the values started at offset 20 and
+			// there were 3 of them
+			f.seek(20);
+			f.read(reinterpret_cast<uint8_t*>(&values), 3);
+
+			uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+			m_Logger.debug("Found sensor toggle state at index %d", sensorId);
+
+			setSensorToggles(sensorId, SensorToggleState{values});
+		});
+	}
+
 	SlimeVR::Utils::forEachFile(DIR_TOGGLES, [&](SlimeVR::Utils::File f) {
-		SensorToggleState sensorToggleState;
-		f.read((uint8_t*)&sensorToggleState, sizeof(SensorToggleState));
+		if (f.size() > sizeof(SensorToggleValues)) {
+			return;
+		}
+		SensorToggleValues values;
+		// With the magic of C++ default initialization, the rest of the values should
+		// be their default after reading
+		f.read(reinterpret_cast<uint8_t*>(&values), f.size());
 
 		uint8_t sensorId = strtoul(f.name(), nullptr, 10);
 		m_Logger.debug("Found sensor toggle state at index %d", sensorId);
 
-		setSensorToggles(sensorId, sensorToggleState);
+		setSensorToggles(sensorId, SensorToggleState{values});
 	});
 }
 

src/debug.h

@@ -96,7 +96,7 @@
 // Not recommended for production
 #define ENABLE_INSPECTION false
 
-#define PROTOCOL_VERSION 21
+#define PROTOCOL_VERSION 22
 
 #ifndef FIRMWARE_VERSION
 #define FIRMWARE_VERSION "UNKNOWN"

src/logging/LogBuffer.cpp

@@ -0,0 +1,110 @@
+#include "LogBuffer.h"
+
+namespace SlimeVR::Logging {
+
+bool LogBuffer::addLog(const char* message) {
+	size_t len = strlen(message);
+	
+	// Need space for message + null terminator + length prefix (2 bytes)
+	size_t required = len + 3;
+	
+	if (required > getAvailableSpace()) {
+		// Buffer full, drop the message or overwrite oldest
+		return false;
+	}
+
+	// Write length as 2-byte prefix (little endian)
+	m_Buffer[m_WritePos] = (len >> 8) & 0xFF;
+	m_WritePos = (m_WritePos + 1) % MAX_BUFFER_SIZE;
+	m_Buffer[m_WritePos] = len & 0xFF;
+	m_WritePos = (m_WritePos + 1) % MAX_BUFFER_SIZE;
+
+	// Write message
+	for (size_t i = 0; i < len; i++) {
+		m_Buffer[m_WritePos] = message[i];
+		m_WritePos = (m_WritePos + 1) % MAX_BUFFER_SIZE;
+	}
+
+	// Write null terminator
+	m_Buffer[m_WritePos] = '\0';
+	m_WritePos = (m_WritePos + 1) % MAX_BUFFER_SIZE;
+
+	return true;
+}
+
+void LogBuffer::processCycle() {
+	size_t bytesWritten = 0;
+
+	while (m_WritePos != m_ReadPos && bytesWritten < MAX_BYTES_PER_CYCLE) {
+		size_t msgLen = getNextMessageLength();
+		
+		if (msgLen == 0 || bytesWritten + msgLen > MAX_BYTES_PER_CYCLE) {
+			// Message too large for this cycle or corrupted
+			break;
+		}
+
+		// Skip length prefix
+		m_ReadPos = (m_ReadPos + 2) % MAX_BUFFER_SIZE;
+
+		// Read and print message
+		for (size_t i = 0; i < msgLen; i++) {
+			Serial.write(m_Buffer[m_ReadPos]);
+			m_ReadPos = (m_ReadPos + 1) % MAX_BUFFER_SIZE;
+		}
+
+		// Skip null terminator
+		m_ReadPos = (m_ReadPos + 1) % MAX_BUFFER_SIZE;
+
+		bytesWritten += msgLen;
+	}
+}
+
+size_t LogBuffer::getPendingBytes() const {
+	if (m_WritePos >= m_ReadPos) {
+		return m_WritePos - m_ReadPos;
+	} else {
+		return MAX_BUFFER_SIZE - m_ReadPos + m_WritePos;
+	}
+}
+
+void LogBuffer::flushAll() {
+	while (m_WritePos != m_ReadPos) {
+		size_t msgLen = getNextMessageLength();
+		
+		if (msgLen == 0) {
+			break;  // Corrupted buffer
+		}
+
+		// Skip length prefix
+		m_ReadPos = (m_ReadPos + 2) % MAX_BUFFER_SIZE;
+
+		// Read and print message
+		for (size_t i = 0; i < msgLen; i++) {
+			Serial.write(m_Buffer[m_ReadPos]);
+			m_ReadPos = (m_ReadPos + 1) % MAX_BUFFER_SIZE;
+		}
+
+		// Skip null terminator
+		m_ReadPos = (m_ReadPos + 1) % MAX_BUFFER_SIZE;
+	}
+}
+
+size_t LogBuffer::getAvailableSpace() const {
+	size_t used = getPendingBytes();
+	return MAX_BUFFER_SIZE - used - 1;  // -1 to distinguish full from empty
+}
+
+size_t LogBuffer::getNextMessageLength() const {
+	if (m_WritePos == m_ReadPos) {
+		return 0;
+	}
+
+	size_t pos = m_ReadPos;
+	uint8_t highByte = m_Buffer[pos];
+	pos = (pos + 1) % MAX_BUFFER_SIZE;
+	uint8_t lowByte = m_Buffer[pos];
+
+	return (highByte << 8) | lowByte;
+}
+
+}  // namespace SlimeVR::Logging

src/logging/LogBuffer.h

@@ -0,0 +1,46 @@
+#ifndef LOGGING_LOGBUFFER_H
+#define LOGGING_LOGBUFFER_H
+
+#include <Arduino.h>
+
+namespace SlimeVR::Logging {
+
+class LogBuffer {
+public:
+	static constexpr size_t MAX_BUFFER_SIZE = 4096;  // Total buffer size
+	static constexpr size_t MAX_BYTES_PER_CYCLE = 512;  // Max bytes to output per cycle
+
+	static LogBuffer& getInstance() {
+		static LogBuffer instance;
+		return instance;
+	}
+
+	// Add a log message to the buffer
+	bool addLog(const char* message);
+
+	// Process buffered logs up to MAX_BYTES_PER_CYCLE
+	void processCycle();
+
+	// Check if there are pending logs
+	bool hasPendingLogs() const { return m_WritePos != m_ReadPos; }
+
+	// Get number of bytes pending
+	size_t getPendingBytes() const;
+
+	// Force flush all pending logs (for critical errors or shutdown)
+	void flushAll();
+
+private:
+	LogBuffer() : m_Buffer{0}, m_WritePos(0), m_ReadPos(0) {}
+
+	char m_Buffer[MAX_BUFFER_SIZE];
+	volatile size_t m_WritePos;
+	volatile size_t m_ReadPos;
+
+	size_t getAvailableSpace() const;
+	size_t getNextMessageLength() const;
+};
+
+}  // namespace SlimeVR::Logging
+
+#endif

src/logging/Logger.cpp

@@ -63,6 +63,23 @@ void Logger::log(Level level, const char* format, va_list args) const {
 		strcat(buf, m_Tag);
 	}
 
-	Serial.printf("[%-5s] [%s] %s\n", levelToString(level), buf, buffer);
+	// Format complete log message
+	char fullMessage[384];  // Enough for header + message
+	snprintf(fullMessage, sizeof(fullMessage), "[%-5s] [%s] %s\n", levelToString(level), buf, buffer);
+
+	// Add to buffer or write directly for critical messages
+	if (level >= ERROR) {
+		// Critical messages: write immediately and flush buffer
+		LogBuffer::getInstance().flushAll();
+		Serial.print(fullMessage);
+	} else {
+		// Normal messages: add to buffer
+		if (!LogBuffer::getInstance().addLog(fullMessage)) {
+			// Buffer full, try to make space
+			LogBuffer::getInstance().processCycle();
+			// Try again
+			LogBuffer::getInstance().addLog(fullMessage);
+		}
+	}
 }
 }  // namespace SlimeVR::Logging

src/logging/Logger.h

@@ -4,6 +4,7 @@
 #include <Arduino.h>
 
 #include "Level.h"
+#include "LogBuffer.h"
 #include "debug.h"
 
 namespace SlimeVR::Logging {
@@ -79,12 +80,20 @@ private:
 			strcat(buf, m_Tag);
 		}
 
-		Serial.printf("[%-5s] [%s] %s", levelToString(level), buf, str);
+		// Build array log message
+		char header[256];
+		snprintf(header, sizeof(header), "[%-5s] [%s] %s", levelToString(level), buf, str);
 
+		// For arrays, we'll output directly to avoid excessive buffering
+		// But for critical levels, flush first
+		if (level >= ERROR) {
+			LogBuffer::getInstance().flushAll();
+		}
+
+		Serial.print(header);
 		for (size_t i = 0; i < size; i++) {
 			Serial.print(array[i]);
 		}
-
 		Serial.println();
 	}
 

src/main.cpp

@@ -159,6 +159,9 @@ void loop() {
 	OTA::otaUpdate();
 	networkManager.update();
 
+	// Process buffered logs
+	SlimeVR::Logging::LogBuffer::getInstance().processCycle();
+
 #if DEBUG_MEASURE_SENSOR_TIME_TAKEN
 	sensorMeasurer.before();
 #endif

src/network/connection.cpp

@@ -620,6 +620,9 @@ void Connection::reset() {
 
 	m_UDP.begin(m_ServerPort);
 
+	// Reset server address to broadcast if disconnected
+	m_ServerHost = IPAddress(255, 255, 255, 255);
+
 	statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
 }
 
@@ -650,6 +653,9 @@ void Connection::update() {
 		);
 		m_Logger.warn("Connection to server timed out");
 
+		// Reset server address to broadcast if disconnected
+		m_ServerHost = IPAddress(255, 255, 255, 255);
+
 		return;
 	}
 
@@ -658,7 +664,6 @@ void Connection::update() {
 		return;
 	}
 
-	m_LastPacketTimestamp = millis();
 	int len = m_UDP.read(m_Packet, sizeof(m_Packet));
 
 #ifdef DEBUG_NETWORK
@@ -673,6 +678,12 @@ void Connection::update() {
 	(void)packetSize;
 #endif
 
+	if (static_cast<ReceivePacketType>(m_Packet[3]) == ReceivePacketType::Handshake) {
+		m_Logger.warn("Handshake received again, ignoring");
+		return;
+	}
+
+	m_LastPacketTimestamp = millis();
 	switch (static_cast<ReceivePacketType>(m_Packet[3])) {
 		case ReceivePacketType::HeartBeat:
 			sendHeartbeat();
@@ -682,8 +693,7 @@ void Connection::update() {
 			break;
 
 		case ReceivePacketType::Handshake:
-			// Assume handshake successful
-			m_Logger.warn("Handshake received again, ignoring");
+			// handled above
 			break;
 
 		case ReceivePacketType::Command:

src/sensors/SensorToggles.cpp

@@ -1,15 +1,18 @@
 #include "SensorToggles.h"
 
+SensorToggleState::SensorToggleState(SensorToggleValues values)
+	: values{values} {}
+
 void SensorToggleState::setToggle(SensorToggles toggle, bool state) {
 	switch (toggle) {
 		case SensorToggles::MagEnabled:
-			magEnabled = state;
+			values.magEnabled = state;
 			break;
 		case SensorToggles::CalibrationEnabled:
-			calibrationEnabled = state;
+			values.calibrationEnabled = state;
 			break;
 		case SensorToggles::TempGradientCalibrationEnabled:
-			tempGradientCalibrationEnabled = state;
+			values.tempGradientCalibrationEnabled = state;
 			break;
 	}
 }
@@ -17,11 +20,11 @@ void SensorToggleState::setToggle(SensorToggles toggle, bool state) {
 bool SensorToggleState::getToggle(SensorToggles toggle) const {
 	switch (toggle) {
 		case SensorToggles::MagEnabled:
-			return magEnabled;
+			return values.magEnabled;
 		case SensorToggles::CalibrationEnabled:
-			return calibrationEnabled;
+			return values.calibrationEnabled;
 		case SensorToggles::TempGradientCalibrationEnabled:
-			return tempGradientCalibrationEnabled;
+			return values.tempGradientCalibrationEnabled;
 	}
 	return false;
 }
@@ -32,6 +35,8 @@ void SensorToggleState::onToggleChange(
 	this->callback = callback;
 }
 
+SensorToggleValues SensorToggleState::getValues() const { return values; }
+
 void SensorToggleState::emitToggleChange(SensorToggles toggle, bool state) const {
 	if (callback) {
 		(*callback)(toggle, state);

src/sensors/SensorToggles.h

@@ -35,8 +35,17 @@ enum class SensorToggles : uint16_t {
 	TempGradientCalibrationEnabled = 3,
 };
 
+struct SensorToggleValues {
+	bool magEnabled = !USE_6_AXIS;
+	bool calibrationEnabled = true;
+	bool tempGradientCalibrationEnabled
+		= false;  // disable by default, it is not clear that it really helps
+};
+
 class SensorToggleState {
 public:
+	SensorToggleState() = default;
+	explicit SensorToggleState(SensorToggleValues values);
 	void setToggle(SensorToggles toggle, bool state);
 	[[nodiscard]] bool getToggle(SensorToggles toggle) const;
 
@@ -44,12 +53,12 @@ public:
 
 	static const char* toggleToString(SensorToggles toggle);
 
+	[[nodiscard]] SensorToggleValues getValues() const;
+
 private:
 	std::optional<std::function<void(SensorToggles, bool)>> callback;
+
 	void emitToggleChange(SensorToggles toggle, bool state) const;
 
-	bool magEnabled = !USE_6_AXIS;
-	bool calibrationEnabled = true;
-	bool tempGradientCalibrationEnabled
-		= false;  // disable by default, it is not clear that it really helps
+	SensorToggleValues values;
 };

src/sensors/sensor.cpp

@@ -33,6 +33,7 @@ SensorStatus Sensor::getSensorState() {
 
 void Sensor::setAcceleration(Vector3 a) {
 	acceleration = a;
+	sensorOffset.sandwich(acceleration);
 	newAcceleration = true;
 }
 

src/sensors/softfusion/CalibrationBase.h

@@ -87,6 +87,7 @@ public:
 
 	virtual const uint8_t* getMotionlessCalibrationData() = 0;
 
+	virtual void signalOverwhelmed() {}
 	virtual void provideAccelSample(const RawSensorT accelSample[3]) {}
 	virtual void provideGyroSample(const RawSensorT gyroSample[3]) {}
 	virtual void provideTempSample(float tempSample) {}

src/sensors/softfusion/drivers/bmi160.h

@@ -25,6 +25,7 @@
 
 #include <algorithm>
 #include <array>
+#include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <limits>
@@ -176,7 +177,7 @@ struct BMI160 {
 		return to_ret;
 	}
 
-	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoLength) & 0x7FF;
 
 		const auto bytes_to_read = std::min(
@@ -219,6 +220,7 @@ struct BMI160 {
 				}
 			}
 		}
+		return static_cast<size_t>(fifo_bytes) > static_cast<size_t>(bytes_to_read);
 	}
 };
 

src/sensors/softfusion/drivers/bmi270.h

@@ -428,7 +428,7 @@ struct BMI270 {
 		return to_ret;
 	}
 
-	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
 		const auto bytes_to_read = std::min(
@@ -482,6 +482,8 @@ struct BMI270 {
 				}
 			}
 		}
+
+		return fifo_bytes > bytes_to_read;
 	}
 };
 

src/sensors/softfusion/drivers/icm42688.h

@@ -30,6 +30,8 @@
 #include "callbacks.h"
 #include "vqf.h"
 
+constexpr static bool DEBUG_ICM42688_HIRES = false;
+
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
 // Driver uses acceleration range at 8g
@@ -44,16 +46,22 @@ struct ICM42688 {
 
 	static constexpr float GyrTs = 1.0 / 200.0;
 	static constexpr float AccTs = 1.0 / 100.0;
-	static constexpr float TempTs = 1.0 / 500.0;
+	static constexpr float TempTs = 1.0 / 200.0;
 
 	static constexpr float MagTs = 1.0 / 100;
 
-	static constexpr float GyroSensitivity = 32.8f;
-	static constexpr float AccelSensitivity = 4096.0f;
+	// When 20-bits data format is used, the only FSR settings that are
+	// operational are ±2000dps for gyroscope and ±16g for accelerometer, even if the
+	// FSR selection register settings are configured for other FSR values. The
+	// corresponding sensitivity scale factor values are 131 LSB/dps for gyroscope and
+	// 8192 LSB/g for accelerometer.
+	static constexpr float GyroSensitivity = (DEBUG_ICM42688_HIRES ? 131.0f : 32.8f);
+	static constexpr float AccelSensitivity
+		= (DEBUG_ICM42688_HIRES ? 8192.0f : 4096.0f);
 
 	static constexpr float TemperatureBias = 25.0f;
-	static constexpr float TemperatureSensitivity = 2.07f;
-
+	static constexpr float TemperatureSensitivity
+		= (DEBUG_ICM42688_HIRES ? 132.48f : 2.07f);
 	static constexpr float TemperatureZROChange = 20.0f;
 
 	static constexpr VQFParams SensorVQFParams{};
@@ -88,7 +96,9 @@ struct ICM42688 {
 			static constexpr uint8_t reg = 0x5f;
 			static constexpr uint8_t value
 				= 0b1 | (0b1 << 1) | (0b1 << 2)
-				| (0b0 << 4);  // fifo accel en=1, gyro=1, temp=1, hires=1
+				| (DEBUG_ICM42688_HIRES ? (0b1 << 4) : (0b0 << 4));
+			// fifo accel en=1, gyro=1, temp=1,
+			// hires=DEBUG_ICM42688_HIRES
 		};
 		struct GyroConfig {
 			static constexpr uint8_t reg = 0x4f;
@@ -115,7 +125,7 @@ struct ICM42688 {
 	};
 
 #pragma pack(push, 1)
-	struct FifoEntryAligned {
+	struct FifoEntryAlignedHires {
 		union {
 			struct {
 				int16_t accel[3];
@@ -128,10 +138,65 @@ struct ICM42688 {
 			} part;
 			uint8_t raw[19];
 		};
+
+		void getGyro(int32_t out[3]) {
+			// 6.1 Packet Structure for high resolution mode
+			// https://invensense.tdk.com/wp-content/uploads/2020/04/ds-000347_icm-42688-p-datasheet.pdf
+			// When 20-bits data format is used, gyroscope data consists of 19-bits
+			// of actual data and the LSB is always set to 0
+			out[0] = static_cast<int32_t>(part.gyro[0]) << 3 | ((part.xlsb & 0xe) >> 1);
+			out[1] = static_cast<int32_t>(part.gyro[1]) << 3 | ((part.ylsb & 0xe) >> 1);
+			out[2] = static_cast<int32_t>(part.gyro[2]) << 3 | ((part.zlsb & 0xe) >> 1);
+		}
+		void getAccel(int32_t out[3]) {
+			// accelerometer data consists of 18-bits of actual data and the two
+			// lowest order bits are always set to 0
+			out[0] = static_cast<int32_t>(part.accel[0]) << 2
+				   | (static_cast<int32_t>((part.xlsb) & 0xf0) >> 6);
+			out[1] = static_cast<int32_t>(part.accel[1]) << 2
+				   | (static_cast<int32_t>((part.ylsb) & 0xf0) >> 6);
+			out[2] = static_cast<int32_t>(part.accel[2]) << 2
+				   | (static_cast<int32_t>((part.zlsb) & 0xf0) >> 6);
+		}
+	};
+
+	struct FifoEntryAlignedDefault {
+		union {
+			struct {
+				int16_t accel[3];
+				int16_t gyro[3];
+				int8_t temp;
+				uint16_t timestamp;
+			} part;
+			uint8_t raw[15];
+		};
+
+		void getGyro(int32_t out[3]) {
+			out[0] = static_cast<int32_t>(part.gyro[0]);
+			out[1] = static_cast<int32_t>(part.gyro[1]);
+			out[2] = static_cast<int32_t>(part.gyro[2]);
+		}
+
+		void getAccel(int32_t out[3]) {
+			out[0] = static_cast<int32_t>(part.accel[0]);
+			out[1] = static_cast<int32_t>(part.accel[1]);
+			out[2] = static_cast<int32_t>(part.accel[2]);
+		}
 	};
 #pragma pack(pop)
 
+	static_assert(sizeof(FifoEntryAlignedHires) == 19);
+	static_assert(sizeof(FifoEntryAlignedDefault) == 15);
+
+	using FifoEntryAligned = std::conditional<
+		DEBUG_ICM42688_HIRES,
+		FifoEntryAlignedHires,
+		FifoEntryAlignedDefault>::type;
+
 	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
+	// max 4 readings in highres mode 8 readings delay too high 6 seems to be the
+	// edge to work reliably. Tested on ESP8266 with 2 IMU
+	static constexpr size_t MaxReadings = DEBUG_ICM42688_HIRES ? 4 : 8;
 
 	bool initialize() {
 		// perform initialization step
@@ -152,10 +217,11 @@ struct ICM42688 {
 		return true;
 	}
 
-	void bulkRead(DriverCallbacks<int32_t>&& callbacks) {
+	bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
-		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
+		std::array<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
+
 		const auto bytes_to_read = std::min(
 									   static_cast<size_t>(read_buffer.size()),
 									   static_cast<size_t>(fifo_bytes)
@@ -171,22 +237,13 @@ struct ICM42688 {
 				sizeof(FifoEntryAligned)
 			);  // skip fifo header
 
-			const int32_t gyroData[3]{
-				static_cast<int32_t>(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf),
-				static_cast<int32_t>(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf),
-				static_cast<int32_t>(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf),
-			};
+			int32_t gyroData[3];
+			entry.getGyro(gyroData);
 			callbacks.processGyroSample(gyroData, GyrTs);
 
 			if (entry.part.accel[0] != -32768) {
-				const int32_t accelData[3]{
-					static_cast<int32_t>(entry.part.accel[0]) << 4
-						| (static_cast<int32_t>(entry.part.xlsb) & 0xf0 >> 4),
-					static_cast<int32_t>(entry.part.accel[1]) << 4
-						| (static_cast<int32_t>(entry.part.ylsb) & 0xf0 >> 4),
-					static_cast<int32_t>(entry.part.accel[2]) << 4
-						| (static_cast<int32_t>(entry.part.zlsb) & 0xf0 >> 4),
-				};
+				int32_t accelData[3];
+				entry.getAccel(accelData);
 				callbacks.processAccelSample(accelData, AccTs);
 			}
 
@@ -197,6 +254,7 @@ struct ICM42688 {
 				);
 			}
 		}
+		return fifo_bytes > bytes_to_read;
 	}
 };
 

src/sensors/softfusion/drivers/icm45base.h

@@ -238,13 +238,13 @@ struct ICM45Base {
 	// stack overflow and panic
 	std::vector<uint8_t> read_buffer;
 
-	void bulkRead(DriverCallbacks<int32_t>&& callbacks) {
+	bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
 		constexpr int16_t InvalidReading = -32768;
 
 		size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount);
 
 		if (fifo_packets <= 1) {
-			return;
+			return false;
 		}
 
 		// AN-000364
@@ -264,9 +264,9 @@ struct ICM45Base {
 		// can cause FIFO data corruption, from happening.
 		--fifo_packets;
 
-		fifo_packets = std::min(fifo_packets, MaxReadings);
+		auto packets_to_read = std::min(fifo_packets, MaxReadings);
 
-		size_t bytes_to_read = fifo_packets * FullFifoEntrySize;
+		size_t bytes_to_read = packets_to_read * FullFifoEntrySize;
 		m_RegisterInterface
 			.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
 
@@ -307,6 +307,8 @@ struct ICM45Base {
 				callbacks.processTempSample(static_cast<int16_t>(entry.temp), TempTs);
 			}
 		}
+
+		return fifo_packets > MaxReadings;
 	}
 
 	template <typename Reg>

src/sensors/softfusion/drivers/lsm6ds-common.h

@@ -55,7 +55,7 @@ struct LSM6DSOutputHandler {
 	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
 
 	template <typename Regs>
-	void bulkRead(
+	bool bulkRead(
 		DriverCallbacks<int16_t>&& callbacks,
 		float GyrTs,
 		float AccTs,
@@ -103,6 +103,7 @@ struct LSM6DSOutputHandler {
 					break;
 			}
 		}
+		return fifo_bytes > bytes_to_read;
 	}
 };
 

src/sensors/softfusion/drivers/lsm6ds3trc.h

@@ -54,7 +54,7 @@ struct LSM6DS3TRC {
 	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 28.571428571f;
-	static constexpr float AccelSensitivity = 4098.360655738f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
@@ -122,14 +122,14 @@ struct LSM6DS3TRC {
 		return true;
 	}
 
-	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto read_result = m_RegisterInterface.readReg16(Regs::FifoStatus);
 		if (read_result & 0x4000) {  // overrun!
 			// disable and re-enable fifo to clear it
 			m_Logger.debug("Fifo overrun, resetting...");
 			m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, 0);
 			m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
-			return;
+			return true;
 		}
 		const auto unread_entries = read_result & 0x7ff;
 		constexpr auto single_measurement_words = 6;
@@ -156,6 +156,8 @@ struct LSM6DS3TRC {
 			callbacks.processAccelSample(&read_buffer[i + 3], AccTs);
 			callbacks.processTempSample(read_buffer[i + 9], TempTs);
 		}
+
+		return static_cast<size_t>(unread_entries) > read_buffer.size();
 	}
 };  // namespace SlimeVR::Sensors::SoftFusion::Drivers
 

src/sensors/softfusion/drivers/lsm6dso.h

@@ -53,7 +53,7 @@ struct LSM6DSO : LSM6DSOutputHandler {
 	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 1000 / 35.0f;
-	static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
@@ -117,8 +117,8 @@ struct LSM6DSO : LSM6DSOutputHandler {
 		return true;
 	}
 
-	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
-		LSM6DSOutputHandler::template bulkRead<Regs>(
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		return LSM6DSOutputHandler::template bulkRead<Regs>(
 			std::move(callbacks),
 			GyrTs,
 			AccTs,

src/sensors/softfusion/drivers/lsm6dsr.h

@@ -51,7 +51,7 @@ struct LSM6DSR : LSM6DSOutputHandler {
 	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 1000 / 35.0f;
-	static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
@@ -115,8 +115,8 @@ struct LSM6DSR : LSM6DSOutputHandler {
 		return true;
 	}
 
-	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
-		LSM6DSOutputHandler::template bulkRead<Regs>(
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		return LSM6DSOutputHandler::template bulkRead<Regs>(
 			std::move(callbacks),
 			GyrTs,
 			AccTs,

src/sensors/softfusion/drivers/lsm6dsv.h

@@ -52,7 +52,7 @@ struct LSM6DSV : LSM6DSOutputHandler {
 	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 1000 / 35.0f;
-	static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
@@ -131,8 +131,8 @@ struct LSM6DSV : LSM6DSOutputHandler {
 		return true;
 	}
 
-	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
-		LSM6DSOutputHandler::template bulkRead<Regs>(
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		return LSM6DSOutputHandler::template bulkRead<Regs>(
 			std::move(callbacks),
 			GyrTs,
 			AccTs,

src/sensors/softfusion/drivers/mpu6050.h

@@ -176,7 +176,7 @@ struct MPU6050 {
 		return result;
 	}
 
-	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto status = m_RegisterInterface.readReg(Regs::IntStatus);
 
 		if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) {
@@ -184,7 +184,7 @@ struct MPU6050 {
 			// This necessitates a reset
 			m_Logger.debug("Fifo overrun, resetting...");
 			resetFIFO();
-			return;
+			return true;
 		}
 
 		std::array<uint8_t, 12 * 10>
@@ -194,7 +194,7 @@ struct MPU6050 {
 		auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size())
 					   / sizeof(FifoSample) * sizeof(FifoSample);
 		if (!readBytes) {
-			return;
+			return false;
 		}
 
 		m_RegisterInterface.readBytes(Regs::FifoData, readBytes, readBuffer.data());
@@ -213,6 +213,8 @@ struct MPU6050 {
 			xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z);
 			callbacks.processGyroSample(xyz, GyrTs);
 		}
+
+		return byteCount > readBytes;
 	}
 };  // namespace SlimeVR::Sensors::SoftFusion::Drivers
 

src/sensors/softfusion/runtimecalibration/CalibrationStep.h

@@ -46,6 +46,9 @@ public:
 	virtual void cancel() = 0;
 
 	virtual bool requiresRest() { return true; }
+	// Signals that the sensor had more packets than the MCU could read, which can
+	// compromise calibration
+	virtual void signalOverwhelmed() {}
 	virtual void processAccelSample(const SensorRawT accelSample[3]) {}
 	virtual void processGyroSample(const SensorRawT accelSample[3]) {}
 	virtual void processTempSample(float tempSample) {}

src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h

@@ -142,10 +142,14 @@ public:
 
 		switch (result) {
 			case CalibrationStep<RawSensorT>::TickResult::DONE:
+				if (nextCalibrationStep == CalibrationStepEnum::SAMPLING_RATE) {
+					stepCalibrationForward(true, false);
+					break;
+				}
 				stepCalibrationForward();
 				break;
 			case CalibrationStep<RawSensorT>::TickResult::SKIP:
-				stepCalibrationForward(false);
+				stepCalibrationForward(false, false);
 				break;
 			case CalibrationStep<RawSensorT>::TickResult::CONTINUE:
 				break;
@@ -182,6 +186,12 @@ public:
 		return activeCalibration.MotionlessData;
 	}
 
+	void signalOverwhelmed() final {
+		if (isCalibrating) {
+			currentStep->signalOverwhelmed();
+		}
+	}
+
 	void provideAccelSample(const RawSensorT accelSample[3]) final {
 		if (isCalibrating) {
 			currentStep->processAccelSample(accelSample);
@@ -247,7 +257,7 @@ private:
 		}
 	}
 
-	void stepCalibrationForward(bool save = true) {
+	void stepCalibrationForward(bool print = true, bool save = true) {
 		currentStep->cancel();
 		switch (nextCalibrationStep) {
 			case CalibrationStepEnum::NONE:
@@ -255,14 +265,14 @@ private:
 			case CalibrationStepEnum::SAMPLING_RATE:
 				nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
 				currentStep = &motionlessCalibrationStep;
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::TIMESTEPS);
 				}
 				break;
 			case CalibrationStepEnum::MOTIONLESS:
 				nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
 				currentStep = &gyroBiasCalibrationStep;
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::MOTIONLESS);
 				}
 				break;
@@ -274,7 +284,7 @@ private:
 					currentStep = &gyroBiasCalibrationStep;
 				}
 
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::GYRO_BIAS);
 				}
 				break;
@@ -282,7 +292,7 @@ private:
 				nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
 				currentStep = &gyroBiasCalibrationStep;
 
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::ACCEL_BIAS);
 				}
 
@@ -306,8 +316,6 @@ private:
 		calibration.data.runtimeCalibration = this->calibration;
 		configuration.setSensor(sensorId, calibration);
 		configuration.save();
-
-		ledManager.blink(100);
 	}
 
 	enum class CalibrationPrintFlags {

src/sensors/softfusion/runtimecalibration/SampleRateCalibrationStep.h

@@ -67,6 +67,14 @@ public:
 	void cancel() override final { calibrationData.reset(); }
 	bool requiresRest() override final { return false; }
 
+	void signalOverwhelmed() override final {
+		// Not good, restart
+		calibrationData.value().accelSamples = 0;
+		calibrationData.value().gyroSamples = 0;
+		calibrationData.value().tempSamples = 0;
+		calibrationData.value().startMillis = millis();
+	}
+
 	void processAccelSample(const SensorRawT accelSample[3]) override final {
 		calibrationData.value().accelSamples++;
 	}

src/sensors/softfusion/softfusionsensor.h

@@ -241,7 +241,7 @@ public:
 		constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6f;
 		elapsed = now - m_lastRotationPacketSent;
 		if (elapsed >= sendInterval) {
-			m_sensor.bulkRead({
+			auto overwhelmed = m_sensor.bulkRead({
 				[&](const auto sample[3], float AccTs) {
 					processAccelSample(sample, AccTs);
 				},
@@ -252,6 +252,9 @@ public:
 					processTempSample(sample, TempTs);
 				},
 			});
+			if (overwhelmed) {
+				calibrator.signalOverwhelmed();
+			}
 			if (!m_fusion.isUpdated()) {
 				checkSensorTimeout();
 				return;

src/serial/serialcommands.cpp

@@ -44,6 +44,21 @@
 #define CALLBACK_SIZE 6  // Default callback size
 #endif
 
+#if defined(VENDOR_URL) && defined(VENDOR_NAME) && defined(PRODUCT_NAME) \
+	&& defined(UPDATE_ADDRESS) && defined(UPDATE_NAME)
+constexpr const char* FULL_VENDOR_STR
+	= "Vendor: " VENDOR_NAME " (" VENDOR_URL "), product: " PRODUCT_NAME
+	  ", firmware update url: " UPDATE_ADDRESS ", name: " UPDATE_NAME;
+#elif defined(VENDOR_URL) && defined(VENDOR_NAME) && defined(PRODUCT_NAME)
+constexpr const char* FULL_VENDOR_STR
+	= "Vendor: " VENDOR_NAME " (" VENDOR_URL "), product: " PRODUCT_NAME;
+#elif defined(VENDOR_NAME) && defined(PRODUCT_NAME)
+constexpr const char* FULL_VENDOR_STR
+	= "Vendor: " VENDOR_NAME ", product: " PRODUCT_NAME;
+#else
+constexpr const char* FULL_VENDOR_STR = "Vendor: Unknown, product: Unknown";
+#endif
+
 namespace SerialCommands {
 SlimeVR::Logging::Logger logger("SerialCommands");
 
@@ -165,37 +180,7 @@ void printState() {
 		wifiNetwork.getWiFiState()
 	);
 
-	char vendorBuffer[512];
-	size_t writtenLength;
-
-	if (strlen(VENDOR_URL) == 0) {
-		sprintf(
-			vendorBuffer,
-			"Vendor: %s, product: %s%n",
-			VENDOR_NAME,
-			PRODUCT_NAME,
-			&writtenLength
-		);
-	} else {
-		sprintf(
-			vendorBuffer,
-			"Vendor: %s (%s), product: %s%n",
-			VENDOR_NAME,
-			VENDOR_URL,
-			PRODUCT_NAME,
-			&writtenLength
-		);
-	}
-
-	if (strlen(UPDATE_ADDRESS) > 0 && strlen(UPDATE_NAME) > 0) {
-		sprintf(
-			vendorBuffer + writtenLength,
-			", firmware update url: %s, name: %s",
-			UPDATE_ADDRESS,
-			UPDATE_NAME
-		);
-	}
-	logger.info("%s", vendorBuffer);
+	logger.info("%s", FULL_VENDOR_STR);
 
 	for (auto& sensor : sensorManager.getSensors()) {
 		logger.info(