From 8e498eca42bfaa4364dbccc1d357ac12dec736bd Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Fri, 30 May 2025 00:31:50 +0200 Subject: [PATCH 1/6] WhoAmI check working --- src/sensors/softfusion/drivers/icm45base.h | 163 +++++++++++++++++++++ src/sensors/softfusion/imuconsts.h | 2 + src/sensors/softfusion/magdriver.cpp | 73 +++++++++ src/sensors/softfusion/magdriver.h | 69 +++++++++ src/sensors/softfusion/softfusionsensor.h | 14 ++ 5 files changed, 321 insertions(+) create mode 100644 src/sensors/softfusion/magdriver.cpp create mode 100644 src/sensors/softfusion/magdriver.h diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 5646fe355..0bfa244b6 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -104,6 +104,77 @@ struct ICM45Base { static constexpr uint8_t FifoCount = 0x12; static constexpr uint8_t FifoData = 0x14; + + // Indirect Register Access + + static constexpr uint32_t IRegWaitTimeMicros = 4; + + enum class Bank : uint8_t { + IMemSram = 0x00, + IPregBar = 0xa0, + IPregSys1 = 0xa4, + IPregSys2 = 0xa5, + IPregTop1 = 0xa2, + }; + + static constexpr uint8_t IRegAddr = 0x7c; + static constexpr uint8_t IRegData = 0x7e; + + // Mag Support + + struct IOCPadScenarioAuxOvrd { + static constexpr uint8_t reg = 0x30; + static constexpr uint8 value = (0b1 << 4) // Enable AUX1 override + | (0b01 << 2) // Enable I2CM master + | (0b1 << 1) // Enable AUX1 enable override + | (0b1 << 0); // Enable AUX1 + }; + + struct I2CMCommand0 { + static constexpr Bank bank = Bank::IPregTop1; + static constexpr uint8 reg = 0x06; + }; + + struct I2CMDevProfile0 { + static constexpr Bank bank = Bank::IPregTop1; + static constexpr uint8 reg = 0x0e; + }; + + struct I2CMDevProfile1 { + static constexpr Bank bank = Bank::IPregTop1; + static constexpr uint8 reg = 0x0f; + }; + + struct I2CMWrData0 { + static constexpr Bank bank = Bank::IPregTop1; + static constexpr uint8 reg = 0x33; + }; + + struct I2CMRdData0 { + static constexpr Bank bank = Bank::IPregTop1; + static constexpr uint8 reg = 0x1b; + }; + + struct DmpExtSenOdrCfg { + // TODO: todo + }; + + struct I2CMControl { + static constexpr Bank bank = Bank::IPregTop1; + static constexpr uint8 reg = 0x16; + }; + + struct I2CMStatus { + static constexpr Bank bank = Bank::IPregTop1; + static constexpr uint8 reg = 0x18; + + static constexpr uint8_t SDAErr = 0b1 << 5; + static constexpr uint8_t SCLErr = 0b1 << 4; + static constexpr uint8_t SRSTErr = 0b1 << 3; + static constexpr uint8_t TimeoutErr = 0b1 << 2; + static constexpr uint8_t Done = 0b1 << 1; + static constexpr uint8_t Busy = 0b1 << 0; + }; }; #pragma pack(push, 1) @@ -149,6 +220,11 @@ struct ICM45Base { BaseRegs::PwrMgmt0::value ); + m_RegisterInterface.writeReg( + BaseRegs::IOCPadScenarioAuxOvrd::reg, + BaseRegs::IOCPadScenarioAuxOvrd::value + ); + read_buffer.resize(FullFifoEntrySize * MaxReadings); delay(1); @@ -231,6 +307,93 @@ struct ICM45Base { } } } + + template + uint8_t readBankRegister() { + uint8_t buffer; + readBankRegister(&buffer, sizeof(buffer)); + return buffer; + } + + template + void readBankRegister(T* buffer, size_t length) { + uint8_t data[] = { + static_cast(Reg::bank), + Reg::reg, + }; + + auto* bufferBytes = reinterpret_cast(buffer); + m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data); + delayMicroseconds(BaseRegs::IRegWaitTimeMicros); + for (size_t i = 0; i < length * sizeof(T); i++) { + bufferBytes[i] = m_RegisterInterface.readReg(BaseRegs::IRegData); + delayMicroseconds(BaseRegs::IRegWaitTimeMicros); + } + } + + template + void writeBankRegister() { + writeBankRegister(&Reg::value, sizeof(Reg::value)); + } + + template + void writeBankRegister(T* buffer, size_t length) { + auto* bufferBytes = reinterpret_cast(buffer); + + uint8_t data[] = { + static_cast(Reg::bank), + Reg::reg, + bufferBytes[0], + }; + + m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data); + delayMicroseconds(BaseRegs::IRegWaitTimeMicros); + for (size_t i = 1; i < length * sizeof(T); i++) { + m_RegisterInterface.writeReg(BaseRegs::IRegData, bufferBytes[i]); + delayMicroseconds(BaseRegs::IRegWaitTimeMicros); + } + } + + template + void writeBankRegister(uint8_t value) { + writeBankRegister(&value, sizeof(value)); + } + + void setAuxId(uint8_t deviceId) { + writeBankRegister(deviceId); + } + + uint8_t readAux(uint8_t address) { + writeBankRegister(address); + + m_Logger.debug("Reading address %02x", address); + + writeBankRegister( + (0b1 << 7) // Last transaction + | (0b0 << 6) // Channel 0 + | (0b01 << 4) // Read with register + | (0b0001 << 0) // Read 1 byte + ); + writeBankRegister( + (0b0 << 6) // No restarts + | (0b0 << 3) // Fast mode + | (0b1 << 0) // Start transaction + ); + + while (readBankRegister() + & BaseRegs::I2CMStatus::Busy) + ; + + m_Logger.debug( + "Read result status: %02x", + readBankRegister() + ); + + uint8_t result = readBankRegister(); + m_Logger.debug("Read result: %02x", result); + + return result; + } }; }; // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/imuconsts.h b/src/sensors/softfusion/imuconsts.h index 3ef880045..90d062182 100644 --- a/src/sensors/softfusion/imuconsts.h +++ b/src/sensors/softfusion/imuconsts.h @@ -56,4 +56,6 @@ struct IMUConsts { return IMU::TempTs; } } + + static constexpr bool SupportsMags = requires(IMU& i) { i.readAux(0x00); }; }; diff --git a/src/sensors/softfusion/magdriver.cpp b/src/sensors/softfusion/magdriver.cpp new file mode 100644 index 000000000..7c8c9b342 --- /dev/null +++ b/src/sensors/softfusion/magdriver.cpp @@ -0,0 +1,73 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "magdriver.h" + +namespace SlimeVR::Sensors::SoftFusion { + +std::vector MagDriver::supportedMags{ + MagDefinition{ + .name = "QMC6309", + + .deviceId = 0x7c, + + .whoAmIReg = 0x00, + .expectedWhoAmI = 0x90, + }, + MagDefinition{ + .name = "IST8306", + + .deviceId = 0x19, + + .whoAmIReg = 0x00, + .expectedWhoAmI = 0x06, + }, +}; + +bool MagDriver::init(MagInterface&& interface) { + for (auto& mag : supportedMags) { + interface.setDeviceId(mag.deviceId); + + logger.info("Trying mag %s!", mag.name); + + uint8_t whoAmI = interface.readByte(mag.whoAmIReg); + if (whoAmI != mag.expectedWhoAmI) { + continue; + } + + // TODO: check mag compatibility + + detectedMag = mag; + + logger.info("Found mag %s! Initializing", mag.name); + // TODO: initialize mag + + break; + } + + this->interface = interface; + + return detectedMag.has_value(); +} + +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/magdriver.h b/src/sensors/softfusion/magdriver.h new file mode 100644 index 000000000..e750ab3c8 --- /dev/null +++ b/src/sensors/softfusion/magdriver.h @@ -0,0 +1,69 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include + +#include "logging/Logger.h" +#include "sensorinterface/RegisterInterface.h" + +namespace SlimeVR::Sensors::SoftFusion { + +enum class MagDataWidth { + SixByte, + NineByte, +}; + +struct MagInterface { + std::function readByte; + std::function writeByte; + std::function setDeviceId; + std::function startPolling; + std::function stopPolling; +}; + +struct MagDefinition { + const char* name; + + uint8_t deviceId; + + uint8_t whoAmIReg; + uint8_t expectedWhoAmI; +}; + +class MagDriver { +public: + bool init(MagInterface&& interface); + +private: + std::optional detectedMag; + MagInterface interface; + + static std::vector supportedMags; + + Logging::Logger logger{"MagDriver"}; +}; + +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 4d62ff772..9c11c6952 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -41,6 +41,7 @@ #include "drivers/callbacks.h" #include "imuconsts.h" #include "motionprocessing/types.h" +#include "sensors/softfusion/magdriver.h" namespace SlimeVR::Sensors { @@ -331,6 +332,17 @@ class SoftFusionSensor : public Sensor { working = true; calibrator.checkStartupCalibration(); + + if constexpr (Consts::SupportsMags) { + magDriver.init(SoftFusion::MagInterface{ + .readByte = [&](uint8_t address) { return m_sensor.readAux(address); }, + .writeByte = [&](uint8_t address, uint8_t value) {}, + .setDeviceId = [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); }, + .startPolling + = [&](uint8_t dataReg, SoftFusion::MagDataWidth dataWidth) {}, + .stopPolling = [&]() {}, + }); + } } void startCalibration(int calibrationType) final { @@ -356,6 +368,8 @@ class SoftFusionSensor : public Sensor { RestCalibrationDetector calibrationDetector; + SoftFusion::MagDriver magDriver; + static bool checkPresent(const RegisterInterface& imuInterface) { I2Cdev::readTimeout = 100; auto value = imuInterface.readReg(SensorType::Regs::WhoAmI::reg); From a01617f98c70ddd9bd2393de176714afaff14129 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 31 May 2025 15:59:25 +0200 Subject: [PATCH 2/6] In theory this should be setting up the mag --- src/sensors/SensorToggles.cpp | 24 +++++++++ src/sensors/SensorToggles.h | 9 ++++ src/sensors/bno080sensor.cpp | 7 +++ src/sensors/sensor.cpp | 10 ++-- src/sensors/softfusion/drivers/icm45base.h | 56 +++++++++++++++---- src/sensors/softfusion/imuconsts.h | 7 +++ src/sensors/softfusion/magdriver.cpp | 63 +++++++++++++++++++--- src/sensors/softfusion/magdriver.h | 9 +++- src/sensors/softfusion/softfusionsensor.h | 36 ++++++++++--- 9 files changed, 194 insertions(+), 27 deletions(-) diff --git a/src/sensors/SensorToggles.cpp b/src/sensors/SensorToggles.cpp index e999dddd2..fe59e7e54 100644 --- a/src/sensors/SensorToggles.cpp +++ b/src/sensors/SensorToggles.cpp @@ -25,3 +25,27 @@ bool SensorToggleState::getToggle(SensorToggles toggle) const { } return false; } + +void SensorToggleState::onToggleChange( + std::function&& callback +) { + this->callback = callback; +} + +void SensorToggleState::emitToggleChange(SensorToggles toggle, bool state) const { + if (callback) { + (*callback)(toggle, state); + } +} + +const char* SensorToggleState::toggleToString(SensorToggles toggle) { + switch (toggle) { + case SensorToggles::MagEnabled: + return "MagEnabled"; + case SensorToggles::CalibrationEnabled: + return "CalibrationEnabled"; + case SensorToggles::TempGradientCalibrationEnabled: + return "TempGradientCalibrationEnabled"; + } + return "Unknown"; +} diff --git a/src/sensors/SensorToggles.h b/src/sensors/SensorToggles.h index 950adc040..d68414422 100644 --- a/src/sensors/SensorToggles.h +++ b/src/sensors/SensorToggles.h @@ -24,6 +24,8 @@ #pragma once #include +#include +#include #include "../debug.h" @@ -38,7 +40,14 @@ class SensorToggleState { void setToggle(SensorToggles toggle, bool state); [[nodiscard]] bool getToggle(SensorToggles toggle) const; + void onToggleChange(std::function&& callback); + + static const char* toggleToString(SensorToggles toggle); + private: + std::optional> callback; + void emitToggleChange(SensorToggles toggle, bool state) const; + bool magEnabled = !USE_6_AXIS; bool calibrationEnabled = true; bool tempGradientCalibrationEnabled = true; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index b34292c96..d5c1044f5 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -135,6 +135,13 @@ void BNO080Sensor::motionSetup() { configured = true; m_tpsCounter.reset(); m_dataCounter.reset(); + + toggles.onToggleChange([&](SensorToggles toggle, bool) { + if (toggle == SensorToggles::MagEnabled) { + // TODO: maybe handle this more gracefully, I'm sure it's possible + motionSetup(); + } + }); } void BNO080Sensor::motionLoop() { diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 2a9058d03..1ffe72112 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -157,12 +157,16 @@ void Sensor::markRestCalibrationComplete(bool completed) { } void Sensor::setFlag(SensorToggles toggle, bool state) { - assert(isFlagSupported(toggle)); + if (!isFlagSupported(toggle)) { + m_Logger.error( + "Toggle %s isn't supported by this sensor!", + SensorToggleState::toggleToString(toggle) + ); + return; + } toggles.setToggle(toggle, state); configuration.setSensorToggles(sensorId, toggles); configuration.save(); - - motionSetup(); } diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 0bfa244b6..3cabd5c31 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -26,6 +26,7 @@ #include "../../../sensorinterface/RegisterInterface.h" #include "callbacks.h" +#include "sensors/softfusion/magdriver.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -366,8 +367,6 @@ struct ICM45Base { uint8_t readAux(uint8_t address) { writeBankRegister(address); - m_Logger.debug("Reading address %02x", address); - writeBankRegister( (0b1 << 7) // Last transaction | (0b0 << 6) // Channel 0 @@ -380,19 +379,58 @@ struct ICM45Base { | (0b1 << 0) // Start transaction ); - while (readBankRegister() + uint8_t lastStatus; + while ((lastStatus = readBankRegister()) & BaseRegs::I2CMStatus::Busy) ; - m_Logger.debug( - "Read result status: %02x", - readBankRegister() + if (lastStatus != BaseRegs::I2CMStatus::Done) { + m_Logger.error( + "Aux read from address 0x%02x returned status 0x%02x", + address, + lastStatus + ); + } + + return readBankRegister(); + } + + void writeAux(uint8 address, uint8 value) { + writeBankRegister(address); + writeBankRegister(value); + writeBankRegister( + (0b1 << 7) // Last transaction + | (0b0 << 6) // Channel 0 + | (0b01 << 4) // Read with register + | (0b0001 << 0) // Read 1 byte + ); + writeBankRegister( + (0b0 << 6) // No restarts + | (0b0 << 3) // Fast mode + | (0b1 << 0) // Start transaction ); - uint8_t result = readBankRegister(); - m_Logger.debug("Read result: %02x", result); + uint8_t lastStatus; + while ((lastStatus = readBankRegister()) + & BaseRegs::I2CMStatus::Busy) + ; + + if (lastStatus != BaseRegs::I2CMStatus::Done) { + m_Logger.error( + "Aux write to address 0x%02x with value 0x%02x returned status 0x%02x", + address, + value, + lastStatus + ); + } + } + + void startAuxPolling(uint8_t dataReg, MagDataWidth dataWidth) { + // TODO: + } - return result; + void stopAuxPolling() { + // TODO: } }; diff --git a/src/sensors/softfusion/imuconsts.h b/src/sensors/softfusion/imuconsts.h index 90d062182..9ee0eae1e 100644 --- a/src/sensors/softfusion/imuconsts.h +++ b/src/sensors/softfusion/imuconsts.h @@ -58,4 +58,11 @@ struct IMUConsts { } static constexpr bool SupportsMags = requires(IMU& i) { i.readAux(0x00); }; + static constexpr bool Supports9ByteMag = []() constexpr { + if constexpr (requires { IMU::Supports9ByteMag; }) { + return IMU::Supports9ByteMag; + } else { + return true; + } + }(); }; diff --git a/src/sensors/softfusion/magdriver.cpp b/src/sensors/softfusion/magdriver.cpp index 7c8c9b342..5037a3b80 100644 --- a/src/sensors/softfusion/magdriver.cpp +++ b/src/sensors/softfusion/magdriver.cpp @@ -33,6 +33,22 @@ std::vector MagDriver::supportedMags{ .whoAmIReg = 0x00, .expectedWhoAmI = 0x90, + + .dataWidth = MagDataWidth::SixByte, + .dataReg = 0x01, + + .setup = + [](MagInterface& interface) { + interface.writeByte(0x0b, 0x80); + interface.writeByte(0x0b, 0x00); // Soft reset + delay(10); + interface.writeByte(0x0b, 0x48); // Set/reset on, 8g full range, 200Hz + interface.writeByte( + 0x0a, + 0x21 + ); // LP filter 2, 8x Oversampling, normal mode + return true; + }, }, MagDefinition{ .name = "IST8306", @@ -41,10 +57,23 @@ std::vector MagDriver::supportedMags{ .whoAmIReg = 0x00, .expectedWhoAmI = 0x06, + + .dataWidth = MagDataWidth::SixByte, + .dataReg = 0x11, + + .setup = + [](MagInterface& interface) { + interface.writeByte(0x32, 0x01); // Soft reset + delay(50); + interface.writeByte(0x30, 0x20); // Noise suppression: low + interface.writeByte(0x41, 0x2d); // Oversampling: 32X + interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz + return true; + }, }, }; -bool MagDriver::init(MagInterface&& interface) { +bool MagDriver::init(MagInterface&& interface, bool supports9ByteMags) { for (auto& mag : supportedMags) { interface.setDeviceId(mag.deviceId); @@ -55,19 +84,41 @@ bool MagDriver::init(MagInterface&& interface) { continue; } - // TODO: check mag compatibility - - detectedMag = mag; + if (!supports9ByteMags && mag.dataWidth == MagDataWidth::NineByte) { + logger.error("The sensor doesn't support this mag!"); + return false; + } logger.info("Found mag %s! Initializing", mag.name); - // TODO: initialize mag + + if (!mag.setup(interface)) { + logger.error("Mag %s failed to initialize!", mag.name); + return false; + } + + detectedMag = mag; break; } this->interface = interface; - return detectedMag.has_value(); } +void MagDriver::startPolling() const { + if (!detectedMag) { + return; + } + + interface.startPolling(detectedMag->dataReg, detectedMag->dataWidth); +} + +void MagDriver::stopPolling() const { + if (!detectedMag) { + return; + } + + interface.stopPolling(); +} + } // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/magdriver.h b/src/sensors/softfusion/magdriver.h index e750ab3c8..b5e637ecc 100644 --- a/src/sensors/softfusion/magdriver.h +++ b/src/sensors/softfusion/magdriver.h @@ -51,11 +51,18 @@ struct MagDefinition { uint8_t whoAmIReg; uint8_t expectedWhoAmI; + + MagDataWidth dataWidth; + uint8 dataReg; + + std::function setup; }; class MagDriver { public: - bool init(MagInterface&& interface); + bool init(MagInterface&& interface, bool supports9ByteMags); + void startPolling() const; + void stopPolling() const; private: std::optional detectedMag; diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 9c11c6952..74daeff37 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -334,15 +334,35 @@ class SoftFusionSensor : public Sensor { calibrator.checkStartupCalibration(); if constexpr (Consts::SupportsMags) { - magDriver.init(SoftFusion::MagInterface{ - .readByte = [&](uint8_t address) { return m_sensor.readAux(address); }, - .writeByte = [&](uint8_t address, uint8_t value) {}, - .setDeviceId = [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); }, - .startPolling - = [&](uint8_t dataReg, SoftFusion::MagDataWidth dataWidth) {}, - .stopPolling = [&]() {}, - }); + magDriver.init( + SoftFusion::MagInterface{ + .readByte + = [&](uint8_t address) { return m_sensor.readAux(address); }, + .writeByte = [&](uint8_t address, uint8_t value) {}, + .setDeviceId + = [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); }, + .startPolling + = [&](uint8_t dataReg, SoftFusion::MagDataWidth dataWidth + ) { m_sensor.startAuxPolling(dataReg, dataWidth); }, + .stopPolling = [&]() { m_sensor.stopAuxPolling(); }, + }, + Consts::Supports9ByteMag + ); + + if (toggles.getToggle(SensorToggles::MagEnabled)) { + magDriver.startPolling(); + } } + + toggles.onToggleChange([&](SensorToggles toggle, bool value) { + if (toggle == SensorToggles::MagEnabled) { + if (value) { + magDriver.startPolling(); + } else { + magDriver.stopPolling(); + } + } + }); } void startCalibration(int calibrationType) final { From c1b8831e05306a47d325fe3c81a95d801b06faa9 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Mon, 2 Jun 2025 21:00:07 +0200 Subject: [PATCH 3/6] Not sure how that happened --- src/sensors/softfusion/drivers/icm45base.h | 24 +++++++++++----------- src/sensors/softfusion/magdriver.h | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 3cabd5c31..de08e4f17 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -125,35 +125,35 @@ struct ICM45Base { struct IOCPadScenarioAuxOvrd { static constexpr uint8_t reg = 0x30; - static constexpr uint8 value = (0b1 << 4) // Enable AUX1 override - | (0b01 << 2) // Enable I2CM master - | (0b1 << 1) // Enable AUX1 enable override - | (0b1 << 0); // Enable AUX1 + static constexpr uint8_t value = (0b1 << 4) // Enable AUX1 override + | (0b01 << 2) // Enable I2CM master + | (0b1 << 1) // Enable AUX1 enable override + | (0b1 << 0); // Enable AUX1 }; struct I2CMCommand0 { static constexpr Bank bank = Bank::IPregTop1; - static constexpr uint8 reg = 0x06; + static constexpr uint8_t reg = 0x06; }; struct I2CMDevProfile0 { static constexpr Bank bank = Bank::IPregTop1; - static constexpr uint8 reg = 0x0e; + static constexpr uint8_t reg = 0x0e; }; struct I2CMDevProfile1 { static constexpr Bank bank = Bank::IPregTop1; - static constexpr uint8 reg = 0x0f; + static constexpr uint8_t reg = 0x0f; }; struct I2CMWrData0 { static constexpr Bank bank = Bank::IPregTop1; - static constexpr uint8 reg = 0x33; + static constexpr uint8_t reg = 0x33; }; struct I2CMRdData0 { static constexpr Bank bank = Bank::IPregTop1; - static constexpr uint8 reg = 0x1b; + static constexpr uint8_t reg = 0x1b; }; struct DmpExtSenOdrCfg { @@ -162,12 +162,12 @@ struct ICM45Base { struct I2CMControl { static constexpr Bank bank = Bank::IPregTop1; - static constexpr uint8 reg = 0x16; + static constexpr uint8_t reg = 0x16; }; struct I2CMStatus { static constexpr Bank bank = Bank::IPregTop1; - static constexpr uint8 reg = 0x18; + static constexpr uint8_t reg = 0x18; static constexpr uint8_t SDAErr = 0b1 << 5; static constexpr uint8_t SCLErr = 0b1 << 4; @@ -395,7 +395,7 @@ struct ICM45Base { return readBankRegister(); } - void writeAux(uint8 address, uint8 value) { + void writeAux(uint8_t address, uint8_t value) { writeBankRegister(address); writeBankRegister(value); writeBankRegister( diff --git a/src/sensors/softfusion/magdriver.h b/src/sensors/softfusion/magdriver.h index b5e637ecc..9e412cf1d 100644 --- a/src/sensors/softfusion/magdriver.h +++ b/src/sensors/softfusion/magdriver.h @@ -53,7 +53,7 @@ struct MagDefinition { uint8_t expectedWhoAmI; MagDataWidth dataWidth; - uint8 dataReg; + uint8_t dataReg; std::function setup; }; From 68c63730677e0711692c38330f9cfa43d9ad0322 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Mon, 2 Jun 2025 21:15:05 +0200 Subject: [PATCH 4/6] Add magnetometer status to GET TEST and GET INFO --- src/sensors/sensor.cpp | 2 ++ src/sensors/sensor.h | 15 +++++++++----- src/sensors/softfusion/magdriver.cpp | 25 +++++++++++++++-------- src/sensors/softfusion/magdriver.h | 1 + src/sensors/softfusion/softfusionsensor.h | 4 ++++ src/serial/serialcommands.cpp | 13 +++++++++++- 6 files changed, 45 insertions(+), 15 deletions(-) diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 1ffe72112..ea484d24c 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -91,6 +91,8 @@ void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +const char* Sensor::getAttachedMagnetometer() const { return nullptr; } + SlimeVR::Configuration::SensorConfigBits Sensor::getSensorConfigData() { return SlimeVR::Configuration::SensorConfigBits{ .magEnabled = toggles.getToggle(SensorToggles::MagEnabled), diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 616bef986..72816da2c 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -72,19 +72,24 @@ class Sensor { addr = registerInterface.getAddress(); } - virtual ~Sensor(){}; - virtual void motionSetup(){}; - virtual void postSetup(){}; - virtual void motionLoop(){}; + virtual ~Sensor() {}; + virtual void motionSetup() {}; + virtual void postSetup() {}; + virtual void motionLoop() {}; virtual void sendData(); virtual void setAcceleration(Vector3 a); virtual void setFusedRotation(Quat r); - virtual void startCalibration(int calibrationType){}; + virtual void startCalibration(int calibrationType) {}; virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState(); virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); virtual void saveTemperatureCalibration(); + // TODO: currently only for softfusionsensor, bmi160 and others should get + // an overload too + virtual const char* getAttachedMagnetometer() const; + // TODO: realistically each sensor should print its own state instead of + // having 15 getters for things only the serial commands use bool isWorking() { return working; }; bool getHadData() const { return hadData; }; bool isValid() { return m_hwInterface != nullptr; }; diff --git a/src/sensors/softfusion/magdriver.cpp b/src/sensors/softfusion/magdriver.cpp index 5037a3b80..6dd58ad74 100644 --- a/src/sensors/softfusion/magdriver.cpp +++ b/src/sensors/softfusion/magdriver.cpp @@ -61,15 +61,14 @@ std::vector MagDriver::supportedMags{ .dataWidth = MagDataWidth::SixByte, .dataReg = 0x11, - .setup = - [](MagInterface& interface) { - interface.writeByte(0x32, 0x01); // Soft reset - delay(50); - interface.writeByte(0x30, 0x20); // Noise suppression: low - interface.writeByte(0x41, 0x2d); // Oversampling: 32X - interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz - return true; - }, + .setup = [](MagInterface& interface) { + interface.writeByte(0x32, 0x01); // Soft reset + delay(50); + interface.writeByte(0x30, 0x20); // Noise suppression: low + interface.writeByte(0x41, 0x2d); // Oversampling: 32X + interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz + return true; + }, }, }; @@ -121,4 +120,12 @@ void MagDriver::stopPolling() const { interface.stopPolling(); } +const char* MagDriver::getAttachedMagName() const { + if (!detectedMag) { + return nullptr; + } + + return detectedMag->name; +} + } // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/magdriver.h b/src/sensors/softfusion/magdriver.h index 9e412cf1d..92fe94c34 100644 --- a/src/sensors/softfusion/magdriver.h +++ b/src/sensors/softfusion/magdriver.h @@ -63,6 +63,7 @@ class MagDriver { bool init(MagInterface&& interface, bool supports9ByteMags); void startPolling() const; void stopPolling() const; + [[nodiscard]] const char* getAttachedMagName() const; private: std::optional detectedMag; diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index baf00f977..c47feec18 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -409,6 +409,10 @@ class SoftFusionSensor : public Sensor { return false; } } + + const char* getAttachedMagnetometer() const final { + return magDriver.getAttachedMagName(); + } }; } // namespace SlimeVR::Sensors diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 57dbdd626..499498b75 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -164,6 +164,10 @@ void printState() { sensor->isWorking() ? "true" : "false", sensor->getHadData() ? "true" : "false" ); + const char* mag = sensor->getAttachedMagnetometer(); + if (mag) { + logger.info("Sensor[%d] magnetometer: %s", sensor->getSensorId(), mag); + } } logger.info( "Battery voltage: %.3f, level: %.1f%%", @@ -288,6 +292,12 @@ void cmdGet(CmdParser* parser) { sensor0->isWorking() ? "true" : "false", sensor0->getHadData() ? "true" : "false" ); + + const char* mag = sensor0->getAttachedMagnetometer(); + if (mag) { + logger.info("[TEST] Sensor[0] magnetometer: %s", mag); + } + if (!sensor0->getHadData()) { logger.error("[TEST] Sensor[0] didn't send any data yet!"); } else { @@ -397,7 +407,8 @@ void cmdTemperatureCalibration(CmdParser* parser) { " TCAL RESET: reset current temperature calibration in RAM (does not delete " "already saved)" ); - logger.info(" TCAL SAVE: save current temperature calibration to persistent flash" + logger.info( + " TCAL SAVE: save current temperature calibration to persistent flash" ); logger.info("Note:"); logger.info( From 1c8b1776b70267dc1636c0abca04669a8de8bf4e Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Mon, 2 Jun 2025 21:18:24 +0200 Subject: [PATCH 5/6] Formatting --- src/sensors/softfusion/CalibrationBase.h | 2 +- src/sensors/softfusion/SoftfusionCalibration.h | 3 +-- src/sensors/softfusion/magdriver.cpp | 17 +++++++++-------- .../runtimecalibration/RuntimeCalibration.h | 3 +-- src/sensors/softfusion/softfusionsensor.h | 17 ++++++++--------- 5 files changed, 20 insertions(+), 22 deletions(-) diff --git a/src/sensors/softfusion/CalibrationBase.h b/src/sensors/softfusion/CalibrationBase.h index 16d823f46..d132a365b 100644 --- a/src/sensors/softfusion/CalibrationBase.h +++ b/src/sensors/softfusion/CalibrationBase.h @@ -64,7 +64,7 @@ class CalibrationBase { } virtual void checkStartupCalibration() {} - virtual void startCalibration(int calibrationType) {}; + virtual void startCalibration(int calibrationType){}; virtual bool calibrationMatches( const SlimeVR::Configuration::SensorConfig& sensorCalibration diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h index b3fadc246..15386f32a 100644 --- a/src/sensors/softfusion/SoftfusionCalibration.h +++ b/src/sensors/softfusion/SoftfusionCalibration.h @@ -165,8 +165,7 @@ class SoftfusionCalibrator : public CalibrationBase { saveCalibration(); } - bool calibrationMatches( - const Configuration::SensorConfig& sensorCalibration + bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration ) final { return sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::SFUSION diff --git a/src/sensors/softfusion/magdriver.cpp b/src/sensors/softfusion/magdriver.cpp index 6dd58ad74..8a602bdaf 100644 --- a/src/sensors/softfusion/magdriver.cpp +++ b/src/sensors/softfusion/magdriver.cpp @@ -61,14 +61,15 @@ std::vector MagDriver::supportedMags{ .dataWidth = MagDataWidth::SixByte, .dataReg = 0x11, - .setup = [](MagInterface& interface) { - interface.writeByte(0x32, 0x01); // Soft reset - delay(50); - interface.writeByte(0x30, 0x20); // Noise suppression: low - interface.writeByte(0x41, 0x2d); // Oversampling: 32X - interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz - return true; - }, + .setup = + [](MagInterface& interface) { + interface.writeByte(0x32, 0x01); // Soft reset + delay(50); + interface.writeByte(0x30, 0x20); // Noise suppression: low + interface.writeByte(0x41, 0x2d); // Oversampling: 32X + interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz + return true; + }, }, }; diff --git a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h index 26e0f85b4..b6567e6bc 100644 --- a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h +++ b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h @@ -64,8 +64,7 @@ class RuntimeCalibrator : public Sensors::CalibrationBase { activeCalibration.T_Ts = Consts::getDefaultTempTs(); } - bool calibrationMatches( - const Configuration::SensorConfig& sensorCalibration + bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration ) final { return sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::RUNTIME_CALIBRATION diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index c47feec18..c24c04cd5 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -168,13 +168,13 @@ class SoftFusionSensor : public Sensor { uint8_t = 0 ) : Sensor( - SensorType::Name, - SensorType::Type, - id, - registerInterface, - rotation, - sensorInterface - ) + SensorType::Name, + SensorType::Type, + id, + registerInterface, + rotation, + sensorInterface + ) , m_fusion( SensorType::SensorVQFParams, SensorType::GyrTs, @@ -292,8 +292,7 @@ class SoftFusionSensor : public Sensor { // zero-ed out if (calibrator.calibrationMatches(sensorCalibration)) { calibrator.assignCalibration(sensorCalibration); - } else if (sensorCalibration.type - == SlimeVR::Configuration::SensorConfigType::NONE) { + } else if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::NONE) { m_Logger.warn( "No calibration data found for sensor %d, ignoring...", sensorId From b99b457e1fd49d62fa7b929e7deaf708aca4d7ff Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Mon, 2 Jun 2025 21:18:49 +0200 Subject: [PATCH 6/6] Formatting 2 --- src/sensors/sensor.h | 10 +++++----- src/serial/serialcommands.cpp | 3 +-- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 72816da2c..ee1417654 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -72,14 +72,14 @@ class Sensor { addr = registerInterface.getAddress(); } - virtual ~Sensor() {}; - virtual void motionSetup() {}; - virtual void postSetup() {}; - virtual void motionLoop() {}; + virtual ~Sensor(){}; + virtual void motionSetup(){}; + virtual void postSetup(){}; + virtual void motionLoop(){}; virtual void sendData(); virtual void setAcceleration(Vector3 a); virtual void setFusedRotation(Quat r); - virtual void startCalibration(int calibrationType) {}; + virtual void startCalibration(int calibrationType){}; virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState(); virtual void printDebugTemperatureCalibrationState(); diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 499498b75..00ce408bb 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -407,8 +407,7 @@ void cmdTemperatureCalibration(CmdParser* parser) { " TCAL RESET: reset current temperature calibration in RAM (does not delete " "already saved)" ); - logger.info( - " TCAL SAVE: save current temperature calibration to persistent flash" + logger.info(" TCAL SAVE: save current temperature calibration to persistent flash" ); logger.info("Note:"); logger.info(