diff --git a/src/sensors/SensorToggles.cpp b/src/sensors/SensorToggles.cpp index fe59e7e..31d9ceb 100644 --- a/src/sensors/SensorToggles.cpp +++ b/src/sensors/SensorToggles.cpp @@ -12,6 +12,8 @@ void SensorToggleState::setToggle(SensorToggles toggle, bool state) { 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 79182dc..51017e1 100644 --- a/src/sensors/softfusion/drivers/callbacks.h +++ b/src/sensors/softfusion/drivers/callbacks.h @@ -28,7 +28,8 @@ 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 de08e4f..5739b04 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; void 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) { + 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; - fifo_packets = std::min(fifo_packets, MaxReadings); size_t bytes_to_read = fifo_packets * FullFifoEntrySize; @@ -365,13 +353,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 @@ -392,17 +388,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 @@ -425,13 +422,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 8a602bd..a9b685e 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", @@ -61,15 +63,16 @@ 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; + }, + + .resolution = 0.3, }, }; @@ -121,7 +124,40 @@ void MagDriver::stopPolling() const { interface.stopPolling(); } -const char* MagDriver::getAttachedMagName() const { +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 +165,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 92fe94c..807a757 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 @@ 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 detectedMag; MagInterface interface; diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 49d37c3..1c4ab7a 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; @@ -163,13 +169,13 @@ public: 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, @@ -251,6 +257,9 @@ public: [&](int16_t sample, float TempTs) { processTempSample(sample, TempTs); }, + [&](uint8_t *sample, float MagTs) { + processMagSample(sample, MagTs); + }, }); if (!m_fusion.isUpdated()) { checkSensorTimeout(); @@ -287,7 +296,8 @@ public: // 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 @@ -333,7 +343,7 @@ 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 +375,10 @@ public: } [[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final { + if (toggle == SensorToggles::MagEnabled) { + return magDriver.isMagAttached(); + } + return toggle == SensorToggles::CalibrationEnabled || toggle == SensorToggles::TempGradientCalibrationEnabled; }