Merge branch 'main' into mag-support

src/sensors/SensorToggles.cpp

@@ -15,6 +15,8 @@ void SensorToggleState::setToggle(SensorToggles toggle, bool state) {
 			values.tempGradientCalibrationEnabled = state;
 			break;
 	}
+
+	emitToggleChange(toggle, state);
 }
 
 bool SensorToggleState::getToggle(SensorToggles toggle) const {

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/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;
@@ -239,31 +236,22 @@ struct ICM45Base {
 	std::vector<uint8_t> read_buffer;
 
 	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) {
-			return false;
+		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);
 
 		size_t bytes_to_read = packets_to_read * FullFifoEntrySize;
@@ -367,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
@@ -394,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
@@ -427,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/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/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;
@@ -251,6 +257,7 @@ public:
 				[&](int16_t sample, float TempTs) {
 					processTempSample(sample, TempTs);
 				},
+				[&](uint8_t* sample, float MagTs) { processMagSample(sample, MagTs); },
 			});
 			if (overwhelmed) {
 				calibrator.signalOverwhelmed();
@@ -336,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
@@ -368,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;
 	}