diff --git a/src/consts.h b/src/consts.h index a6640879a..b3d9a89ff 100644 --- a/src/consts.h +++ b/src/consts.h @@ -42,6 +42,8 @@ enum class ImuID { LSM6DSV, LSM6DSO, LSM6DSR, + ICM45686, + ICM45605, Empty = 255 }; @@ -62,6 +64,8 @@ enum class ImuID { #define IMU_LSM6DSO SoftFusionLSM6DSO #define IMU_LSM6DSR SoftFusionLSM6DSR #define IMU_MPU6050_SF SoftFusionMPU6050 +#define IMU_ICM45686 SoftFusionICM45686 +#define IMU_ICM45605 SoftFusionICM45605 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 1c1dc9cc7..f875671af 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -38,6 +38,8 @@ #include "softfusion/drivers/lsm6dso.h" #include "softfusion/drivers/lsm6dsr.h" #include "softfusion/drivers/mpu6050.h" +#include "softfusion/drivers/icm45686.h" +#include "softfusion/drivers/icm45605.h" #include "softfusion/i2cimpl.h" @@ -56,6 +58,8 @@ namespace SlimeVR using SoftFusionLSM6DSO = SoftFusionSensor; using SoftFusionLSM6DSR = SoftFusionSensor; using SoftFusionMPU6050 = SoftFusionSensor; + using SoftFusionICM45686 = SoftFusionSensor; + using SoftFusionICM45605 = SoftFusionSensor; // TODO Make it more generic in the future and move another place (abstract sensor interface) void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) @@ -172,7 +176,7 @@ namespace SlimeVR m_LastBundleSentAtMicros = now; #endif - + #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED networkConnection.beginBundle(); #endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 94299f12c..60aefe79e 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -101,6 +101,8 @@ const char * getIMUNameByType(ImuID imuType) { return "LSM6DSO"; case ImuID::LSM6DSR: return "LSM6DSR"; + case ImuID::ICM45686: + return "ICM45686"; case ImuID::Unknown: case ImuID::Empty: return "UNKNOWN"; diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index e071db55a..769e34c89 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -50,6 +50,8 @@ struct ICM42688 static constexpr float GyroSensitivity = 32.8f; static constexpr float AccelSensitivity = 4096.0f; + static constexpr bool Uses32BitSensorData = true; + I2CImpl i2c; SlimeVR::Logging::Logger &logger; ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger &logger) @@ -76,7 +78,7 @@ struct ICM42688 }; struct FifoConfig1 { static constexpr uint8_t reg = 0x5f; - static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2); //fifo accel en=1, gyro=1, temp=0 todo: fsync, hires + static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2) | (0b0 << 4); //fifo accel en=1, gyro=1, temp=0, hires=1 }; struct GyroConfig { static constexpr uint8_t reg = 0x4f; @@ -106,15 +108,18 @@ struct ICM42688 struct { int16_t accel[3]; int16_t gyro[3]; - uint8_t temp; - uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned + uint16_t temp; + uint16_t timestamp; // cannot do uint16_t because it's unaligned + uint8_t xlsb; + uint8_t ylsb; + uint8_t zlsb; } part; - uint8_t raw[15]; + uint8_t raw[19]; }; }; #pragma pack(pop) - static constexpr size_t FullFifoEntrySize = 16; + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; bool initialize() { @@ -143,7 +148,7 @@ struct ICM42688 template void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); - + std::array read_buffer; // max 8 readings const auto bytes_to_read = std::min(static_cast(read_buffer.size()), static_cast(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize; @@ -151,14 +156,25 @@ struct ICM42688 for (auto i=0u; i(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf), + static_cast(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf), + static_cast(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf), + }; + processGyroSample(gyroData, GyrTs); if (entry.part.accel[0] != -32768) { - processAccelSample(entry.part.accel, AccTs); - } - } + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 | (static_cast(entry.part.xlsb) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 | (static_cast(entry.part.ylsb) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 | (static_cast(entry.part.zlsb) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + } } }; -} // namespace \ No newline at end of file +} // namespace diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h new file mode 100644 index 000000000..c27813f8f --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -0,0 +1,53 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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 "icm45base.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45605 : ICM45Base { + static constexpr auto Name = "ICM-45605"; + static constexpr auto Type = ImuID::ICM45605; + + ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : ICM45Base(i2c, logger) {} + + using ICM45Base::i2c; + + bool initialize() { + ICM45Base::softResetIMU(); + return ICM45Base::initializeBase(); + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h new file mode 100644 index 000000000..e3aff45bb --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -0,0 +1,67 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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 "icm45base.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45686 : ICM45Base { + static constexpr auto Name = "ICM-45688"; + static constexpr auto Type = ImuID::ICM45686; + + ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : ICM45Base(i2c, logger) {} + + struct AdditionalRegs { + struct Pin9Config { + static constexpr uint8_t reg = 0x31; + static constexpr uint8_t value = 0b00000110; // pin 9 to clkin + }; + + struct RtcConfig { + static constexpr uint8_t reg = 0x26; + static constexpr uint8_t value = 0b00100011; // enable RTC + }; + }; + + using ICM45Base::i2c; + + bool initialize() { + ICM45Base::softResetIMU(); + i2c.writeReg(AdditionalRegs::Pin9Config::reg, AdditionalRegs::Pin9Config::value); + i2c.writeReg(AdditionalRegs::RtcConfig::reg, AdditionalRegs::RtcConfig::value); + return ICM45Base::initializeBase(); + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h new file mode 100644 index 000000000..f9ffcc84a --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -0,0 +1,194 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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 + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45Base { + static constexpr uint8_t Address = 0x68; + + static constexpr float GyrTs = 1.0 / 409.6; + static constexpr float AccTs = 1.0 / 102.4; + + static constexpr float MagTs = 1.0 / 100; + + static constexpr float GyroSensitivity = 131.072f; + static constexpr float AccelSensitivity = 16384.0f; + + static constexpr bool Uses32BitSensorData = true; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x72; + static constexpr uint8_t value = 0xe5; + }; + static constexpr uint8_t TempData = 0x0c; + + struct DeviceConfig { + static constexpr uint8_t reg = 0x7f; + static constexpr uint8_t valueSwReset = 0b11; + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value + = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value + = (0b000 << 4) | 0b1001; // 32g, odr = 102.4Hz + }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x1d; + static constexpr uint8_t value + = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth + // 8k bytes <-- this disables all APEX + // features, but we don't need them + }; + + struct FifoConfig3 { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) + | (0b1 << 3); // enable FIFO, + // enable accel, + // enable gyro, + // enable hires mode + }; + + struct PwrMgmt0 { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value + = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise + }; + + static constexpr uint8_t FifoCount = 0x12; + static constexpr uint8_t FifoData = 0x14; + }; + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + struct { + int16_t accel[3]; + int16_t gyro[3]; + uint16_t temp; + uint16_t timestamp; + uint8_t lsb[3]; + } part; + uint8_t raw[19]; + }; + }; +#pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + void softResetIMU() { + i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); + delay(35); + } + + bool initializeBase() { + // perform initialization step + i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); + i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); + i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + i2c.writeReg(Regs::FifoConfig3::reg, Regs::FifoConfig3::value); + i2c.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::value); + delay(1); + + return true; + } + + float getDirectTemp() const { + const auto value = static_cast(i2c.readReg16(Regs::TempData)); + float result = ((float)value / 132.48f) + 25.0f; + return result; + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + const auto fifo_packets = i2c.readReg16(Regs::FifoCount); + const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ) + / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { + FifoEntryAligned entry; + memcpy( + entry.raw, + &read_buffer[i + 0x1], + sizeof(FifoEntryAligned) + ); // skip fifo header + const int32_t gyroData[3]{ + static_cast(entry.part.gyro[0]) << 4 + | (entry.part.lsb[0] & 0xf), + static_cast(entry.part.gyro[1]) << 4 + | (entry.part.lsb[1] & 0xf), + static_cast(entry.part.gyro[2]) << 4 + | (entry.part.lsb[2] & 0xf), + }; + processGyroSample(gyroData, GyrTs); + + if (entry.part.accel[0] != -32768) { + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 + | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + } + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index ec076a757..b9773f4d3 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -23,6 +23,9 @@ #pragma once +#include +#include + #include "../sensor.h" #include "../SensorFusionRestDetect.h" @@ -35,7 +38,12 @@ template typename T, typename I2CImpl> class SoftFusionSensor : public Sensor { using imu = T; - using RawVectorT = std::array; + + static constexpr bool Uses32BitSensorData = requires(imu& i){ i.Uses32BitSensorData; }; + + using RawSensorT = typename std::conditional::type; + using RawVectorT = std::array; + static constexpr auto UpsideDownCalibrationInit = true; static constexpr auto GyroCalibDelaySeconds = 5; static constexpr auto GyroCalibSeconds = 5; @@ -89,7 +97,7 @@ class SoftFusionSensor : public Sensor m_fusion = SensorFusionRestDetect(m_calibration.G_Ts, m_calibration.A_Ts, m_calibration.M_Ts); } - void processAccelSample(const int16_t xyz[3], const sensor_real_t timeDelta) + void processAccelSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) { sensor_real_t accelData[] = { static_cast(xyz[0]), @@ -107,7 +115,7 @@ class SoftFusionSensor : public Sensor m_fusion.updateAcc(accelData, m_calibration.A_Ts); } - void processGyroSample(const int16_t xyz[3], const sensor_real_t timeDelta) + void processGyroSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) { const sensor_real_t scaledData[] = { static_cast(GScale * (static_cast(xyz[0]) - m_calibration.G_off[0])), @@ -122,16 +130,17 @@ class SoftFusionSensor : public Sensor while (millis() < targetDelay) { #ifdef ESP8266 - ESP.wdtFeed(); - #endif + ESP.wdtFeed(); + #endif auto currentSecondsRemaining = (targetDelay - millis()) / 1000; if (currentSecondsRemaining != lastSecondsRemaining) { m_Logger.info("%d...", currentSecondsRemaining + 1); lastSecondsRemaining = currentSecondsRemaining; } + m_sensor.bulkRead( - [](const int16_t xyz[3], const sensor_real_t timeDelta) { }, - [](const int16_t xyz[3], const sensor_real_t timeDelta) { } + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} ); } } @@ -144,12 +153,12 @@ class SoftFusionSensor : public Sensor while (millis() < targetDelay) { m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { accel[0] = xyz[0]; accel[1] = xyz[1]; accel[2] = xyz[2]; }, - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { gyro[0] = xyz[0]; gyro[1] = xyz[1]; gyro[2] = xyz[2]; @@ -179,8 +188,8 @@ class SoftFusionSensor : public Sensor if (elapsed >= targetPollIntervalMicros) { m_lastPollTime = now - (elapsed - targetPollIntervalMicros); m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { processAccelSample(xyz, timeDelta); }, - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { processGyroSample(xyz, timeDelta); } + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { processAccelSample(xyz, timeDelta); }, + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { processGyroSample(xyz, timeDelta); } ); optimistic_yield(100); if (!m_fusion.isUpdated()) return; @@ -229,7 +238,7 @@ class SoftFusionSensor : public Sensor } bool initResult = false; - + if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData)); @@ -339,11 +348,11 @@ class SoftFusionSensor : public Sensor while (millis() < targetCalib) { #ifdef ESP8266 - ESP.wdtFeed(); - #endif + ESP.wdtFeed(); + #endif m_sensor.bulkRead( - [](const int16_t xyz[3], const sensor_real_t timeDelta) { }, - [&sumXYZ, &sampleCount](const int16_t xyz[3], const sensor_real_t timeDelta) { + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [&sumXYZ, &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) { sumXYZ[0] += xyz[0]; sumXYZ[1] += xyz[1]; sumXYZ[2] += xyz[2]; @@ -393,10 +402,10 @@ class SoftFusionSensor : public Sensor bool samplesGathered = false; while (!samplesGathered) { #ifdef ESP8266 - ESP.wdtFeed(); - #endif + ESP.wdtFeed(); + #endif m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { const sensor_real_t scaledData[] = { static_cast(AScale * static_cast(xyz[0])), static_cast(AScale * static_cast(xyz[1])), @@ -443,7 +452,7 @@ class SoftFusionSensor : public Sensor samplesGathered = true; } }, - [](const int16_t xyz[3], const sensor_real_t timeDelta) { } + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} ); } ledManager.off(); @@ -481,8 +490,8 @@ class SoftFusionSensor : public Sensor while ((currentTime = millis()) < calibTarget) { m_sensor.bulkRead( - [&accelSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, - [&gyroSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { gyroSamples++; } + [&accelSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, + [&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { gyroSamples++; } ); }