Merge branch 'main' into mag-support

fix no broadcast after disconnect After a disconnect the trackers did not send a broadcast to discover the server but did directly try to send it to the server. So if for some reason the server did change its ip address the tracker had to be rebooted.

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() {
@@ -123,7 +128,8 @@ void Configuration::save() {
 		m_Logger.trace("Saving sensor toggle state for %d", i);
 
 		file = LittleFS.open(path, "w");
-		file.write((uint8_t*)&m_SensorToggles[i], sizeof(SensorToggleState));
+		auto toggleValues = m_SensorToggles[i].getValues();
+		file.write((uint8_t*)&toggleValues, sizeof(SensorToggleValues));
 		file.close();
 	}
 
@@ -133,6 +139,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 +244,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/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;
 	}
 

src/sensors/SensorToggles.cpp

@@ -1,27 +1,32 @@
 #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;
 	}
+
+	emitToggleChange(toggle, 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 +37,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/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/callbacks.h

@@ -28,7 +28,12 @@
 
 template <typename SampleType>
 struct DriverCallbacks {
-	std::function<void(const SampleType sample[3], float AccTs)> processAccelSample;
-	std::function<void(const SampleType sample[3], float GyrTs)> processGyroSample;
-	std::function<void(int16_t sample, float TempTs)> processTempSample;
+	std::function<void(const SampleType sample[3], float AccTs)> processAccelSample
+		= [](SampleType*, float) {};
+	std::function<void(const SampleType sample[3], float GyrTs)> processGyroSample
+		= [](SampleType*, float) {};
+	std::function<void(int16_t sample, float TempTs)> processTempSample
+		= [](int16_t, float) {};
+	std::function<void(uint8_t* sample, float Magts)> processMagSample
+		= [](uint8_t*, float) {};
 };

src/sensors/softfusion/drivers/icm42688.h

@@ -152,7 +152,7 @@ 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
@@ -197,6 +197,8 @@ struct ICM42688 {
 				);
 			}
 		}
+
+		return fifo_bytes > bytes_to_read;
 	}
 };
 

src/sensors/softfusion/drivers/icm45base.h

@@ -44,7 +44,8 @@ struct ICM45Base {
 	static constexpr float AccTs = 1.0 / 102.4;
 	static constexpr float TempTs = 1.0 / 409.6;
 
-	static constexpr float MagTs = 1.0 / 100;
+	static constexpr uint32_t MagPollingHz = 10;
+	static constexpr float MagTs = 1.0 / MagPollingHz;
 
 	static constexpr float GyroSensitivity = 131.072f;
 	static constexpr float AccelSensitivity = 16384.0f;
@@ -83,7 +84,7 @@ struct ICM45Base {
 		struct FifoConfig0 {
 			static constexpr uint8_t reg = 0x1d;
 			static constexpr uint8_t value
-				= (0b01 << 6) | (0b011111);  // stream to FIFO mode, FIFO depth
+				= (0b10 << 6) | (0b011111);  // stop on full FIFO mode, FIFO depth
 											 // 8k bytes <-- this disables all APEX
 											 // features, but we don't need them
 		};
@@ -156,10 +157,6 @@ struct ICM45Base {
 			static constexpr uint8_t reg = 0x1b;
 		};
 
-		struct DmpExtSenOdrCfg {
-			// TODO: todo
-		};
-
 		struct I2CMControl {
 			static constexpr Bank bank = Bank::IPregTop1;
 			static constexpr uint8_t reg = 0x16;
@@ -238,35 +235,26 @@ struct ICM45Base {
 	// stack overflow and panic
 	std::vector<uint8_t> read_buffer;
 
-	void bulkRead(DriverCallbacks<int32_t>&& callbacks) {
+	bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
+		if (magPollingEnabled && millis() - lastMagPollMillis >= MagTs * 1000) {
+			uint8_t magData[9];
+			readAux(magDataReg, magData, magDataWidth == MagDataWidth::SixByte ? 6 : 9);
+
+			callbacks.processMagSample(magData, 1.0f / MagPollingHz);
+			lastMagPollMillis += MagTs * 1000;
+		}
+
 		constexpr int16_t InvalidReading = -32768;
 
 		size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount);
 
-		if (fifo_packets <= 1) {
+		if (fifo_packets == 0) {
 			return;
 		}
 
-		// AN-000364
-		// 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA
-		//
-		// Description: When in FIFO streaming mode, a FIFO empty event
-		// (caused by host reading the last byte of the last FIFO frame) can
-		// cause FIFO data corruption in the first FIFO frame that arrives
-		// after the FIFO empty condition. Once the issue is triggered, the
-		// FIFO state is compromised and cannot recover. FIFO must be set in
-		// bypass mode to flush out the wrong state
-		//
-		// When operating in FIFO streaming mode, if FIFO threshold
-		// interrupt is triggered with M number of FIFO frames accumulated
-		// in the FIFO buffer, the host should only read the first M-1
-		// number of FIFO frames. This prevents the FIFO empty event, that
-		// can cause FIFO data corruption, from happening.
-		--fifo_packets;
+		auto packets_to_read = std::min(fifo_packets, MaxReadings);
 
-		fifo_packets = 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 +295,8 @@ struct ICM45Base {
 				callbacks.processTempSample(static_cast<int16_t>(entry.temp), TempTs);
 			}
 		}
+
+		return fifo_packets > MaxReadings;
 	}
 
 	template <typename Reg>
@@ -365,13 +355,21 @@ struct ICM45Base {
 	}
 
 	uint8_t readAux(uint8_t address) {
+		uint8_t buffer;
+		readAux(address, &buffer, sizeof(buffer));
+		return buffer;
+	}
+
+	void readAux(uint8_t address, uint8_t* buffer, size_t length) {
+		assert(length <= 15);
+
 		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
 
 		writeBankRegister<typename BaseRegs::I2CMCommand0>(
 			(0b1 << 7)  // Last transaction
 			| (0b0 << 6)  // Channel 0
 			| (0b01 << 4)  // Read with register
-			| (0b0001 << 0)  // Read 1 byte
+			| (length << 0)  // Read "length" bytes
 		);
 		writeBankRegister<typename BaseRegs::I2CMControl>(
 			(0b0 << 6)  // No restarts
@@ -392,17 +390,18 @@ struct ICM45Base {
 			);
 		}
 
-		return readBankRegister<typename BaseRegs::I2CMRdData0>();
+		readBankRegister<typename BaseRegs::I2CMRdData0>(buffer, length);
 	}
 
 	void writeAux(uint8_t address, uint8_t value) {
-		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
-		writeBankRegister<typename BaseRegs::I2CMWrData0>(value);
+		uint8_t writeData[] = {address, value};
+
+		writeBankRegister<typename BaseRegs::I2CMWrData0>(writeData, sizeof(writeData));
 		writeBankRegister<typename BaseRegs::I2CMCommand0>(
 			(0b1 << 7)  // Last transaction
 			| (0b0 << 6)  // Channel 0
-			| (0b01 << 4)  // Read with register
-			| (0b0001 << 0)  // Read 1 byte
+			| (0b00 << 4)  // Write
+			| (0b0010 << 0)  // Write 2 bytes
 		);
 		writeBankRegister<typename BaseRegs::I2CMControl>(
 			(0b0 << 6)  // No restarts
@@ -425,13 +424,19 @@ struct ICM45Base {
 		}
 	}
 
+	bool magPollingEnabled = false;
+	uint8_t magDataReg = 0x00;
+	MagDataWidth magDataWidth;
+	uint64_t lastMagPollMillis = 0;
+
 	void startAuxPolling(uint8_t dataReg, MagDataWidth dataWidth) {
-		// TODO:
+		magPollingEnabled = true;
+		magDataReg = dataReg;
+		magDataWidth = dataWidth;
+		lastMagPollMillis = millis();
 	}
 
-	void stopAuxPolling() {
-		// TODO:
-	}
+	void stopAuxPolling() { magPollingEnabled = false; }
 };
 
 };  // namespace SlimeVR::Sensors::SoftFusion::Drivers

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/magdriver.cpp

@@ -49,6 +49,8 @@ std::vector<MagDefinition> MagDriver::supportedMags{
 				);  // LP filter 2, 8x Oversampling, normal mode
 				return true;
 			},
+
+		.resolution = 0.025f,
 	},
 	MagDefinition{
 		.name = "IST8306",
@@ -70,6 +72,8 @@ std::vector<MagDefinition> MagDriver::supportedMags{
 				interface.writeByte(0x31, 0x02);  // Continuous measurement @ 10Hz
 				return true;
 			},
+
+		.resolution = 0.3,
 	},
 };
 
@@ -121,6 +125,37 @@ void MagDriver::stopPolling() const {
 	interface.stopPolling();
 }
 
+void MagDriver::scaleMagSample(const uint8_t* magSample, float* scaled) const {
+#pragma pack(push, 1)
+	struct MagData6Byte {
+		int16_t x;
+		int16_t y;
+		int16_t z;
+	};
+	struct MagData9Byte {
+		int32_t x : 24;
+		int32_t y : 24;
+		int32_t z : 24;
+	};
+#pragma pack(pop)
+
+	if (!detectedMag) {
+		return;
+	}
+
+	if (detectedMag->dataWidth == MagDataWidth::SixByte) {
+		const auto* data = reinterpret_cast<const MagData6Byte*>(magSample);
+		scaled[0] = data->x * detectedMag->resolution;
+		scaled[1] = data->y * detectedMag->resolution;
+		scaled[2] = data->z * detectedMag->resolution;
+	} else {
+		const auto* data = reinterpret_cast<const MagData9Byte*>(magSample);
+		scaled[0] = data->x * detectedMag->resolution;
+		scaled[1] = data->y * detectedMag->resolution;
+		scaled[2] = data->z * detectedMag->resolution;
+	}
+}
+
 const char* MagDriver::getAttachedMagName() const {
 	if (!detectedMag) {
 		return nullptr;
@@ -129,4 +164,6 @@ const char* MagDriver::getAttachedMagName() const {
 	return detectedMag->name;
 }
 
+bool MagDriver::isMagAttached() const { return detectedMag.has_value(); }
+
 }  // namespace SlimeVR::Sensors::SoftFusion

src/sensors/softfusion/magdriver.h

@@ -56,6 +56,8 @@ struct MagDefinition {
 	uint8_t dataReg;
 
 	std::function<bool(MagInterface& interface)> setup;
+
+	float resolution;
 };
 
 class MagDriver {
@@ -63,8 +65,11 @@ public:
 	bool init(MagInterface&& interface, bool supports9ByteMags);
 	void startPolling() const;
 	void stopPolling() const;
+	void scaleMagSample(const uint8_t* sample, float* scaled) const;
 	[[nodiscard]] const char* getAttachedMagName() const;
 
+	[[nodiscard]] bool isMagAttached() const;
+
 private:
 	std::optional<MagDefinition> detectedMag;
 	MagInterface interface;

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);
 				}
 

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

@@ -150,6 +150,12 @@ class SoftFusionSensor : public Sensor {
 		}
 	}
 
+	void processMagSample(const uint8_t* sample, const sensor_real_t timeDelta) {
+		float scaledSample[3];
+		magDriver.scaleMagSample(sample, scaledSample);
+		m_fusion.updateMag(scaledSample);
+	}
+
 public:
 	static constexpr auto TypeID = SensorType::Type;
 	static constexpr uint8_t Address = SensorType::Address;
@@ -241,7 +247,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);
 				},
@@ -251,7 +257,11 @@ public:
 				[&](int16_t sample, float TempTs) {
 					processTempSample(sample, TempTs);
 				},
+				[&](uint8_t* sample, float MagTs) { processMagSample(sample, MagTs); },
 			});
+			if (overwhelmed) {
+				calibrator.signalOverwhelmed();
+			}
 			if (!m_fusion.isUpdated()) {
 				checkSensorTimeout();
 				return;
@@ -333,7 +343,8 @@ public:
 				SoftFusion::MagInterface{
 					.readByte
 					= [&](uint8_t address) { return m_sensor.readAux(address); },
-					.writeByte = [&](uint8_t address, uint8_t value) {},
+					.writeByte = [&](uint8_t address, uint8_t value
+								 ) { m_sensor.writeAux(address, value); },
 					.setDeviceId
 					= [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); },
 					.startPolling
@@ -365,6 +376,10 @@ public:
 	}
 
 	[[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final {
+		if (toggle == SensorToggles::MagEnabled) {
+			return magDriver.isMagAttached();
+		}
+
 		return toggle == SensorToggles::CalibrationEnabled
 			|| toggle == SensorToggles::TempGradientCalibrationEnabled;
 	}

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(