diff --git a/src/sensors/SensorToggles.cpp b/src/sensors/SensorToggles.cpp index 848a65d6..80ae52f4 100644 --- a/src/sensors/SensorToggles.cpp +++ b/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 { diff --git a/src/sensors/softfusion/drivers/callbacks.h b/src/sensors/softfusion/drivers/callbacks.h index 79182dc2..8da51859 100644 --- a/src/sensors/softfusion/drivers/callbacks.h +++ b/src/sensors/softfusion/drivers/callbacks.h @@ -28,7 +28,12 @@ template struct DriverCallbacks { - std::function processAccelSample; - std::function processGyroSample; - std::function processTempSample; + std::function processAccelSample + = [](SampleType*, float) {}; + std::function processGyroSample + = [](SampleType*, float) {}; + std::function processTempSample + = [](int16_t, float) {}; + std::function processMagSample + = [](uint8_t*, float) {}; }; diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 3993e841..3adab0cf 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/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 read_buffer; bool bulkRead(DriverCallbacks&& 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(address); writeBankRegister( (0b1 << 7) // Last transaction | (0b0 << 6) // Channel 0 | (0b01 << 4) // Read with register - | (0b0001 << 0) // Read 1 byte + | (length << 0) // Read "length" bytes ); writeBankRegister( (0b0 << 6) // No restarts @@ -394,17 +390,18 @@ struct ICM45Base { ); } - return readBankRegister(); + readBankRegister(buffer, length); } void writeAux(uint8_t address, uint8_t value) { - writeBankRegister(address); - writeBankRegister(value); + uint8_t writeData[] = {address, value}; + + writeBankRegister(writeData, sizeof(writeData)); writeBankRegister( (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( (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 diff --git a/src/sensors/softfusion/magdriver.cpp b/src/sensors/softfusion/magdriver.cpp index 8a602bda..eddfd464 100644 --- a/src/sensors/softfusion/magdriver.cpp +++ b/src/sensors/softfusion/magdriver.cpp @@ -49,6 +49,8 @@ std::vector MagDriver::supportedMags{ ); // LP filter 2, 8x Oversampling, normal mode return true; }, + + .resolution = 0.025f, }, MagDefinition{ .name = "IST8306", @@ -70,6 +72,8 @@ std::vector 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(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(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 diff --git a/src/sensors/softfusion/magdriver.h b/src/sensors/softfusion/magdriver.h index 92fe94c3..807a757a 100644 --- a/src/sensors/softfusion/magdriver.h +++ b/src/sensors/softfusion/magdriver.h @@ -56,6 +56,8 @@ struct MagDefinition { uint8_t dataReg; std::function setup; + + float resolution; }; class MagDriver { @@ -63,8 +65,11 @@ class MagDriver { 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 detectedMag; MagInterface interface; diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index f260c7b2..46fd7e62 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/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 @@ class SoftFusionSensor : public Sensor { [&](int16_t sample, float TempTs) { processTempSample(sample, TempTs); }, + [&](uint8_t* sample, float MagTs) { processMagSample(sample, MagTs); }, }); if (overwhelmed) { calibrator.signalOverwhelmed(); @@ -336,7 +343,8 @@ class SoftFusionSensor : public Sensor { 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 @@ class SoftFusionSensor : public Sensor { } [[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final { + if (toggle == SensorToggles::MagEnabled) { + return magDriver.isMagAttached(); + } + return toggle == SensorToggles::CalibrationEnabled || toggle == SensorToggles::TempGradientCalibrationEnabled; }