github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-06 02:01:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Spi support (2) (#425)

Browse Source 
* Make SPI work

Move sensor building logic to a separate class and file

Don't use templates for RegisterInterface/I2CImpl/SPIImpl

This started getting ridiculous, now we can actually maintain it.

Make BNO085 work again

Add BNO to automatic detection, remove a bunch of others

Not all IMU types are enabled right now due to code size optimization, but it could be expanded in the future with optimization of Softfusion.

Pick IMU type automatically by asking it

* ESP32 spelling fix (#396)

Fix: ES32 -> ESP32

* (Probably) multiply acceleration by tracker rotation offset

* Split SensorBuilder into a header-source file pair

* Fix copyright

* Fix some issues with glove after all the changes

Add definitions for SlimeVR v1.2
Fix typo and add comments to quaternion sandwich function

* Add NO_WIRE definition for SPI devices

* Fix formatting

* Minor fix

* If ICM-45686 not found, ask again nicely

* Fix MCP interface not building

* Remove uneccessary "default" ctor from SPIImpl

* Remove buildSensorReal

* Invert if statement in sensorbuilder+

* Fix formatting

* If ICM-45686 not found, ask again nicely

* Fix MCP interface not building

* Fix formatting

* Various cleanup

* Formattign

---------

Co-authored-by: Butterscotch! <bscotchvanilla@gmail.com>
Co-authored-by: gorbit99 <gorbitgames@gmail.com>
This commit is contained in:
Eiren Rain
2025-04-25 22:36:49 +02:00
committed by GitHub
parent bbba63e06c
commit 72fa060506
43 changed files with 1344 additions and 525 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
lib/math/quat.cpp
View File

@@ -229,3 +229,12 @@ void Quat::set_axis_angle(const Vector3& axis, const float& angle) {
cos_angle);
}
}
void Quat::sandwich(Vector3& v) {
float tempX, tempY;
tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x;
tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x * v.y;
v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x - y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z;
v.x = tempX;
v.y = tempY;
}

8
lib/math/quat.h
View File

@@ -79,6 +79,14 @@ public:
Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq, const float& t) const;
bool equalsWithEpsilon(const Quat& q2);
/**
* @brief Rotate the vector by this quaternion
* (a sandwich product)
*
* @param vector the vector to be rotated
*/
void sandwich(Vector3& vector);
void set_axis_angle(const Vector3& axis, const float& angle);
inline void get_axis_angle(Vector3& r_axis, double& r_angle) const {
r_angle = 2 * std::acos(w);

4
platformio-tools.ini
View File

@@ -72,7 +72,7 @@ build_flags =
-DESP32C3
board = dfrobot_beetle_esp32c3
[env:BOARD_ES32C3DEVKITM1]
[env:BOARD_ESP32C3DEVKITM1]
platform = espressif32 @ 6.7.0
platform_packages =
framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
@@ -82,7 +82,7 @@ build_flags =
-DESP32C3
board = esp32-c3-devkitm-1
[env:BOARD_ES32C6DEVKITC1]
[env:BOARD_ESP32C6DEVKITC1]
platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
build_flags =
${env.build_flags}

12
src/consts.h
View File

@@ -49,6 +49,7 @@ enum class SensorTypeID : uint8_t {
Empty = 255
};
#define IMU_AUTO SensorAuto
#define IMU_UNKNOWN ErroneousSensor
#define IMU_MPU9250 MPU9250Sensor
#define IMU_MPU6500 MPU6050Sensor
@@ -72,27 +73,28 @@ enum class SensorTypeID : uint8_t {
#define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
#define BOARD_UNKNOWN 0
#define BOARD_SLIMEVR_LEGACY 1
#define BOARD_SLIMEVR_DEV 2
#define BOARD_SLIMEVR_LEGACY 1 // More ancient development version of SlimeVR
#define BOARD_SLIMEVR_DEV 2 // Ancient development version of SlimeVR
#define BOARD_NODEMCU 3
#define BOARD_CUSTOM 4
#define BOARD_WROOM32 5
#define BOARD_WEMOSD1MINI 6
#define BOARD_TTGO_TBASE 7
#define BOARD_ESP01 8
#define BOARD_SLIMEVR 9
#define BOARD_SLIMEVR 9 // SlimeVR v1.0 & v1.1
#define BOARD_LOLIN_C3_MINI 10
#define BOARD_BEETLE32C3 11
#define BOARD_ES32C3DEVKITM1 12
#define BOARD_ESP32C3DEVKITM1 12
#define BOARD_OWOTRACK 13 // Only used by owoTrack mobile app
#define BOARD_WRANGLER 14 // Only used by wrangler app
#define BOARD_MOCOPI 15 // Used by mocopi/moslime
#define BOARD_WEMOSWROOM02 16
#define BOARD_XIAO_ESP32C3 17
#define BOARD_HARITORA 18 // Used by Haritora/SlimeTora
#define BOARD_ES32C6DEVKITC1 19
#define BOARD_ESP32C6DEVKITC1 19
#define BOARD_GLOVE_IMU_SLIMEVR_DEV 20 // IMU Glove
#define BOARD_GESTURES 21 // Used by Gestures
#define BOARD_SLIMEVR_V1_2 22 // SlimeVR v1.2
#define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
#define BAT_EXTERNAL 1

74
src/defines.h
View File

@@ -26,9 +26,9 @@
// ================================================
// Set parameters of IMU and board used
#define IMU IMU_BNO085
#define SECOND_IMU IMU
#define BOARD BOARD_SLIMEVR
#define IMU IMU_AUTO
#define SECOND_IMU IMU_AUTO
#define BOARD BOARD_SLIMEVR_V1_2
#define IMU_ROTATION DEG_270
#define SECOND_IMU_ROTATION DEG_270
@@ -54,6 +54,27 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
*/
#ifndef SENSOR_DESC_LIST
#if BOARD == BOARD_SLIMEVR_V1_2
#define SENSOR_DESC_LIST \
SENSOR_DESC_ENTRY( \
IMU, \
DIRECT_SPI(24'000'000, MSBFIRST, SPI_MODE3, DIRECT_PIN(15)), \
IMU_ROTATION, \
NO_WIRE, \
PRIMARY_IMU_OPTIONAL, \
DIRECT_PIN(PIN_IMU_INT), \
0 \
) \
SENSOR_DESC_ENTRY( \
SECOND_IMU, \
SECONDARY_IMU_ADDRESS_TWO, \
SECOND_IMU_ROTATION, \
DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \
SECONDARY_IMU_OPTIONAL, \
DIRECT_PIN(PIN_IMU_INT_2), \
0 \
)
#else
#define SENSOR_DESC_LIST \
SENSOR_DESC_ENTRY( \
IMU, \
@@ -74,14 +95,14 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
0 \
)
#endif
#endif
#else
// Predefines for the GLOVE
#ifndef SENSOR_DESC_LIST
#define MAX_SENSORS_COUNT 10
#define TRACKER_TYPE TrackerType::TRACKER_TYPE_SVR_GLOVE_LEFT
#define GLOVE_SIDE GloveSide::GLOVE_LEFT
#define GLOVE_SIDE GLOVE_LEFT
#define PRIMARY_IMU_ADDRESS_ONE 0x4a
#define SECONDARY_IMU_ADDRESS_TWO 0x4b
@@ -177,19 +198,19 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
0 \
)
#if GLOVE_SIDE == GloveSide::GLOVE_LEFT
#define SENSOR_INFO_LIST \
SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_LEFT_HAND) \
SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_LEFT_LITTLE_INTERMEDIATE) \
SENSOR_INFO_ENTRY(2, SensorPosition::POSITION_LEFT_RING_INTERMEDIATE) \
SENSOR_INFO_ENTRY(3, SensorPosition::POSITION_LEFT_RING_DISTAL) \
SENSOR_INFO_ENTRY(4, SensorPosition::POSITION_LEFT_MIDDLE_INTERMEDIATE) \
SENSOR_INFO_ENTRY(5, SensorPosition::POSITION_LEFT_MIDDLE_DISTAL) \
SENSOR_INFO_ENTRY(6, SensorPosition::POSITION_LEFT_INDEX_INTERMEDIATE) \
SENSOR_INFO_ENTRY(7, SensorPosition::POSITION_LEFT_INDEX_DISTAL) \
SENSOR_INFO_ENTRY(8, SensorPosition::SensorPosition::POSITION_LEFT_THUMB_PROXIMAL) \
#if GLOVE_SIDE == GLOVE_LEFT
#define SENSOR_INFO_LIST \
SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_LEFT_HAND) \
SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_LEFT_LITTLE_INTERMEDIATE) \
SENSOR_INFO_ENTRY(2, SensorPosition::POSITION_LEFT_RING_INTERMEDIATE) \
SENSOR_INFO_ENTRY(3, SensorPosition::POSITION_LEFT_RING_DISTAL) \
SENSOR_INFO_ENTRY(4, SensorPosition::POSITION_LEFT_MIDDLE_INTERMEDIATE) \
SENSOR_INFO_ENTRY(5, SensorPosition::POSITION_LEFT_MIDDLE_DISTAL) \
SENSOR_INFO_ENTRY(6, SensorPosition::POSITION_LEFT_INDEX_INTERMEDIATE) \
SENSOR_INFO_ENTRY(7, SensorPosition::POSITION_LEFT_INDEX_DISTAL) \
SENSOR_INFO_ENTRY(8, SensorPosition::POSITION_LEFT_THUMB_PROXIMAL) \
SENSOR_INFO_ENTRY(9, SensorPosition::POSITION_LEFT_THUMB_DISTAL)
#elif GLOVE_SDIE == GloveSide::GLOVE_RIGHT
#elif GLOVE_SDIE == GLOVE_RIGHT
#define SENSOR_INFO_LIST \
SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_RIGHT_HAND) \
SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_RIGHT_LITTLE_INTERMEDIATE) \
@@ -255,6 +276,23 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
#ifndef BATTERY_SHIELD_R2
#define BATTERY_SHIELD_R2 40.2
#endif
#elif BOARD == BOARD_SLIMEVR_V1_2
#define PIN_IMU_SDA 4
#define PIN_IMU_SCL 5
#define PIN_IMU_INT 2
#define PIN_IMU_INT_2 16
#define PIN_BATTERY_LEVEL 17
#define LED_PIN 2
#define LED_INVERTED true
#ifndef BATTERY_SHIELD_RESISTANCE
#define BATTERY_SHIELD_RESISTANCE 0
#endif
#ifndef BATTERY_SHIELD_R1
#define BATTERY_SHIELD_R1 10
#endif
#ifndef BATTERY_SHIELD_R2
#define BATTERY_SHIELD_R2 40.2
#endif
#elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV
#define PIN_IMU_SDA 4
#define PIN_IMU_SCL 5
@@ -331,7 +369,7 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
#define PIN_BATTERY_LEVEL 3
#define LED_PIN 10
#define LED_INVERTED false
#elif BOARD == BOARD_ES32C3DEVKITM1 || BOARD == BOARD_ES32C6DEVKITC1
#elif BOARD == BOARD_ESP32C3DEVKITM1 || BOARD == BOARD_ESP32C6DEVKITC1
#define PIN_IMU_SDA 5
#define PIN_IMU_SCL 4
#define PIN_IMU_INT 6

14
src/logging/Logger.cpp
View File

@@ -7,49 +7,49 @@ void Logger::setTag(const char* tag) {
strcpy(m_Tag, tag);
}
void Logger::trace(const char* format, ...) {
void Logger::trace(const char* format, ...) const {
va_list args;
va_start(args, format);
log(TRACE, format, args);
va_end(args);
}
void Logger::debug(const char* format, ...) {
void Logger::debug(const char* format, ...) const {
va_list args;
va_start(args, format);
log(DEBUG, format, args);
va_end(args);
}
void Logger::info(const char* format, ...) {
void Logger::info(const char* format, ...) const {
va_list args;
va_start(args, format);
log(INFO, format, args);
va_end(args);
}
void Logger::warn(const char* format, ...) {
void Logger::warn(const char* format, ...) const {
va_list args;
va_start(args, format);
log(WARN, format, args);
va_end(args);
}
void Logger::error(const char* format, ...) {
void Logger::error(const char* format, ...) const {
va_list args;
va_start(args, format);
log(ERROR, format, args);
va_end(args);
}
void Logger::fatal(const char* format, ...) {
void Logger::fatal(const char* format, ...) const {
va_list args;
va_start(args, format);
log(FATAL, format, args);
va_end(args);
}
void Logger::log(Level level, const char* format, va_list args) {
void Logger::log(Level level, const char* format, va_list args) const {
if (level < LOG_LEVEL) {
return;
}

28
src/logging/Logger.h
View File

@@ -27,48 +27,48 @@ public:
void setTag(const char* tag);
void trace(const char* str, ...);
void debug(const char* str, ...);
void info(const char* str, ...);
void warn(const char* str, ...);
void error(const char* str, ...);
void fatal(const char* str, ...);
void trace(const char* str, ...) const;
void debug(const char* str, ...) const;
void info(const char* str, ...) const;
void warn(const char* str, ...) const;
void error(const char* str, ...) const;
void fatal(const char* str, ...) const;
template <typename T>
inline void traceArray(const char* str, const T* array, size_t size) {
inline void traceArray(const char* str, const T* array, size_t size) const {
logArray(TRACE, str, array, size);
}
template <typename T>
inline void debugArray(const char* str, const T* array, size_t size) {
inline void debugArray(const char* str, const T* array, size_t size) const {
logArray(DEBUG, str, array, size);
}
template <typename T>
inline void infoArray(const char* str, const T* array, size_t size) {
inline void infoArray(const char* str, const T* array, size_t size) const {
logArray(INFO, str, array, size);
}
template <typename T>
inline void warnArray(const char* str, const T* array, size_t size) {
inline void warnArray(const char* str, const T* array, size_t size) const {
logArray(WARN, str, array, size);
}
template <typename T>
inline void errorArray(const char* str, const T* array, size_t size) {
inline void errorArray(const char* str, const T* array, size_t size) const {
logArray(ERROR, str, array, size);
}
template <typename T>
inline void fatalArray(const char* str, const T* array, size_t size) {
inline void fatalArray(const char* str, const T* array, size_t size) const {
logArray(FATAL, str, array, size);
}
private:
void log(Level level, const char* str, va_list args);
void log(Level level, const char* str, va_list args) const;
template <typename T>
void logArray(Level level, const char* str, const T* array, size_t size) {
void logArray(Level level, const char* str, const T* array, size_t size) const {
if (level < LOG_LEVEL) {
return;
}

2
src/main.cpp
View File

@@ -78,7 +78,7 @@ void setup() {
// this, check needs to be re-added.
auto clearResult = I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL);
if (clearResult != 0) {
logger.error("Can't clear I2C bus, error %d", clearResult);
logger.warn("Can't clear I2C bus, error %d", clearResult);
}
// join I2C bus

6
src/network/connection.h
View File

@@ -137,9 +137,9 @@ public:
bool endBundle();
private:
void updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors);
void updateSensorState(std::vector<std::unique_ptr<::Sensor>>& sensors);
void maybeRequestFeatureFlags();
bool isSensorStateUpdated(int i, std::unique_ptr<Sensor>& sensor);
bool isSensorStateUpdated(int i, std::unique_ptr<::Sensor>& sensor);
bool beginPacket();
bool endPacket();
@@ -209,7 +209,7 @@ private:
void sendTrackerDiscovery();
// PACKET_SENSOR_INFO 15
void sendSensorInfo(Sensor& sensor);
void sendSensorInfo(::Sensor& sensor);
void sendAcknowledgeConfigChange(uint8_t sensorId, SensorToggles configType);

45
src/sensorinterface/RegisterInterface.cpp Normal file
View File

@@ -0,0 +1,45 @@
/* 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 "RegisterInterface.h"
namespace SlimeVR::Sensors {
[[nodiscard]] uint8_t EmptyRegisterInterface::readReg(uint8_t regAddr) const {
return 0;
};
[[nodiscard]] uint16_t EmptyRegisterInterface::readReg16(uint8_t regAddr) const {
return 0;
};
void EmptyRegisterInterface::writeReg(uint8_t regAddr, uint8_t value) const {};
void EmptyRegisterInterface::writeReg16(uint8_t regAddr, uint16_t value) const {};
void EmptyRegisterInterface::readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer)
const {};
void EmptyRegisterInterface::writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer)
const {};
[[nodiscard]] uint8_t EmptyRegisterInterface::getAddress() const { return 0; };
bool EmptyRegisterInterface::hasSensorOnBus() { return true; };
[[nodiscard]] std::string EmptyRegisterInterface::toString() const { return "Empty"; };
EmptyRegisterInterface EmptyRegisterInterface::instance;
} // namespace SlimeVR::Sensors

59
src/sensorinterface/RegisterInterface.h Normal file
View File

@@ -0,0 +1,59 @@
/* SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Eiren Rain & 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 <cstdint>
#include <string>
#include "I2Cdev.h"
namespace SlimeVR::Sensors {
struct RegisterInterface {
static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2;
[[nodiscard]] virtual uint8_t readReg(uint8_t regAddr) const = 0;
[[nodiscard]] virtual uint16_t readReg16(uint8_t regAddr) const = 0;
virtual void writeReg(uint8_t regAddr, uint8_t value) const = 0;
virtual void writeReg16(uint8_t regAddr, uint16_t value) const = 0;
virtual void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0;
virtual void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0;
[[nodiscard]] virtual uint8_t getAddress() const = 0;
virtual bool hasSensorOnBus() = 0;
[[nodiscard]] virtual std::string toString() const = 0;
};
struct EmptyRegisterInterface : public RegisterInterface {
[[nodiscard]] uint8_t readReg(uint8_t regAddr) const final;
[[nodiscard]] uint16_t readReg16(uint8_t regAddr) const final;
void writeReg(uint8_t regAddr, uint8_t value) const final;
void writeReg16(uint8_t regAddr, uint16_t value) const final;
void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const final;
void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const final;
[[nodiscard]] uint8_t getAddress() const final;
bool hasSensorOnBus() final;
[[nodiscard]] std::string toString() const final;
static EmptyRegisterInterface instance;
};
} // namespace SlimeVR::Sensors

144
src/sensorinterface/SPIImpl.h Normal file
View File

@@ -0,0 +1,144 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Eiren Rain & 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 <PinInterface.h>
#include <SPI.h>
#include <cstdint>
#include "../logging/Logger.h"
#include "RegisterInterface.h"
#define ICM_READ_FLAG 0x80
namespace SlimeVR::Sensors {
struct SPIImpl : public RegisterInterface {
SPIImpl(SPIClass& spiClass, SPISettings spiSettings, PinInterface* csPin)
: m_spiClass(spiClass)
, m_spiSettings(spiSettings)
, m_csPin(csPin) {
m_Logger.info(
"SPI settings: clock: %d, bit order: 0x%02X, data mode: 0x%02X",
spiSettings._clock,
spiSettings._bitOrder,
spiSettings._dataMode
);
csPin->pinMode(OUTPUT);
csPin->digitalWrite(HIGH);
spiClass.begin();
}
uint8_t readReg(uint8_t regAddr) const override {
m_spiClass.beginTransaction(m_spiSettings);
m_csPin->digitalWrite(LOW);
m_spiClass.transfer(regAddr | ICM_READ_FLAG);
uint8_t buffer = m_spiClass.transfer(0);
m_csPin->digitalWrite(HIGH);
m_spiClass.endTransaction();
return buffer;
}
uint16_t readReg16(uint8_t regAddr) const override {
m_spiClass.beginTransaction(m_spiSettings);
m_csPin->digitalWrite(LOW);
m_spiClass.transfer(regAddr | ICM_READ_FLAG);
uint8_t b1 = m_spiClass.transfer(0);
uint8_t b2 = m_spiClass.transfer(0);
m_csPin->digitalWrite(HIGH);
m_spiClass.endTransaction();
return b2 << 8 | b1;
}
void writeReg(uint8_t regAddr, uint8_t value) const override {
m_spiClass.beginTransaction(m_spiSettings);
m_csPin->digitalWrite(LOW);
m_spiClass.transfer(regAddr);
m_spiClass.transfer(value);
m_csPin->digitalWrite(HIGH);
m_spiClass.endTransaction();
}
void writeReg16(uint8_t regAddr, uint16_t value) const override {
m_spiClass.beginTransaction(m_spiSettings);
m_csPin->digitalWrite(LOW);
m_spiClass.transfer(regAddr);
m_spiClass.transfer(value & 0xFF);
m_spiClass.transfer(value >> 8);
m_csPin->digitalWrite(HIGH);
m_spiClass.endTransaction();
}
void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
m_spiClass.beginTransaction(m_spiSettings);
m_csPin->digitalWrite(LOW);
;
m_spiClass.transfer(regAddr | ICM_READ_FLAG);
for (uint8_t i = 0; i < size; ++i) {
buffer[i] = m_spiClass.transfer(0);
}
m_csPin->digitalWrite(HIGH);
m_spiClass.endTransaction();
}
void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
m_spiClass.beginTransaction(m_spiSettings);
m_csPin->digitalWrite(LOW);
m_spiClass.transfer(regAddr);
for (uint8_t i = 0; i < size; ++i) {
m_spiClass.transfer(buffer[i]);
}
m_csPin->digitalWrite(HIGH);
m_spiClass.endTransaction();
}
bool hasSensorOnBus() override {
return true; // TODO
}
uint8_t getAddress() const override { return 0; }
std::string toString() const override { return std::string("SPI"); }
private:
SPIClass& m_spiClass;
SPISettings m_spiSettings;
PinInterface* m_csPin;
SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("SPI");
};
} // namespace SlimeVR::Sensors

4
src/sensorinterface/SensorInterfaceManager.h
View File

@@ -31,6 +31,8 @@
#include "I2CPCAInterface.h"
#include "I2CWireSensorInterface.h"
#include "MCP23X17PinInterface.h"
#include "SensorInterface.h"
#include "i2cimpl.h"
namespace SlimeVR {
@@ -77,7 +79,7 @@ private:
SensorInterface<DirectPinInterface, int> directPinInterfaces{[](int pin) {
return pin != 255 && pin != -1;
}};
SensorInterface<MCP23X17PinInterface, int> mcpPinInterfaces;
SensorInterface<MCP23X17PinInterface, Adafruit_MCP23X17*, int> mcpPinInterfaces;
SensorInterface<I2CWireSensorInterface, int, int> i2cWireInterfaces;
SensorInterface<I2CPCASensorInterface, int, int, int, int> pcaWireInterfaces;
};

31
src/sensors/softfusion/i2cimpl.h → src/sensorinterface/i2cimpl.h
View File

@@ -23,25 +23,26 @@
#pragma once
#include <i2cscan.h>
#include <cstdint>
#include "I2Cdev.h"
#include "RegisterInterface.h"
namespace SlimeVR::Sensors::SoftFusion {
struct I2CImpl {
static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2;
namespace SlimeVR::Sensors {
struct I2CImpl : public RegisterInterface {
I2CImpl(uint8_t devAddr)
: m_devAddr(devAddr) {}
uint8_t readReg(uint8_t regAddr) const {
uint8_t readReg(uint8_t regAddr) const override {
uint8_t buffer = 0;
I2Cdev::readByte(m_devAddr, regAddr, &buffer);
return buffer;
}
uint16_t readReg16(uint8_t regAddr) const {
uint16_t readReg16(uint8_t regAddr) const override {
uint16_t buffer = 0;
I2Cdev::readBytes(
m_devAddr,
@@ -52,11 +53,11 @@ struct I2CImpl {
return buffer;
}
void writeReg(uint8_t regAddr, uint8_t value) const {
void writeReg(uint8_t regAddr, uint8_t value) const override {
I2Cdev::writeByte(m_devAddr, regAddr, value);
}
void writeReg16(uint8_t regAddr, uint16_t value) const {
void writeReg16(uint8_t regAddr, uint16_t value) const override {
I2Cdev::writeBytes(
m_devAddr,
regAddr,
@@ -65,16 +66,24 @@ struct I2CImpl {
);
}
void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const {
void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
I2Cdev::readBytes(m_devAddr, regAddr, size, buffer);
}
void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const {
void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer);
}
bool hasSensorOnBus() { return I2CSCAN::hasDevOnBus(m_devAddr); }
uint8_t getAddress() const override { return m_devAddr; }
std::string toString() const {
return std::string("I2C(") + std::to_string(m_devAddr) + std::string(")");
}
private:
uint8_t m_devAddr;
};
} // namespace SlimeVR::Sensors::SoftFusion
} // namespace SlimeVR::Sensors

3
src/sensors/ADCResistanceSensor.cpp
View File

@@ -24,6 +24,7 @@
#include "GlobalVars.h"
namespace SlimeVR::Sensors {
void ADCResistanceSensor::motionLoop() {
#if ESP8266
float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution;
@@ -39,3 +40,5 @@ void ADCResistanceSensor::motionLoop() {
void ADCResistanceSensor::sendData() {
networkConnection.sendFlexData(sensorId, m_Data);
}
} // namespace SlimeVR::Sensors

6
src/sensors/ADCResistanceSensor.h
View File

@@ -22,9 +22,10 @@
*/
#pragma once
#include "../sensorinterface/RegisterInterface.h"
#include "sensor.h"
#include "sensorinterface/SensorInterface.h"
namespace SlimeVR::Sensors {
class ADCResistanceSensor : Sensor {
public:
static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE;
@@ -40,7 +41,7 @@ public:
"ADCResistanceSensor",
SensorTypeID::ADC_RESISTANCE,
id,
pin,
EmptyRegisterInterface::instance,
0.0f,
new SlimeVR::EmptySensorInterface
)
@@ -67,3 +68,4 @@ private:
float m_Data = 0.0f;
};
} // namespace SlimeVR::Sensors

15
src/sensors/EmptySensor.h
View File

@@ -24,14 +24,20 @@
#ifndef SENSORS_EMPTYSENSOR_H
#define SENSORS_EMPTYSENSOR_H
#include "../sensorinterface/RegisterInterface.h"
#include "sensor.h"
namespace SlimeVR {
namespace Sensors {
namespace SlimeVR::Sensors {
class EmptySensor : public Sensor {
public:
EmptySensor(uint8_t id)
: Sensor("EmptySensor", SensorTypeID::Empty, id, 0, 0.0){};
: Sensor(
"EmptySensor",
SensorTypeID::Empty,
id,
EmptyRegisterInterface::instance,
0.0
){};
~EmptySensor(){};
void motionSetup() override final{};
@@ -42,7 +48,6 @@ public:
return SensorStatus::SENSOR_OFFLINE;
};
};
} // namespace Sensors
} // namespace SlimeVR
} // namespace SlimeVR::Sensors
#endif

9
src/sensors/ErroneousSensor.h
View File

@@ -24,14 +24,14 @@
#ifndef SENSORS_ERRONEOUSSENSOR_H
#define SENSORS_ERRONEOUSSENSOR_H
#include "../sensorinterface/RegisterInterface.h"
#include "sensor.h"
namespace SlimeVR {
namespace Sensors {
namespace SlimeVR::Sensors {
class ErroneousSensor : public Sensor {
public:
ErroneousSensor(uint8_t id, SensorTypeID type)
: Sensor("ErroneousSensor", type, id, 0, 0.0)
: Sensor("ErroneousSensor", type, id, EmptyRegisterInterface::instance, 0.0)
, m_ExpectedType(type){};
~ErroneousSensor(){};
@@ -44,7 +44,6 @@ public:
private:
SensorTypeID m_ExpectedType;
};
} // namespace Sensors
} // namespace SlimeVR
} // namespace SlimeVR::Sensors
#endif

311
src/sensors/SensorBuilder.cpp Normal file
View File

@@ -0,0 +1,311 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Eiren Rain, 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 "SensorBuilder.h"
namespace SlimeVR::Sensors {
SensorBuilder::SensorBuilder(SensorManager* sensorManager)
: m_Manager(sensorManager) {}
uint8_t SensorBuilder::buildAllSensors() {
SensorInterfaceManager interfaceManager;
uint8_t sensorID = 0;
uint8_t activeSensorCount = 0;
#define NO_PIN nullptr
#define NO_WIRE new EmptySensorInterface
#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin)
#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda)
#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(&m_Manager->m_MCP, pin)
#define PCA_WIRE(scl, sda, addr, ch) \
interfaceManager.pcaWireInterface().get(scl, sda, addr, ch)
#define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \
*(new SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN))
#define SENSOR_DESC_ENTRY(ImuType, ...) \
{ \
do { \
std::unique_ptr<::Sensor> sensor; \
if constexpr (std::is_same<ImuType, SensorAuto>::value) { \
auto sensorType = findSensorType(sensorID, __VA_ARGS__); \
if (sensorType == SensorTypeID::Unknown) { \
m_Manager->m_Logger.error( \
"Can't find sensor type for sensor %d", \
sensorID \
); \
break; \
} else { \
m_Manager->m_Logger.info( \
"Sensor %d automatically detected with %s", \
sensorID, \
getIMUNameByType(sensorType) \
); \
sensor \
= buildSensorDynamically(sensorType, sensorID, __VA_ARGS__); \
} \
} else { \
sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__); \
} \
if (sensor->isWorking()) { \
m_Manager->m_Logger.info("Sensor %d configured", sensorID); \
activeSensorCount++; \
} \
m_Manager->m_Sensors.push_back(std::move(sensor)); \
} while (false); \
sensorID++; \
}
// Apply descriptor list and expand to entries
SENSOR_DESC_LIST;
#define SENSOR_INFO_ENTRY(ImuID, ...) \
{ m_Manager->m_Sensors[ImuID]->setSensorInfo(__VA_ARGS__); }
// Apply sensor info list
SENSOR_INFO_LIST;
return activeSensorCount;
}
#define BUILD_SENSOR_ARGS \
sensorID, imuInterface, rotation, sensorInterface, optional, intPin, extraParam
std::unique_ptr<::Sensor> SensorBuilder::buildSensorDynamically(
SensorTypeID type,
uint8_t sensorID,
RegisterInterface& imuInterface,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
) {
switch (type) {
// case SensorTypeID::MPU9250:
// return buildSensor<MPU9250Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::BNO080:
// return buildSensor<BNO080Sensor>(BUILD_SENSOR_ARGS);
case SensorTypeID::BNO085:
return buildSensor<BNO085Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::BNO055:
// return buildSensor<BNO055Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::MPU6050:
// return buildSensor<SoftFusionMPU6050>(
// BUILD_SENSOR_ARGS
// );
// case SensorTypeID::BNO086:
// return buildSensor<BNO086Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::BMI160:
// return buildSensor<BMI160Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::ICM20948:
// return buildSensor<ICM20948Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::ICM42688:
// return buildSensor<SoftFusionICM42688>(
// BUILD_SENSOR_ARGS
// );
case SensorTypeID::BMI270:
return buildSensor<SoftFusionBMI270>(BUILD_SENSOR_ARGS);
// case SensorTypeID::LSM6DS3TRC:
// return buildSensor<SoftFusionLSM6DS3TRC>(
// BUILD_SENSOR_ARGS
// );
case SensorTypeID::LSM6DSV:
return buildSensor<SoftFusionLSM6DSV>(BUILD_SENSOR_ARGS);
// case SensorTypeID::LSM6DSO:
// return buildSensor<SoftFusionLSM6DSO>(
// BUILD_SENSOR_ARGS
// );
// case SensorTypeID::LSM6DSR:
// return buildSensor<SoftFusionLSM6DSR>(
// BUILD_SENSOR_ARGS
// );
case SensorTypeID::ICM45686:
return buildSensor<SoftFusionICM45686>(BUILD_SENSOR_ARGS);
// case SensorTypeID::ICM45605:
// return buildSensor<SoftFusionICM45605>(
// BUILD_SENSOR_ARGS
// );
default:
m_Manager->m_Logger.error(
"Unable to create sensor with type %s (%d)",
getIMUNameByType(type),
type
);
}
return std::make_unique<EmptySensor>(sensorID);
}
std::unique_ptr<::Sensor> SensorBuilder::buildSensorDynamically(
SensorTypeID type,
uint8_t sensorID,
uint8_t imuInterface,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
) {
switch (type) {
// case SensorTypeID::MPU9250:
// return buildSensor<MPU9250Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::BNO080:
// return buildSensor<BNO080Sensor>(BUILD_SENSOR_ARGS);
case SensorTypeID::BNO085:
return buildSensor<BNO085Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::BNO055:
// return buildSensor<BNO055Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::MPU6050:
// return buildSensor<SoftFusionMPU6050>(
// BUILD_SENSOR_ARGS
// );
// case SensorTypeID::BNO086:
// return buildSensor<BNO086Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::BMI160:
// return buildSensor<BMI160Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::ICM20948:
// return buildSensor<ICM20948Sensor>(BUILD_SENSOR_ARGS);
// case SensorTypeID::ICM42688:
// return buildSensor<SoftFusionICM42688>(
// BUILD_SENSOR_ARGS
// );
case SensorTypeID::BMI270:
return buildSensor<SoftFusionBMI270>(BUILD_SENSOR_ARGS);
// case SensorTypeID::LSM6DS3TRC:
// return buildSensor<SoftFusionLSM6DS3TRC>(
// BUILD_SENSOR_ARGS
// );
case SensorTypeID::LSM6DSV:
return buildSensor<SoftFusionLSM6DSV>(BUILD_SENSOR_ARGS);
// case SensorTypeID::LSM6DSO:
// return buildSensor<SoftFusionLSM6DSO>(
// BUILD_SENSOR_ARGS
// );
// case SensorTypeID::LSM6DSR:
// return buildSensor<SoftFusionLSM6DSR>(
// BUILD_SENSOR_ARGS
// );
case SensorTypeID::ICM45686:
return buildSensor<SoftFusionICM45686>(BUILD_SENSOR_ARGS);
// case SensorTypeID::ICM45605:
// return buildSensor<SoftFusionICM45605>(
// BUILD_SENSOR_ARGS
// );
default:
m_Manager->m_Logger.error(
"Unable to create sensor with type %s (%d)",
getIMUNameByType(type),
type
);
}
return std::make_unique<EmptySensor>(sensorID);
}
SensorTypeID SensorBuilder::findSensorType(
uint8_t sensorID,
uint8_t imuAddress,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
) {
sensorInterface->init();
sensorInterface->swapIn();
// if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuAddress))
// { return SensorTypeID::LSM6DS3TRC;
// }
// if (SoftFusionICM42688::checkPresent(sensorID, imuAddress)) {
// return SensorTypeID::ICM42688;
//}
if (SoftFusionBMI270::checkPresent(sensorID, imuAddress)) {
return SensorTypeID::BMI270;
}
if (SoftFusionLSM6DSV::checkPresent(sensorID, imuAddress)) {
return SensorTypeID::LSM6DSV;
}
// if (SoftFusionLSM6DSO::checkPresent(sensorID, imuAddress)) {
// return SensorTypeID::LSM6DSO;
// }
// if (SoftFusionLSM6DSR::checkPresent(sensorID, imuAddress)) {
// return SensorTypeID::LSM6DSR;
// }
// if (SoftFusionMPU6050::checkPresent(sensorID, imuAddress)) {
// return SensorTypeID::MPU6050;
// }
if (SoftFusionICM45686::checkPresent(sensorID, imuAddress)) {
return SensorTypeID::ICM45686;
}
return BNO080Sensor::checkPresent(sensorID, imuAddress, intPin);
// if (SoftFusionICM45605::checkPresent(sensorID, imuAddress)) {
// return SensorTypeID::ICM45605;
// }
return SensorTypeID::Unknown;
}
SensorTypeID SensorBuilder::findSensorType(
uint8_t sensorID,
RegisterInterface& imuInterface,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
) {
sensorInterface->init();
sensorInterface->swapIn();
// if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuInterface))
// { return SensorTypeID::LSM6DS3TRC;
// }
// if (SoftFusionICM42688::checkPresent(sensorID, imuInterface)) {
// return SensorTypeID::ICM42688;
//}
if (SoftFusionBMI270::checkPresent(sensorID, imuInterface)) {
return SensorTypeID::BMI270;
}
if (SoftFusionLSM6DSV::checkPresent(sensorID, imuInterface)) {
return SensorTypeID::LSM6DSV;
}
// if (SoftFusionLSM6DSO::checkPresent(sensorID, imuInterface)) {
// return SensorTypeID::LSM6DSO;
// }
// if (SoftFusionLSM6DSR::checkPresent(sensorID, imuInterface)) {
// return SensorTypeID::LSM6DSR;
// }
// if (SoftFusionMPU6050::checkPresent(sensorID, imuInterface)) {
// return SensorTypeID::MPU6050;
// }
if (SoftFusionICM45686::checkPresent(sensorID, imuInterface)) {
return SensorTypeID::ICM45686;
}
return BNO080Sensor::checkPresent(sensorID, sensorInterface, intPin);
// if (SoftFusionICM45605::checkPresent(sensorID, imuInterface)) {
// return SensorTypeID::ICM45605;
// }
// return SensorTypeID::Unknown;
}
} // namespace SlimeVR::Sensors

235
src/sensors/SensorBuilder.h Normal file
View File

@@ -0,0 +1,235 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Eiren Rain & 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 <map>
#include <memory>
#include <optional>
#include <type_traits>
#include "EmptySensor.h"
#include "ErroneousSensor.h"
#include "SensorManager.h"
#include "bmi160sensor.h"
#include "bno055sensor.h"
#include "bno080sensor.h"
#include "globals.h"
#include "icm20948sensor.h"
#include "logging/Logger.h"
#include "mpu6050sensor.h"
#include "mpu9250sensor.h"
#include "sensor.h"
#include "sensorinterface/DirectPinInterface.h"
#include "sensorinterface/I2CPCAInterface.h"
#include "sensorinterface/I2CWireSensorInterface.h"
#include "sensorinterface/MCP23X17PinInterface.h"
#include "sensorinterface/RegisterInterface.h"
#include "sensorinterface/SPIImpl.h"
#include "sensorinterface/SensorInterfaceManager.h"
#include "sensorinterface/i2cimpl.h"
#include "softfusion/drivers/bmi270.h"
#include "softfusion/drivers/icm42688.h"
#include "softfusion/drivers/icm45605.h"
#include "softfusion/drivers/icm45686.h"
#include "softfusion/drivers/lsm6ds3trc.h"
#include "softfusion/drivers/lsm6dso.h"
#include "softfusion/drivers/lsm6dsr.h"
#include "softfusion/drivers/lsm6dsv.h"
#include "softfusion/drivers/mpu6050.h"
#include "softfusion/softfusionsensor.h"
#ifndef PRIMARY_IMU_ADDRESS_ONE
#define PRIMARY_IMU_ADDRESS_ONE 0
#endif
#ifndef SECONDARY_IMU_ADDRESS_TWO
#define SECONDARY_IMU_ADDRESS_TWO 0
#endif
#if USE_RUNTIME_CALIBRATION
#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator
#else
#include "sensors/softfusion/SoftfusionCalibration.h"
#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator
#endif
#if ESP32
#include "driver/i2c.h"
#endif
namespace SlimeVR::Sensors {
using SoftFusionLSM6DS3TRC
= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SFCALIBRATOR>;
using SoftFusionICM42688
= SoftFusionSensor<SoftFusion::Drivers::ICM42688, SFCALIBRATOR>;
using SoftFusionBMI270 = SoftFusionSensor<SoftFusion::Drivers::BMI270, SFCALIBRATOR>;
using SoftFusionLSM6DSV = SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SFCALIBRATOR>;
using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SFCALIBRATOR>;
using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SFCALIBRATOR>;
using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SFCALIBRATOR>;
using SoftFusionICM45686
= SoftFusionSensor<SoftFusion::Drivers::ICM45686, SFCALIBRATOR>;
using SoftFusionICM45605
= SoftFusionSensor<SoftFusion::Drivers::ICM45605, SFCALIBRATOR>;
class SensorAuto {};
struct SensorBuilder {
public:
SensorManager* m_Manager;
SensorBuilder(SensorManager* sensorManager);
uint8_t buildAllSensors();
std::unique_ptr<::Sensor> buildSensorDynamically(
SensorTypeID type,
uint8_t sensorID,
RegisterInterface& imuInterface,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
);
std::unique_ptr<::Sensor> buildSensorDynamically(
SensorTypeID type,
uint8_t sensorID,
uint8_t imuInterface,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
);
SensorTypeID findSensorType(
uint8_t sensorID,
uint8_t imuAddress,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
);
SensorTypeID findSensorType(
uint8_t sensorID,
RegisterInterface& imuInterface,
float rotation,
SensorInterface* sensorInterface,
bool optional,
PinInterface* intPin,
int extraParam
);
template <typename ImuType>
std::unique_ptr<::Sensor> buildSensor(
uint8_t sensorID,
RegisterInterface& imuInterface,
float rotation,
SensorInterface* sensorInterface,
bool optional = false,
PinInterface* intPin = nullptr,
int extraParam = 0
) {
m_Manager->m_Logger.trace(
"Building IMU with: id=%d,\n\
address=%s, rotation=%f,\n\
interface=%s, int=%s, extraParam=%d, optional=%d",
sensorID,
imuInterface.toString(),
rotation,
sensorInterface,
intPin,
extraParam,
optional
);
// Now start detecting and building the IMU
std::unique_ptr<::Sensor> sensor;
// Init I2C bus for each sensor upon startup
sensorInterface->init();
sensorInterface->swapIn();
if (!imuInterface.hasSensorOnBus()) {
if (!optional) {
m_Manager->m_Logger.error(
"Mandatory sensor %d not found at address %s",
sensorID + 1,
imuInterface.toString()
);
return std::make_unique<ErroneousSensor>(sensorID, ImuType::TypeID);
} else {
m_Manager->m_Logger.debug(
"Optional sensor %d not found at address %s",
sensorID + 1,
imuInterface.toString()
);
return std::make_unique<EmptySensor>(sensorID);
}
}
m_Manager->m_Logger.trace(
"Sensor %d found at address %s",
sensorID + 1,
imuInterface.toString()
);
sensor = std::make_unique<ImuType>(
sensorID,
imuInterface,
rotation,
sensorInterface,
intPin,
extraParam
);
sensor->motionSetup();
return sensor;
}
template <typename ImuType>
std::unique_ptr<::Sensor> buildSensor(
uint8_t sensorID,
uint8_t imuAddress,
float rotation,
SensorInterface* sensorInterface,
bool optional = false,
PinInterface* intPin = nullptr,
int extraParam = 0
) {
uint8_t address = imuAddress > 0 ? imuAddress : ImuType::Address + sensorID;
return buildSensor<ImuType>(
sensorID,
*(new I2CImpl(address)),
rotation,
sensorInterface,
optional,
intPin,
extraParam
);
}
};
} // namespace SlimeVR::Sensors

101
src/sensors/SensorManager.cpp
View File

@@ -23,107 +23,18 @@
#include "SensorManager.h"
#include <map>
#include <type_traits>
#include "SensorBuilder.h"
#include "bmi160sensor.h"
#include "bno055sensor.h"
#include "bno080sensor.h"
#include "icm20948sensor.h"
#include "mpu6050sensor.h"
#include "mpu9250sensor.h"
#include "sensorinterface/SensorInterfaceManager.h"
#include "sensors/softfusion/SoftfusionCalibration.h"
#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
#include "softfusion/drivers/bmi270.h"
#include "softfusion/drivers/icm42688.h"
#include "softfusion/drivers/icm45605.h"
#include "softfusion/drivers/icm45686.h"
#include "softfusion/drivers/lsm6ds3trc.h"
#include "softfusion/drivers/lsm6dso.h"
#include "softfusion/drivers/lsm6dsr.h"
#include "softfusion/drivers/lsm6dsv.h"
#include "softfusion/drivers/mpu6050.h"
#include "softfusion/i2cimpl.h"
#include "softfusion/softfusionsensor.h"
#if ESP32
#include "driver/i2c.h"
#endif
#if USE_RUNTIME_CALIBRATION
#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator
#else
#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator
#endif
namespace SlimeVR {
namespace Sensors {
using SoftFusionLSM6DS3TRC = SoftFusionSensor<
SoftFusion::Drivers::LSM6DS3TRC,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
using SoftFusionICM42688 = SoftFusionSensor<
SoftFusion::Drivers::ICM42688,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
using SoftFusionBMI270
= SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionLSM6DSV
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionLSM6DSO
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionLSM6DSR
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionMPU6050
= SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionICM45686 = SoftFusionSensor<
SoftFusion::Drivers::ICM45686,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
using SoftFusionICM45605 = SoftFusionSensor<
SoftFusion::Drivers::ICM45605,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
namespace SlimeVR::Sensors {
void SensorManager::setup() {
SensorInterfaceManager interfaceManager;
uint8_t sensorID = 0;
uint8_t activeSensorCount = 0;
if (m_MCP.begin_I2C()) {
m_Logger.info("MCP initialized");
}
#define NO_PIN nullptr
#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin)
#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda)
#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin)
#define PCA_WIRE(scl, sda, addr, ch) \
interfaceManager.pcaWireInterface().get(scl, sda, addr, ch);
SensorBuilder sensorBuilder = SensorBuilder(this);
uint8_t activeSensorCount = sensorBuilder.buildAllSensors();
#define SENSOR_DESC_ENTRY(ImuType, ...) \
{ \
auto sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__); \
if (sensor->isWorking()) { \
m_Logger.info("Sensor %d configured", sensorID + 1); \
activeSensorCount++; \
} \
m_Sensors.push_back(std::move(sensor)); \
sensorID++; \
}
// Apply descriptor list and expand to entries
SENSOR_DESC_LIST;
#define SENSOR_INFO_ENTRY(ImuID, ...) \
{ m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); }
SENSOR_INFO_LIST;
#undef SENSOR_DESC_ENTRY
#undef NO_PIN
#undef DIRECT_PIN
#undef DIRECT_WIRE
m_Logger.info("%d sensor(s) configured", activeSensorCount);
// Check and scan i2c if no sensors active
if (activeSensorCount == 0) {
@@ -209,5 +120,5 @@ void SensorManager::update() {
networkConnection.endBundle();
#endif
}
} // namespace Sensors
} // namespace SlimeVR
} // namespace SlimeVR::Sensors

102
src/sensors/SensorManager.h
View File

@@ -31,31 +31,14 @@
#include "ErroneousSensor.h"
#include "globals.h"
#include "logging/Logger.h"
#include "sensor.h"
#include "sensoraddress.h"
#include "sensorinterface/DirectPinInterface.h"
#include "sensorinterface/I2CPCAInterface.h"
#include "sensorinterface/I2CWireSensorInterface.h"
#include "sensorinterface/MCP23X17PinInterface.h"
#include "sensorinterface/RegisterInterface.h"
#include "sensorinterface/i2cimpl.h"
namespace SlimeVR {
namespace Sensors {
#ifndef PRIMARY_IMU_ADDRESS_ONE
#define PRIMARY_IMU_ADDRESS_ONE true
#endif
#ifndef PRIMARY_IMU_ADDRESS_TWO
#define PRIMARY_IMU_ADDRESS_TWO false
#endif
#ifndef SECONDARY_IMU_ADDRESS_TWO
#define SECONDARY_IMU_ADDRESS_TWO false
#endif
#ifndef SECONDARY_IMU_ADDRESS_ONE
#define SECONDARY_IMU_ADDRESS_ONE true
#endif
namespace SlimeVR::Sensors {
class SensorManager {
public:
@@ -80,81 +63,8 @@ private:
std::vector<std::unique_ptr<::Sensor>> m_Sensors;
Adafruit_MCP23X17 m_MCP;
template <typename ImuType>
std::unique_ptr<::Sensor> buildSensor(
uint8_t sensorID,
ImuAddress imuAddress,
float rotation,
SensorInterface* sensorInterface,
bool optional = false,
PinInterface* intPin = nullptr,
int extraParam = 0
) {
uint8_t i2cAddress = imuAddress.getAddress(ImuType::Address);
m_Logger.trace(
"Building IMU with: id=%d,\n\
address=0x%02X, rotation=%f,\n\
interface=%s, int=%s, extraParam=%d, optional=%d",
sensorID,
i2cAddress,
rotation,
sensorInterface,
intPin,
extraParam,
optional
);
// Now start detecting and building the IMU
std::unique_ptr<::Sensor> sensor;
// Init I2C bus for each sensor upon startup
sensorInterface->init();
sensorInterface->swapIn();
if (I2CSCAN::hasDevOnBus(i2cAddress)) {
m_Logger
.trace("Sensor %d found at address 0x%02X", sensorID + 1, i2cAddress);
} else if (I2CSCAN::hasDevOnBus(i2cAddress
)) { // scan again because ICM45* may not respond on first I2C
// transaction
m_Logger.trace(
"Sensor %d found at address 0x%02X on second scan",
sensorID + 1,
i2cAddress
);
} else {
if (!optional) {
m_Logger.error(
"Mandatory sensor %d not found at address 0x%02X",
sensorID + 1,
i2cAddress
);
sensor = std::make_unique<ErroneousSensor>(sensorID, ImuType::TypeID);
} else {
m_Logger.debug(
"Optional sensor %d not found at address 0x%02X",
sensorID + 1,
i2cAddress
);
sensor = std::make_unique<EmptySensor>(sensorID);
}
return sensor;
}
sensor = std::make_unique<ImuType>(
sensorID,
i2cAddress,
rotation,
sensorInterface,
intPin,
extraParam
);
sensor->motionSetup();
return sensor;
}
uint32_t m_LastBundleSentAtMicros = micros();
friend class SensorBuilder;
};
} // namespace Sensors
} // namespace SlimeVR
} // namespace SlimeVR::Sensors

4
src/sensors/bmi160sensor.h
View File

@@ -139,7 +139,7 @@ public:
BMI160Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface*,
@@ -149,7 +149,7 @@ public:
"BMI160Sensor",
SensorTypeID::BMI160,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface
)

4
src/sensors/bno055sensor.h
View File

@@ -35,7 +35,7 @@ public:
BNO055Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface*,
@@ -45,7 +45,7 @@ public:
"BNO055Sensor",
SensorTypeID::BNO055,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface
){};

41
src/sensors/bno080sensor.h
View File

@@ -25,6 +25,7 @@
#define SENSORS_BNO080SENSOR_H
#include <BNO080.h>
#include <i2cscan.h>
#include "sensor.h"
@@ -37,7 +38,7 @@ public:
BNO080Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface* intPin,
@@ -47,7 +48,7 @@ public:
"BNO080Sensor",
SensorTypeID::BNO080,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface
)
@@ -63,19 +64,43 @@ public:
bool isFlagSupported(SensorToggles toggle) const final;
void sendTempIfNeeded();
static SensorTypeID checkPresent(
uint8_t sensorID,
SlimeVR::SensorInterface* sensorInterface,
PinInterface* intPin
) {
// Lazy check for if BNO is present, we only check if I2C has an address here
if (I2CSCAN::hasDevOnBus(Address + sensorID)) {
return SensorTypeID::BNO085; // Assume it's 085, more precise diff will
// require talking to it
}
return SensorTypeID::Unknown;
}
static SensorTypeID
checkPresent(uint8_t sensorID, uint8_t imuAddress, PinInterface* intPin) {
uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID;
// Lazy check for if BNO is present, we only check if I2C has an address here
if (I2CSCAN::hasDevOnBus(address)) {
return SensorTypeID::BNO085; // Assume it's 085, more precise diff will
// require talking to it
}
return SensorTypeID::Unknown;
}
protected:
// forwarding constructor
BNO080Sensor(
const char* sensorName,
SensorTypeID imuId,
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface* intPin,
int
)
: Sensor(sensorName, imuId, id, i2cAddress, rotation, sensorInterface)
: Sensor(sensorName, imuId, id, registerInterface, rotation, sensorInterface)
, m_IntPin(intPin){};
private:
@@ -107,7 +132,7 @@ public:
static constexpr auto TypeID = SensorTypeID::BNO085;
BNO085Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface* intPin,
@@ -117,7 +142,7 @@ public:
"BNO085Sensor",
SensorTypeID::BNO085,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface,
intPin,
@@ -130,7 +155,7 @@ public:
static constexpr auto TypeID = SensorTypeID::BNO086;
BNO086Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface* intPin,
@@ -140,7 +165,7 @@ public:
"BNO086Sensor",
SensorTypeID::BNO086,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface,
intPin,

4
src/sensors/icm20948sensor.h
View File

@@ -35,7 +35,7 @@ public:
ICM20948Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface*,
@@ -45,7 +45,7 @@ public:
"ICM20948Sensor",
SensorTypeID::ICM20948,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface
) {}

4
src/sensors/mpu6050sensor.h
View File

@@ -36,7 +36,7 @@ public:
MPU6050Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface*,
@@ -46,7 +46,7 @@ public:
"MPU6050Sensor",
SensorTypeID::MPU6050,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface
){};

4
src/sensors/mpu9250sensor.h
View File

@@ -49,7 +49,7 @@ public:
MPU9250Sensor(
uint8_t id,
uint8_t i2cAddress,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
PinInterface*,
@@ -59,7 +59,7 @@ public:
"MPU9250Sensor",
SensorTypeID::MPU9250,
id,
i2cAddress,
registerInterface,
rotation,
sensorInterface
)

1
src/sensors/sensor.cpp
View File

@@ -33,6 +33,7 @@ SensorStatus Sensor::getSensorState() {
void Sensor::setAcceleration(Vector3 a) {
acceleration = a;
sensorOffset.sandwich(acceleration);
newAcceleration = true;
}

10
src/sensors/sensor.h
View File

@@ -35,7 +35,9 @@
#include "configuration/Configuration.h"
#include "globals.h"
#include "logging/Logger.h"
#include "sensorinterface/RegisterInterface.h"
#include "sensorinterface/SensorInterface.h"
#include "sensorinterface/i2cimpl.h"
#include "status/TPSCounter.h"
#include "utils.h"
@@ -54,12 +56,12 @@ public:
const char* sensorName,
SensorTypeID type,
uint8_t id,
uint8_t address,
SlimeVR::Sensors::RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface = nullptr
)
: m_hwInterface(sensorInterface)
, addr(address)
, m_RegisterInterface(registerInterface)
, sensorId(id)
, sensorType(type)
, sensorOffset({Quat(Vector3(0, 0, 1), rotation)})
@@ -67,6 +69,7 @@ public:
char buf[4];
sprintf(buf, "%u", id);
m_Logger.setTag(buf);
addr = registerInterface.getAddress();
}
virtual ~Sensor(){};
@@ -112,7 +115,8 @@ public:
SlimeVR::SensorInterface* m_hwInterface = nullptr;
protected:
uint8_t addr = 0;
SlimeVR::Sensors::RegisterInterface& m_RegisterInterface;
uint8_t addr;
uint8_t sensorId = 0;
SensorTypeID sensorType = SensorTypeID::Unknown;
bool working = false;

4
src/sensors/sensorposition.h
View File

@@ -76,4 +76,6 @@ enum class SensorPosition : uint8_t {
POSITION_RIGHT_LITTLE_DISTAL = 50
};
enum class GloveSide { NO_GLOVE = 0, GLOVE_LEFT = 1, GLOVE_RIGHT = 2 };
#define NO_GLOVE 0
#define GLOVE_LEFT 1
#define GLOVE_RIGHT 2

123
src/sensors/softfusion/drivers/bmi270.h
View File

@@ -29,6 +29,7 @@
#include <cstring>
#include <limits>
#include "../../../sensorinterface/RegisterInterface.h"
#include "bmi270fw.h"
#include "vqf.h"
@@ -39,7 +40,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Gyroscope ODR = 400Hz, accel ODR = 100Hz
// Timestamps reading are not used
template <typename I2CImpl>
struct BMI270 {
static constexpr uint8_t Address = 0x68;
static constexpr auto Name = "BMI270";
@@ -68,13 +68,13 @@ struct BMI270 {
uint8_t x, y, z;
};
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
int8_t zxFactor;
BMI270(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: i2c(i2c)
, logger(logger)
, zxFactor(0) {}
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;
int8_t m_zxFactor;
BMI270(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: m_RegisterInterface(registerInterface)
, m_Logger(logger)
, m_zxFactor(0) {}
struct Regs {
struct WhoAmI {
@@ -244,14 +244,20 @@ struct BMI270 {
bool restartAndInit() {
// perform initialization step
i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset);
m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset);
delay(12);
// disable power saving
i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving);
m_RegisterInterface.writeReg(
Regs::PwrConf::reg,
Regs::PwrConf::valueNoPowerSaving
);
delay(1);
// firmware upload
i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit);
m_RegisterInterface.writeReg(
Regs::InitCtrl::reg,
Regs::InitCtrl::valueStartInit
);
for (uint16_t pos = 0; pos < sizeof(bmi270_firmware);) {
// tell the device current position
@@ -261,57 +267,63 @@ struct BMI270 {
const uint16_t pos_words = pos >> 1; // convert current position to words
const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00);
i2c.writeReg16(Regs::InitAddr, position);
m_RegisterInterface.writeReg16(Regs::InitAddr, position);
// write actual payload chunk
const uint16_t burstWrite = std::min(
sizeof(bmi270_firmware) - pos,
I2CImpl::MaxTransactionLength
static_cast<size_t>(sizeof(bmi270_firmware) - pos),
RegisterInterface::MaxTransactionLength
);
i2c.writeBytes(
m_RegisterInterface.writeBytes(
Regs::InitData,
burstWrite,
const_cast<uint8_t*>(bmi270_firmware + pos)
);
pos += burstWrite;
}
i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit);
m_RegisterInterface.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit);
delay(140);
// leave fifo_self_wakeup enabled
i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup);
m_RegisterInterface.writeReg(
Regs::PwrConf::reg,
Regs::PwrConf::valueFifoSelfWakeup
);
// check if IMU initialized correctly
if (!(i2c.readReg(Regs::InternalStatus::reg)
if (!(m_RegisterInterface.readReg(Regs::InternalStatus::reg)
& Regs::InternalStatus::initializedBit)) {
// firmware upload fail or sensor not initialized
return false;
}
// read zx factor used to reduce gyro cross-sensitivity error
const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas);
const uint8_t zx_factor_reg = m_RegisterInterface.readReg(Regs::RaGyrCas);
const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80;
zxFactor = static_cast<int8_t>(zx_factor_reg | sign_byte);
m_zxFactor = static_cast<int8_t>(zx_factor_reg | sign_byte);
return true;
}
void setNormalConfig(MotionlessCalibrationData& gyroSensitivity) {
i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value);
i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value);
m_RegisterInterface.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value);
m_RegisterInterface.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value);
i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value);
i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value);
m_RegisterInterface.writeReg(Regs::AccConf::reg, Regs::AccConf::value);
m_RegisterInterface.writeReg(Regs::AccRange::reg, Regs::AccRange::value);
if (gyroSensitivity.valid) {
i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value);
i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x);
m_RegisterInterface.writeReg(Regs::Offset6::reg, Regs::Offset6::value);
m_RegisterInterface.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x);
}
i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
m_RegisterInterface.writeReg(
Regs::PwrCtrl::reg,
Regs::PwrCtrl::valueGyrAccTempOn
);
delay(100); // power up delay
i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
m_RegisterInterface.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
m_RegisterInterface.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
delay(4);
i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush);
m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush);
delay(2);
}
@@ -330,43 +342,54 @@ struct BMI270 {
// need to start from clean state according to spec
restartAndInit();
// only Accel ON
i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn);
m_RegisterInterface.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn);
delay(100);
i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning);
i2c.writeReg(Regs::FeatPage, 1);
i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT);
i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger);
m_RegisterInterface.writeReg(
Regs::GyrCrtConf::reg,
Regs::GyrCrtConf::valueRunning
);
m_RegisterInterface.writeReg(Regs::FeatPage, 1);
m_RegisterInterface.writeReg16(
Regs::GTrig1::reg,
Regs::GTrig1::valueTriggerCRT
);
m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger);
delay(200);
while (i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) {
logger.info("CRT running. Do not move tracker!");
while (m_RegisterInterface.readReg(Regs::GyrCrtConf::reg)
== Regs::GyrCrtConf::valueRunning) {
m_Logger.info("CRT running. Do not move tracker!");
delay(200);
}
i2c.writeReg(Regs::FeatPage, 0);
m_RegisterInterface.writeReg(Regs::FeatPage, 0);
uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg)
uint8_t status = m_RegisterInterface.readReg(Regs::GyrGainStatus::reg)
>> Regs::GyrGainStatus::statusOffset;
// turn gyroscope back on
i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
m_RegisterInterface.writeReg(
Regs::PwrCtrl::reg,
Regs::PwrCtrl::valueGyrAccTempOn
);
delay(100);
bool success;
if (status != 0) {
logger.error(
m_Logger.error(
"CRT failed with status 0x%x. Recalibrate again to enable CRT.",
status
);
if (status == 0x03) {
logger.error("Reason: tracker was moved during CRT!");
m_Logger.error("Reason: tracker was moved during CRT!");
}
success = false;
} else {
std::array<uint8_t, 3> crt_values;
i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data());
logger.debug(
m_RegisterInterface
.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data());
m_Logger.debug(
"CRT finished successfully, result 0x%x, 0x%x, 0x%x",
crt_values[0],
crt_values[1],
@@ -395,11 +418,12 @@ struct BMI270 {
// temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9
// degrees C (0x7FFF)
constexpr float TempStep = 128. / 65535;
const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
const auto value
= static_cast<int16_t>(m_RegisterInterface.readReg16(Regs::TempData));
return static_cast<float>(value) * TempStep + 23.0f;
}
using FifoBuffer = std::array<uint8_t, I2CImpl::MaxTransactionLength>;
using FifoBuffer = std::array<uint8_t, RegisterInterface::MaxTransactionLength>;
FifoBuffer read_buffer;
template <typename T>
@@ -416,13 +440,14 @@ struct BMI270 {
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
const auto bytes_to_read = std::min(
static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(fifo_bytes)
);
i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
m_RegisterInterface
.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
for (uint32_t i = 0u; i < bytes_to_read;) {
const uint8_t header = getFromFifo<uint8_t>(i, read_buffer);
@@ -451,7 +476,7 @@ struct BMI270 {
gyro[0] = std::clamp(
static_cast<int32_t>(gyro[0])
- static_cast<int16_t>(
(static_cast<int32_t>(zxFactor) * gyro[2]) / 512
(static_cast<int32_t>(m_zxFactor) * gyro[2]) / 512
),
static_cast<int32_t>(ShortLimit::min()),
static_cast<int32_t>(ShortLimit::max())

33
src/sensors/softfusion/drivers/icm42688.h
View File

@@ -36,7 +36,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Gyroscope ODR = 500Hz, accel ODR = 100Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
template <typename I2CImpl>
struct ICM42688 {
static constexpr uint8_t Address = 0x68;
static constexpr auto Name = "ICM-42688";
@@ -66,11 +65,11 @@ struct ICM42688 {
.restThAcc = 0.196f,
};
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: i2c(i2c)
, logger(logger) {}
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;
ICM42688(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: m_RegisterInterface(registerInterface)
, m_Logger(logger) {}
struct Regs {
struct WhoAmI {
@@ -143,15 +142,18 @@ struct ICM42688 {
bool initialize() {
// perform initialization step
i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset);
m_RegisterInterface.writeReg(
Regs::DeviceConfig::reg,
Regs::DeviceConfig::valueSwReset
);
delay(20);
i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value);
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::FifoConfig1::reg, Regs::FifoConfig1::value);
i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value);
m_RegisterInterface.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value);
m_RegisterInterface.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
m_RegisterInterface.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
m_RegisterInterface.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
m_RegisterInterface.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
m_RegisterInterface.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value);
delay(1);
return true;
@@ -163,7 +165,7 @@ struct ICM42688 {
GyroCall&& processGyroSample,
TempCall&& processTemperatureSample
) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
const auto bytes_to_read = std::min(
@@ -171,7 +173,8 @@ struct ICM42688 {
static_cast<size_t>(fifo_bytes)
)
/ FullFifoEntrySize * FullFifoEntrySize;
i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
m_RegisterInterface
.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
FifoEntryAligned entry;
memcpy(

11
src/sensors/softfusion/drivers/icm45605.h
View File

@@ -35,8 +35,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
template <typename I2CImpl>
struct ICM45605 : public ICM45Base<I2CImpl> {
struct ICM45605 : public ICM45Base {
static constexpr auto Name = "ICM-45605";
static constexpr auto Type = SensorTypeID::ICM45605;
@@ -48,8 +47,8 @@ struct ICM45605 : public ICM45Base<I2CImpl> {
.restThAcc = 0.0098f,
};
ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: ICM45Base<I2CImpl>{i2c, logger} {}
ICM45605(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: ICM45Base{registerInterface, logger} {}
struct Regs {
struct WhoAmI {
@@ -59,8 +58,8 @@ struct ICM45605 : public ICM45Base<I2CImpl> {
};
bool initialize() {
ICM45Base<I2CImpl>::softResetIMU();
return ICM45Base<I2CImpl>::initializeBase();
ICM45Base::softResetIMU();
return ICM45Base::initializeBase();
}
};

17
src/sensors/softfusion/drivers/icm45686.h
View File

@@ -35,8 +35,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
template <typename I2CImpl>
struct ICM45686 : public ICM45Base<I2CImpl> {
struct ICM45686 : public ICM45Base {
static constexpr auto Name = "ICM-45686";
static constexpr auto Type = SensorTypeID::ICM45686;
@@ -48,8 +47,8 @@ struct ICM45686 : public ICM45Base<I2CImpl> {
.restThAcc = 0.196f,
};
ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: ICM45Base<I2CImpl>{i2c, logger} {}
ICM45686(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: ICM45Base{registerInterface, logger} {}
struct Regs {
struct WhoAmI {
@@ -68,13 +67,11 @@ struct ICM45686 : public ICM45Base<I2CImpl> {
};
};
using ICM45Base<I2CImpl>::i2c;
bool initialize() {
ICM45Base<I2CImpl>::softResetIMU();
i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value);
i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value);
return ICM45Base<I2CImpl>::initializeBase();
ICM45Base::softResetIMU();
m_RegisterInterface.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value);
m_RegisterInterface.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value);
return ICM45Base::initializeBase();
}
};

48
src/sensors/softfusion/drivers/icm45base.h
View File

@@ -24,6 +24,8 @@
#include <array>
#include <cstdint>
#include "../../../sensorinterface/RegisterInterface.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 32g
@@ -33,7 +35,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
template <typename I2CImpl>
struct ICM45Base {
static constexpr uint8_t Address = 0x68;
@@ -53,11 +54,11 @@ struct ICM45Base {
static constexpr bool Uses32BitSensorData = true;
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: i2c(i2c)
, logger(logger) {}
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;
ICM45Base(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: m_RegisterInterface(registerInterface)
, m_Logger(logger) {}
struct BaseRegs {
static constexpr uint8_t TempData = 0x0c;
@@ -119,17 +120,35 @@ struct ICM45Base {
static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
void softResetIMU() {
i2c.writeReg(BaseRegs::DeviceConfig::reg, BaseRegs::DeviceConfig::valueSwReset);
m_RegisterInterface.writeReg(
BaseRegs::DeviceConfig::reg,
BaseRegs::DeviceConfig::valueSwReset
);
delay(35);
}
bool initializeBase() {
// perform initialization step
i2c.writeReg(BaseRegs::GyroConfig::reg, BaseRegs::GyroConfig::value);
i2c.writeReg(BaseRegs::AccelConfig::reg, BaseRegs::AccelConfig::value);
i2c.writeReg(BaseRegs::FifoConfig0::reg, BaseRegs::FifoConfig0::value);
i2c.writeReg(BaseRegs::FifoConfig3::reg, BaseRegs::FifoConfig3::value);
i2c.writeReg(BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::value);
m_RegisterInterface.writeReg(
BaseRegs::GyroConfig::reg,
BaseRegs::GyroConfig::value
);
m_RegisterInterface.writeReg(
BaseRegs::AccelConfig::reg,
BaseRegs::AccelConfig::value
);
m_RegisterInterface.writeReg(
BaseRegs::FifoConfig0::reg,
BaseRegs::FifoConfig0::value
);
m_RegisterInterface.writeReg(
BaseRegs::FifoConfig3::reg,
BaseRegs::FifoConfig3::value
);
m_RegisterInterface.writeReg(
BaseRegs::PwrMgmt0::reg,
BaseRegs::PwrMgmt0::value
);
delay(1);
return true;
@@ -148,7 +167,7 @@ struct ICM45Base {
constexpr int16_t InvalidReading = -32768;
size_t fifo_packets = i2c.readReg16(BaseRegs::FifoCount);
size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount);
if (fifo_packets >= 1) {
//
@@ -178,7 +197,8 @@ struct ICM45Base {
fifo_packets = std::min(fifo_packets, MaxReadings);
size_t bytes_to_read = fifo_packets * FullFifoEntrySize;
i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
m_RegisterInterface
.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
uint8_t header = read_buffer[i];

24
src/sensors/softfusion/drivers/lsm6ds-common.h
View File

@@ -27,16 +27,20 @@
#include <array>
#include <cstdint>
#include "../../../sensorinterface/RegisterInterface.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
template <typename I2CImpl>
struct LSM6DSOutputHandler {
LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: i2c(i2c)
, logger(logger) {}
LSM6DSOutputHandler(
RegisterInterface& registerInterface,
SlimeVR::Logging::Logger& logger
)
: m_RegisterInterface(registerInterface)
, m_Logger(logger) {}
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;
#pragma pack(push, 1)
struct FifoEntryAligned {
@@ -61,12 +65,13 @@ struct LSM6DSOutputHandler {
constexpr auto FIFO_SAMPLES_MASK = 0x3ff;
constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800;
const auto fifo_status = i2c.readReg16(Regs::FifoStatus);
const auto fifo_status = m_RegisterInterface.readReg16(Regs::FifoStatus);
const auto available_axes = fifo_status & FIFO_SAMPLES_MASK;
const auto fifo_bytes = available_axes * FullFifoEntrySize;
if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) {
// FIFO overrun is expected to happen during startup and calibration
logger.error("FIFO OVERRUN! This occuring during normal usage is an issue."
m_Logger.error(
"FIFO OVERRUN! This occuring during normal usage is an issue."
);
}
@@ -76,7 +81,8 @@ struct LSM6DSOutputHandler {
static_cast<size_t>(fifo_bytes)
)
/ FullFifoEntrySize * FullFifoEntrySize;
i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
m_RegisterInterface
.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
FifoEntryAligned entry;
uint8_t tag = read_buffer[i] >> 3;

36
src/sensors/softfusion/drivers/lsm6ds3trc.h
View File

@@ -27,6 +27,7 @@
#include <array>
#include <cstdint>
#include "../../../sensorinterface/RegisterInterface.h"
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -35,7 +36,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 416Hz
template <typename I2CImpl>
struct LSM6DS3TRC {
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DS3TR-C";
@@ -64,11 +64,11 @@ struct LSM6DS3TRC {
.restThAcc = 0.392f,
};
I2CImpl i2c;
SlimeVR::Logging::Logger logger;
LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: i2c(i2c)
, logger(logger) {}
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger m_Logger;
LSM6DS3TRC(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: m_RegisterInterface(registerInterface)
, m_Logger(logger) {}
struct Regs {
struct WhoAmI {
@@ -111,14 +111,14 @@ struct LSM6DS3TRC {
bool initialize() {
// perform initialization step
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
delay(20);
i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value);
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
i2c.writeReg(Regs::FifoCtrl2::reg, Regs::FifoCtrl2::value);
i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value);
i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
m_RegisterInterface.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
m_RegisterInterface.writeReg(Regs::FifoCtrl2::reg, Regs::FifoCtrl2::value);
m_RegisterInterface.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value);
m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
return true;
}
@@ -128,12 +128,12 @@ struct LSM6DS3TRC {
GyroCall&& processGyroSample,
TempCall&& processTemperatureSample
) {
const auto read_result = i2c.readReg16(Regs::FifoStatus);
const auto read_result = m_RegisterInterface.readReg16(Regs::FifoStatus);
if (read_result & 0x4000) { // overrun!
// disable and re-enable fifo to clear it
logger.debug("Fifo overrun, resetting...");
i2c.writeReg(Regs::FifoCtrl5::reg, 0);
i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
m_Logger.debug("Fifo overrun, resetting...");
m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, 0);
m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
return;
}
const auto unread_entries = read_result & 0x7ff;
@@ -150,7 +150,7 @@ struct LSM6DS3TRC {
* sizeof(uint16_t) / single_measurement_bytes
* single_measurement_bytes;
i2c.readBytes(
m_RegisterInterface.readBytes(
Regs::FifoData,
bytes_to_read,
reinterpret_cast<uint8_t*>(read_buffer.data())

44
src/sensors/softfusion/drivers/lsm6dso.h
View File

@@ -36,8 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 104Hz
template <typename I2CImpl>
struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
struct LSM6DSO : LSM6DSOutputHandler {
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DSO";
static constexpr auto Type = SensorTypeID::LSM6DSO;
@@ -68,8 +67,6 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
.restThAcc = 0.192f,
};
using LSM6DSOutputHandler<I2CImpl>::i2c;
struct Regs {
struct WhoAmI {
static constexpr uint8_t reg = 0x0f;
@@ -104,18 +101,24 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
static constexpr uint8_t FifoData = 0x78;
};
LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
LSM6DSO(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: LSM6DSOutputHandler(registerInterface, logger) {}
bool initialize() {
// perform initialization step
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
delay(20);
i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
m_RegisterInterface.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
m_RegisterInterface.writeReg(
Regs::FifoCtrl3BDR::reg,
Regs::FifoCtrl3BDR::value
);
m_RegisterInterface.writeReg(
Regs::FifoCtrl4Mode::reg,
Regs::FifoCtrl4Mode::value
);
return true;
}
@@ -125,15 +128,14 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
LSM6DSOutputHandler<I2CImpl>::
template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
LSM6DSOutputHandler::template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
}
};

44
src/sensors/softfusion/drivers/lsm6dsr.h
View File

@@ -35,8 +35,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 104Hz
template <typename I2CImpl>
struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
struct LSM6DSR : LSM6DSOutputHandler {
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DSR";
static constexpr auto Type = SensorTypeID::LSM6DSR;
@@ -67,8 +66,6 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
.restThAcc = 0.192f,
};
using LSM6DSOutputHandler<I2CImpl>::i2c;
struct Regs {
struct WhoAmI {
static constexpr uint8_t reg = 0x0f;
@@ -103,18 +100,24 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
static constexpr uint8_t FifoData = 0x78;
};
LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
LSM6DSR(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: LSM6DSOutputHandler(registerInterface, logger) {}
bool initialize() {
// perform initialization step
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
delay(20);
i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
m_RegisterInterface.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
m_RegisterInterface.writeReg(
Regs::FifoCtrl3BDR::reg,
Regs::FifoCtrl3BDR::value
);
m_RegisterInterface.writeReg(
Regs::FifoCtrl4Mode::reg,
Regs::FifoCtrl4Mode::value
);
return true;
}
@@ -124,15 +127,14 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
LSM6DSOutputHandler<I2CImpl>::
template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
LSM6DSOutputHandler::template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
}
};

50
src/sensors/softfusion/drivers/lsm6dsv.h
View File

@@ -36,8 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 1000dps
// Gyroscope ODR = 480Hz, accel ODR = 120Hz
template <typename I2CImpl>
struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
struct LSM6DSV : LSM6DSOutputHandler {
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DSV";
static constexpr auto Type = SensorTypeID::LSM6DSV;
@@ -68,8 +67,6 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
.restThAcc = 0.192f,
};
using LSM6DSOutputHandler<I2CImpl>::i2c;
struct Regs {
struct WhoAmI {
static constexpr uint8_t reg = 0x0f;
@@ -116,21 +113,27 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
static constexpr uint8_t FifoData = 0x78;
};
LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
LSM6DSV(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: LSM6DSOutputHandler(registerInterface, logger) {}
bool initialize() {
// perform initialization step
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
delay(20);
i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value);
i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value);
i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value);
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value);
i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value);
i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
m_RegisterInterface.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value);
m_RegisterInterface.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value);
m_RegisterInterface.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value);
m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
m_RegisterInterface.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value);
m_RegisterInterface.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value);
m_RegisterInterface.writeReg(
Regs::FifoCtrl3BDR::reg,
Regs::FifoCtrl3BDR::value
);
m_RegisterInterface.writeReg(
Regs::FifoCtrl4Mode::reg,
Regs::FifoCtrl4Mode::value
);
return true;
}
@@ -140,15 +143,14 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
LSM6DSOutputHandler<I2CImpl>::
template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
LSM6DSOutputHandler::bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
}
};

55
src/sensors/softfusion/drivers/mpu6050.h
View File

@@ -29,6 +29,7 @@
#include <array>
#include <cstdint>
#include "../../../sensorinterface/RegisterInterface.h"
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -37,7 +38,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 1000dps
// Gyroscope ODR = accel ODR = 250Hz
template <typename I2CImpl>
struct MPU6050 {
struct FifoSample {
uint8_t accel_x_h, accel_x_l;
@@ -77,11 +77,11 @@ struct MPU6050 {
.restThAcc = 0.784f,
};
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: i2c(i2c)
, logger(logger) {}
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;
MPU6050(RegisterInterface& i2c, SlimeVR::Logging::Logger& logger)
: m_RegisterInterface(i2c)
, m_Logger(logger) {}
struct Regs {
struct WhoAmI {
@@ -120,49 +120,52 @@ struct MPU6050 {
}
void resetFIFO() {
i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue);
m_RegisterInterface.writeReg(
Regs::UserCtrl::reg,
Regs::UserCtrl::fifoResetValue
);
}
bool initialize() {
// Reset
i2c.writeReg(
m_RegisterInterface.writeReg(
MPU6050_RA_PWR_MGMT_1,
0x80
); // PWR_MGMT_1: reset with 100ms delay (also disables sleep)
delay(100);
i2c.writeReg(
m_RegisterInterface.writeReg(
MPU6050_RA_SIGNAL_PATH_RESET,
0x07
); // full SIGNAL_PATH_RESET: with another 100ms delay
delay(100);
// Configure
i2c.writeReg(
m_RegisterInterface.writeReg(
MPU6050_RA_PWR_MGMT_1,
0x01
); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
i2c.writeReg(
m_RegisterInterface.writeReg(
MPU6050_RA_USER_CTRL,
0x00
); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP
i2c.writeReg(
m_RegisterInterface.writeReg(
MPU6050_RA_INT_ENABLE,
0x10
); // 0001 0000 INT_ENABLE: only FIFO overflow interrupt
i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
i2c.writeReg(
m_RegisterInterface.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
m_RegisterInterface.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
m_RegisterInterface.writeReg(
MPU6050_RA_CONFIG,
0x02
); // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro
// output rate to 1KHz)
i2c.writeReg(
m_RegisterInterface.writeReg(
MPU6050_RA_SMPLRT_DIV,
0x03
); // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate
// = Gyroscope Output Rate / (1 + SMPLRT_DIV))
i2c.writeReg(
m_RegisterInterface.writeReg(
MPU6050_RA_FIFO_EN,
0x78
); // 0111 1000 FIFO_EN: All gyro axes + Accel
@@ -173,26 +176,30 @@ struct MPU6050 {
}
float getDirectTemp() const {
auto value = byteSwap(i2c.readReg16(Regs::OutTemp));
auto value = byteSwap(m_RegisterInterface.readReg16(Regs::OutTemp));
float result = (static_cast<int16_t>(value) / 340.0f) + 36.53f;
return result;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
const auto status = i2c.readReg(Regs::IntStatus);
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
const auto status = m_RegisterInterface.readReg(Regs::IntStatus);
if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) {
// Overflows make it so we lose track of which packet is which
// This necessitates a reset
logger.debug("Fifo overrun, resetting...");
m_Logger.debug("Fifo overrun, resetting...");
resetFIFO();
return;
}
std::array<uint8_t, 12 * 10>
readBuffer; // max 10 packages of 12byte values (sample) of data form fifo
auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount));
auto byteCount = byteSwap(m_RegisterInterface.readReg16(Regs::FifoCount));
auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size())
/ sizeof(FifoSample) * sizeof(FifoSample);
@@ -200,7 +207,7 @@ struct MPU6050 {
return;
}
i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data());
m_RegisterInterface.readBytes(Regs::FifoData, readBytes, readBuffer.data());
for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) {
const FifoSample* sample = reinterpret_cast<FifoSample*>(&readBuffer[i]);

86
src/sensors/softfusion/softfusionsensor.h
View File

@@ -27,6 +27,7 @@
#include <cstdlib>
#include <tuple>
#include "../../sensorinterface/i2cimpl.h"
#include "../RestCalibrationDetector.h"
#include "../SensorFusionRestDetect.h"
#include "../sensor.h"
@@ -37,36 +38,33 @@
namespace SlimeVR::Sensors {
template <
template <typename I2CImpl>
typename T,
typename I2CImpl,
typename SensorType,
template <typename IMU, typename RawSensorT, typename RawVectorT>
typename Calibrator>
class SoftFusionSensor : public Sensor {
using imu = T<I2CImpl>;
static constexpr sensor_real_t getDefaultTempTs() {
if constexpr (DirectTempReadOnly) {
return DirectTempReadTs;
} else {
return imu::TempTs;
return SensorType::TempTs;
}
}
static constexpr bool Uses32BitSensorData
= requires(imu& i) { i.Uses32BitSensorData; };
= requires(SensorType& i) { i.Uses32BitSensorData; };
static constexpr bool DirectTempReadOnly = requires(imu& i) { i.getDirectTemp(); };
static constexpr bool DirectTempReadOnly
= requires(SensorType& i) { i.getDirectTemp(); };
using RawSensorT =
typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
using RawVectorT = std::array<RawSensorT, 3>;
static constexpr float GScale
= ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0);
static constexpr float AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity;
= ((32768. / SensorType::GyroSensitivity) / 32768.) * (PI / 180.0);
static constexpr float AScale = CONST_EARTH_GRAVITY / SensorType::AccelSensitivity;
using Calib = Calibrator<imu, RawSensorT, RawVectorT>;
using Calib = Calibrator<SensorType, RawSensorT, RawVectorT>;
static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration;
@@ -76,12 +74,13 @@ class SoftFusionSensor : public Sensor {
uint32_t lastTempPollTime = micros();
bool detected() const {
const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg);
if (imu::Regs::WhoAmI::value != value) {
const auto value
= m_sensor.m_RegisterInterface.readReg(SensorType::Regs::WhoAmI::reg);
if (SensorType::Regs::WhoAmI::value != value) {
m_Logger.error(
"Sensor not detected, expected reg 0x%02x = 0x%02x but got 0x%02x",
imu::Regs::WhoAmI::reg,
imu::Regs::WhoAmI::value,
SensorType::Regs::WhoAmI::reg,
SensorType::Regs::WhoAmI::value,
value
);
return false;
@@ -139,9 +138,10 @@ class SoftFusionSensor : public Sensor {
void
processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) {
if constexpr (!DirectTempReadOnly) {
const float scaledTemperature = imu::TemperatureBias
+ static_cast<float>(rawTemperature)
* (1.0 / imu::TemperatureSensitivity);
const float scaledTemperature
= SensorType::TemperatureBias
+ static_cast<float>(rawTemperature)
* (1.0 / SensorType::TemperatureSensitivity);
lastReadTemperature = scaledTemperature;
if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) {
@@ -204,20 +204,32 @@ class SoftFusionSensor : public Sensor {
}
public:
static constexpr auto TypeID = imu::Type;
static constexpr uint8_t Address = imu::Address;
static constexpr auto TypeID = SensorType::Type;
static constexpr uint8_t Address = SensorType::Address;
SoftFusionSensor(
uint8_t id,
uint8_t i2cAddress,
RegisterInterface& registerInterface,
float rotation,
SlimeVR::SensorInterface* sensorInterface,
SlimeVR::SensorInterface* sensorInterface = nullptr,
PinInterface* intPin = nullptr,
uint8_t = 0
)
: Sensor(imu::Name, imu::Type, id, i2cAddress, rotation, sensorInterface)
, m_fusion(imu::SensorVQFParams, imu::GyrTs, imu::AccTs, imu::MagTs)
, m_sensor(I2CImpl(i2cAddress), m_Logger) {}
: Sensor(
SensorType::Name,
SensorType::Type,
id,
registerInterface,
rotation,
sensorInterface
)
, m_fusion(
SensorType::SensorVQFParams,
SensorType::GyrTs,
SensorType::AccTs,
SensorType::MagTs
)
, m_sensor(registerInterface, m_Logger) {}
~SoftFusionSensor() override = default;
void checkSensorTimeout() {
@@ -346,7 +358,7 @@ public:
bool initResult = false;
if constexpr (Calib::HasMotionlessCalib) {
typename imu::MotionlessCalibrationData calibData;
typename SensorType::MotionlessCalibrationData calibData;
std::memcpy(
&calibData,
calibrator.getMotionlessCalibrationData(),
@@ -409,7 +421,7 @@ public:
SensorStatus getSensorState() final { return m_status; }
SensorFusionRestDetect m_fusion;
imu m_sensor;
SensorType m_sensor;
Calib calibrator{
m_fusion,
m_sensor,
@@ -428,6 +440,26 @@ public:
uint32_t m_lastTemperaturePacketSent = 0;
RestCalibrationDetector calibrationDetector;
static bool checkPresent(uint8_t sensorID, const RegisterInterface& imuInterface) {
I2Cdev::readTimeout = 100;
auto value = imuInterface.readReg(SensorType::Regs::WhoAmI::reg);
I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
if (SensorType::Regs::WhoAmI::value == value) {
return true;
}
return false;
}
static bool checkPresent(uint8_t sensorID, uint8_t imuAddress) {
uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID;
// Ask twice, because we're nice like this
if (!I2CSCAN::hasDevOnBus(address) && !I2CSCAN::hasDevOnBus(address)) {
return false;
}
const I2CImpl& i2c = I2CImpl(address);
return checkPresent(sensorID, i2c);
}
};
} // namespace SlimeVR::Sensors