Fix 0 byte string sending and check for lengths

Embed vendor information into build parameters & defines so SlimeVR vendors can maintain their own firmware updates without adding new boards

Bump protocol version to 21

.clang-tidy

@@ -0,0 +1,200 @@
+# Generated from CLion Inspection settings
+---
+CheckOptions:
+  - key: readability-identifier-naming.ClassCase
+    value: CamelCase
+  - key: readability-identifier-naming.ClassMemberCase
+    value: camelBack
+  - key: readability-identifier-naming.ConstexprVariableCase
+    value: CamelCase
+  - key: readability-identifier-naming.ConstexprVariablePrefix
+    value: k
+  - key: readability-identifier-naming.EnumCase
+    value: CamelCase
+  - key: readability-identifier-naming.EnumConstantCase
+    value: CamelCase
+  - key: readability-identifier-naming.FunctionCase
+    value: camelBack
+  - key: readability-identifier-naming.GlobalConstantCase
+    value: CamelCase
+  - key: readability-identifier-naming.GlobalConstantPrefix
+    value: k
+  - key: readability-identifier-naming.StaticConstantCase
+    value: CamelCase
+  - key: readability-identifier-naming.StaticConstantPrefix
+    value: k
+  - key: readability-identifier-naming.StaticVariableCase
+    value: camelBack
+  - key: readability-identifier-naming.MacroDefinitionCase
+    value: UPPER_CASE
+  - key: readability-identifier-naming.MacroDefinitionIgnoredRegexp
+    value: '^[A-Z]+(_[A-Z]+)*_$'
+  - key: readability-identifier-naming.MemberCase
+    value: camelBack
+  - key: readability-identifier-naming.PrivateMemberSuffix
+    value: m_
+  - key: readability-identifier-naming.PublicMemberSuffix
+    value: ''
+  - key: readability-identifier-naming.NamespaceCase
+    value: CamelCase
+  - key: readability-identifier-naming.ParameterCase
+    value: camelBack
+  - key: readability-identifier-naming.TypeAliasCase
+    value: CamelCase
+  - key: readability-identifier-naming.TypedefCase
+    value: CamelCase
+  - key: readability-identifier-naming.VariableCase
+    value: camelBack
+  - key: readability-identifier-naming.IgnoreMainLikeFunctions
+    value: 1
+
+Checks: '-*,
+bugprone-argument-comment,
+bugprone-assert-side-effect,
+bugprone-bad-signal-to-kill-thread,
+bugprone-branch-clone,
+bugprone-copy-constructor-init,
+bugprone-dangling-handle,
+bugprone-dynamic-static-initializers,
+bugprone-fold-init-type,
+bugprone-forward-declaration-namespace,
+bugprone-forwarding-reference-overload,
+bugprone-inaccurate-erase,
+bugprone-incorrect-roundings,
+bugprone-integer-division,
+bugprone-lambda-function-name,
+bugprone-macro-parentheses,
+bugprone-macro-repeated-side-effects,
+bugprone-misplaced-operator-in-strlen-in-alloc,
+bugprone-misplaced-pointer-arithmetic-in-alloc,
+bugprone-misplaced-widening-cast,
+bugprone-move-forwarding-reference,
+bugprone-multiple-statement-macro,
+bugprone-no-escape,
+bugprone-parent-virtual-call,
+bugprone-posix-return,
+bugprone-reserved-identifier,
+bugprone-sizeof-container,
+bugprone-sizeof-expression,
+bugprone-spuriously-wake-up-functions,
+bugprone-string-constructor,
+bugprone-string-integer-assignment,
+bugprone-string-literal-with-embedded-nul,
+bugprone-suspicious-enum-usage,
+bugprone-suspicious-include,
+bugprone-suspicious-memset-usage,
+bugprone-suspicious-missing-comma,
+bugprone-suspicious-semicolon,
+bugprone-suspicious-string-compare,
+bugprone-suspicious-memory-comparison,
+bugprone-suspicious-realloc-usage,
+bugprone-swapped-arguments,
+bugprone-terminating-continue,
+bugprone-throw-keyword-missing,
+bugprone-too-small-loop-variable,
+bugprone-undefined-memory-manipulation,
+bugprone-undelegated-constructor,
+bugprone-unhandled-self-assignment,
+bugprone-unused-raii,
+bugprone-unused-return-value,
+bugprone-use-after-move,
+bugprone-virtual-near-miss,
+cert-dcl21-cpp,
+cert-dcl58-cpp,
+cert-err34-c,
+cert-err52-cpp,
+cert-err60-cpp,
+cert-flp30-c,
+cert-msc50-cpp,
+cert-msc51-cpp,
+cert-str34-c,
+cppcoreguidelines-interfaces-global-init,
+cppcoreguidelines-narrowing-conversions,
+cppcoreguidelines-pro-type-member-init,
+cppcoreguidelines-pro-type-static-cast-downcast,
+cppcoreguidelines-slicing,
+google-default-arguments,
+google-explicit-constructor,
+google-runtime-operator,
+hicpp-exception-baseclass,
+hicpp-multiway-paths-covered,
+misc-misplaced-const,
+misc-new-delete-overloads,
+misc-no-recursion,
+misc-non-copyable-objects,
+misc-throw-by-value-catch-by-reference,
+misc-unconventional-assign-operator,
+misc-uniqueptr-reset-release,
+modernize-avoid-bind,
+modernize-concat-nested-namespaces,
+modernize-deprecated-headers,
+modernize-deprecated-ios-base-aliases,
+modernize-loop-convert,
+modernize-make-shared,
+modernize-make-unique,
+modernize-pass-by-value,
+modernize-raw-string-literal,
+modernize-redundant-void-arg,
+modernize-replace-auto-ptr,
+modernize-replace-disallow-copy-and-assign-macro,
+modernize-replace-random-shuffle,
+modernize-return-braced-init-list,
+modernize-shrink-to-fit,
+modernize-unary-static-assert,
+modernize-use-auto,
+modernize-use-bool-literals,
+modernize-use-emplace,
+modernize-use-equals-default,
+modernize-use-equals-delete,
+modernize-use-nodiscard,
+modernize-use-noexcept,
+modernize-use-nullptr,
+modernize-use-override,
+modernize-use-transparent-functors,
+modernize-use-uncaught-exceptions,
+mpi-buffer-deref,
+mpi-type-mismatch,
+openmp-use-default-none,
+performance-faster-string-find,
+performance-for-range-copy,
+performance-implicit-conversion-in-loop,
+performance-inefficient-algorithm,
+performance-inefficient-string-concatenation,
+performance-inefficient-vector-operation,
+performance-move-const-arg,
+performance-move-constructor-init,
+performance-no-automatic-move,
+performance-noexcept-move-constructor,
+performance-trivially-destructible,
+performance-type-promotion-in-math-fn,
+performance-unnecessary-copy-initialization,
+performance-unnecessary-value-param,
+portability-simd-intrinsics,
+readability-avoid-const-params-in-decls,
+readability-const-return-type,
+readability-container-size-empty,
+readability-convert-member-functions-to-static,
+readability-delete-null-pointer,
+readability-deleted-default,
+#readability-identifier-naming,
+readability-inconsistent-declaration-parameter-name,
+readability-make-member-function-const,
+readability-misleading-indentation,
+readability-misplaced-array-index,
+readability-non-const-parameter,
+readability-redundant-control-flow,
+readability-redundant-declaration,
+readability-redundant-function-ptr-dereference,
+readability-redundant-smartptr-get,
+readability-redundant-string-cstr,
+readability-redundant-string-init,
+readability-simplify-subscript-expr,
+#readability-static-accessed-through-instance,
+readability-static-definition-in-anonymous-namespace,
+readability-string-compare,
+readability-uniqueptr-delete-release,
+readability-use-anyofallof'
+
+#FormatStyle: google
+
+HeaderFilterRegex: '^((?!/.pio/|/lib/).)*$'

.github/CODEOWNERS

@@ -0,0 +1,13 @@
+# Global code owner
+* @Eirenliel
+
+# Make Loucas code owner of the defines to keep fw tool compatibility
+/src/defines.h @loucass003
+/src/consts.h @loucass003
+/src/debug.h @loucass003
+
+# Sfusion framework
+/src/sensors/softfusion/ @gorbit99 @l0ud
+/srs/sensors/SensorFusion* @gorbit99 @l0ud
+/srs/sensors/motionprocessing/ @gorbit99 @l0ud
+/lib/vqf/

.github/workflows/actions.yml

@@ -2,11 +2,35 @@ name: Build
 
 on:
   push:
+    branches:
+      - main
+    tags:
+      - "*"
   pull_request:
+  workflow_dispatch:
+  create:
 
 jobs:
+  format:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - uses: jidicula/clang-format-action@v4.14.0
+        with:
+          clang-format-version: "17"
+          fallback-style: google
+  #      Disable clang-tidy for now
+  #      - name: Get clang-tidy
+  #        run: |
+  #          apt-get update
+  #          apt-get install -y clang-tidy
+  #      - uses: ZehMatt/clang-tidy-annotations@v1.0.0
+  #        with:
+  #          build_dir: 'build'
+  #          cmake_args: '-G Ninja -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++'
+
   build:
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-latest
 
     steps:
       - uses: actions/checkout@v4
@@ -17,6 +41,9 @@ jobs:
             ~/.platformio/.cache
           key: ${{ runner.os }}-pio
 
+      - name: Get tags
+        run: git fetch --tags origin --recurse-submodules=no --force
+
       - name: Set up Python
         uses: actions/setup-python@v5
         with:
@@ -35,3 +62,13 @@ jobs:
         with:
           name: binaries
           path: ./build/*.bin
+
+      - name: Upload to draft release
+        uses: softprops/action-gh-release@v2
+        if: startsWith(github.ref, 'refs/tags/')
+        with:
+          draft: true
+          generate_release_notes: true
+          files: |
+            ./build/BOARD_SLIMEVR-firmware.bin
+            ./build/BOARD_SLIMEVR_V1_2-firmware.bin

.github/workflows/release.yml

@@ -1,20 +0,0 @@
-name: Releases
-
-on:
-  push:
-    tags:
-    - '*'
-
-jobs:
-
-  build:
-    runs-on: ubuntu-latest
-    permissions:
-      contents: write
-    steps:
-    - uses: actions/checkout@v4
-    - uses: ncipollo/release-action@v1
-      with:
-        artifacts: "./build/*.bin"
-        draft: true
-        token: ${{ secrets.GITHUB_TOKEN }}

.gitignore

@@ -4,3 +4,4 @@ build/
 venv/
 cache/
 .idea/
+compile_commands.json

boards/esp32s3_supermini.json

@@ -0,0 +1,48 @@
+{
+    "build": {
+      "arduino": {
+        "ldscript": "esp32s3_out.ld",
+        "memory_type": "qio_qspi"
+      },
+      "core": "esp32",
+      "extra_flags": [
+        "-DARDUINO_ESP32S3_SUPERMINI",
+        "-DBOARD_HAS_PSRAM",
+        "-DARDUINO_USB_CDC_ON_BOOT=1"
+      ],
+      "f_cpu": "240000000L",
+      "f_flash": "80000000L",
+      "flash_mode": "qio",
+      "hwids": [
+        [
+          "0x303D",
+          "0x100D"
+        ]
+      ],
+      "mcu": "esp32s3",
+      "variants_dir": "boards/variants",
+      "variant": "esp32s3_supermini"
+    },
+    "connectivity": [
+      "bluetooth",
+      "wifi"
+    ],
+    "debug": {
+      "openocd_target": "esp32s3.cfg"
+    },
+    "frameworks": [
+      "arduino",
+      "espidf"
+    ],
+    "name": "ESP32-S3 SuperMini",
+    "upload": {
+      "flash_size": "4MB",
+      "maximum_ram_size": 327680,
+      "maximum_size": 4194304,
+      "require_upload_port": true,
+      "speed": 460800
+    },
+    "url": "https://www.aliexpress.com",
+    "vendor": "AliExpress"
+}
+  

boards/variants/esp32s3_supermini/pins_arduino.h

@@ -0,0 +1,64 @@
+#ifndef Pins_Arduino_h
+#define Pins_Arduino_h
+
+#include <stdint.h>
+
+#include "soc/soc_caps.h"
+
+#define USB_VID 0x303d
+#define USB_PID 0x100d
+
+static const uint8_t LED_BUILTIN = SOC_GPIO_PIN_COUNT + 48;
+#define BUILTIN_LED LED_BUILTIN  // backward compatibility
+#define LED_BUILTIN LED_BUILTIN  // allow testing #ifdef LED_BUILTIN
+#define RGB_BUILTIN LED_BUILTIN
+#undef RGB_BRIGHTNESS
+#define RGB_BRIGHTNESS 4  // 64 max
+
+// uart0
+static const uint8_t TX = 43;
+static const uint8_t RX = 44;
+
+static const uint8_t SDA = 5;
+static const uint8_t SCL = 6;
+
+static const uint8_t SS = 10;
+static const uint8_t MOSI = 11;
+static const uint8_t MISO = 13;
+static const uint8_t SCK = 12;
+
+// adc
+static const uint8_t A0 = 1;
+static const uint8_t A1 = 2;
+static const uint8_t A2 = 3;
+static const uint8_t A3 = 4;
+static const uint8_t A4 = 5;
+static const uint8_t A5 = 6;
+static const uint8_t A6 = 7;
+static const uint8_t A7 = 8;
+static const uint8_t A8 = 9;
+static const uint8_t A9 = 10;
+static const uint8_t A10 = 11;
+static const uint8_t A11 = 12;
+static const uint8_t A12 = 13;
+static const uint8_t A13 = 14;
+static const uint8_t A14 = 15;
+static const uint8_t A15 = 16;
+
+// touch
+static const uint8_t T1 = 1;
+static const uint8_t T2 = 2;
+static const uint8_t T3 = 3;
+static const uint8_t T4 = 4;
+static const uint8_t T5 = 5;
+static const uint8_t T6 = 6;
+static const uint8_t T7 = 7;
+static const uint8_t T8 = 8;
+static const uint8_t T9 = 9;
+static const uint8_t T10 = 10;
+static const uint8_t T11 = 11;
+static const uint8_t T12 = 12;
+static const uint8_t T13 = 13;
+static const uint8_t T14 = 14;
+
+#endif /* Pins_Arduino_h */

ci/build.py

@@ -22,7 +22,7 @@ class DeviceConfiguration:
         self.platformio_board = platformio_board
 
     def filename(self) -> str:
-        return f"{self.platformio_board}.bin"
+        return f"{self.board}-firmware.bin"
 
     def __str__(self) -> str:
         return f"{self.platform}@{self.board}"
@@ -95,7 +95,7 @@ def build() -> int:
         status = build_for_device(device)
 
         if not status:
-            failed_builds.append(device.platformio_board)
+            failed_builds.append(device.board)
 
     if len(failed_builds) > 0:
         print(f"  🡢 {COLOR_RED}Failed!{COLOR_RESET}")

lib/bmi160/BMI160.h

@@ -1,767 +0,0 @@
-/*
-===============================================
-BMI160 accelerometer/gyroscope library for Intel(R) Curie(TM) devices.
-Copyright (c) 2015 Intel Corporation.  All rights reserved.
-
-Based on MPU6050 Arduino library provided by Jeff Rowberg as part of his
-excellent I2Cdev device library: https://github.com/jrowberg/i2cdevlib
-
-===============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
-
-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.
-===============================================
-*/
-
-#ifndef _BMI160_H_
-#define _BMI160_H_
-
-#include "Arduino.h"
-#include "I2Cdev.h"
-
-#define BMI160_SPI_READ_BIT         7
-
-#define BMI160_RA_CHIP_ID           0x00
-
-#define BMI160_MAG_PMU_STATUS_BIT   0
-#define BMI160_MAG_PMU_STATUS_LEN   2
-
-#define BMI160_STATUS_DRDY_MAG      5
-#define BMI160_STATUS_MAG_MAN_OP    2
-#define BMI160_MAG_RATE_SEL_BIT     0
-#define BMI160_MAG_RATE_SEL_LEN     4
-#define BMI160_FIFO_MAG_EN_BIT      5
-
-#define BMI160_RA_MAG_CONF             0x44
-#define BMI160_RA_MAG_IF_0_DEVADDR     0x4B
-#define BMI160_RA_MAG_IF_1_MODE        0x4C
-#define BMI160_RA_MAG_IF_2_READ_RA     0x4D
-#define BMI160_RA_MAG_IF_3_WRITE_RA    0x4E
-#define BMI160_RA_MAG_IF_4_WRITE_VALUE 0x4F
-#define BMI160_RA_IF_CONF              0x6B
-
-#define BMI160_IF_CONF_MODE_PRI_AUTO_SEC_OFF 0 << 4
-#define BMI160_IF_CONF_MODE_PRI_I2C_SEC_OIS  1 << 4
-#define BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG 2 << 4
-
-#define BMI160_MAG_SETUP_MODE       0x80
-#define BMI160_MAG_DATA_MODE_2      0x01
-#define BMI160_MAG_DATA_MODE_6      0x02
-#define BMI160_MAG_DATA_MODE_8      0x03
-
-typedef enum {
-    BMI160_MAG_RATE_25_32thHZ = 1,  /**< 25/32 Hz */
-    BMI160_MAG_RATE_25_16thHZ,      /**< 25/16 Hz */
-    BMI160_MAG_RATE_25_8thHZ,       /**< 25/8  Hz */
-    BMI160_MAG_RATE_25_4thHZ,       /**< 25/4  Hz */
-    BMI160_MAG_RATE_25_2thHZ,       /**< 25/2  Hz */
-    BMI160_MAG_RATE_25HZ,           /**< 25    Hz */
-    BMI160_MAG_RATE_50HZ,           /**< 50    Hz */
-    BMI160_MAG_RATE_100HZ,          /**< 100   Hz */
-    BMI160_MAG_RATE_200HZ,          /**< 200   Hz */
-    BMI160_MAG_RATE_400HZ,          /**< 400   Hz */
-    BMI160_MAG_RATE_800HZ,          /**< 800   Hz */
-} BMI160MagRate;
-
-#define BMI160_CMD_MAG_MODE_NORMAL  0x19
-
-#define BMI160_EN_PULL_UP_REG_1     0x37
-#define BMI160_EN_PULL_UP_REG_2     0x9A
-#define BMI160_EN_PULL_UP_REG_3     0xC0
-#define BMI160_EN_PULL_UP_REG_4     0x90
-#define BMI160_EN_PULL_UP_REG_5     0x80
-
-#define BMI160_7F                   0x7F
-
-#define BMI160_ACC_PMU_STATUS_BIT   4
-#define BMI160_ACC_PMU_STATUS_LEN   2
-#define BMI160_GYR_PMU_STATUS_BIT   2
-#define BMI160_GYR_PMU_STATUS_LEN   2
-
-#define BMI160_RA_PMU_STATUS        0x03
-
-#define BMI160_RA_MAG_X_L           0x04
-#define BMI160_RA_MAG_X_H           0x05
-#define BMI160_RA_MAG_Y_L           0x06
-#define BMI160_RA_MAG_Y_H           0x07
-#define BMI160_RA_MAG_Z_L           0x08
-#define BMI160_RA_MAG_Z_H           0x09
-#define BMI160_RA_GYRO_X_L          0x0C
-#define BMI160_RA_GYRO_X_H          0x0D
-#define BMI160_RA_GYRO_Y_L          0x0E
-#define BMI160_RA_GYRO_Y_H          0x0F
-#define BMI160_RA_GYRO_Z_L          0x10
-#define BMI160_RA_GYRO_Z_H          0x11
-#define BMI160_RA_ACCEL_X_L         0x12
-#define BMI160_RA_ACCEL_X_H         0x13
-#define BMI160_RA_ACCEL_Y_L         0x14
-#define BMI160_RA_ACCEL_Y_H         0x15
-#define BMI160_RA_ACCEL_Z_L         0x16
-#define BMI160_RA_ACCEL_Z_H         0x17
-
-#define BMI160_RA_SENSORTIME        0x18
-
-#define BMI160_STATUS_FOC_RDY       3
-#define BMI160_STATUS_NVM_RDY       4
-#define BMI160_STATUS_DRDY_MAG      5
-#define BMI160_STATUS_DRDY_GYR      6
-#define BMI160_STATUS_DRDY_ACC      7
-
-#define BMI160_RA_STATUS            0x1B
-
-#define BMI160_STEP_INT_BIT         0
-#define BMI160_ANYMOTION_INT_BIT    2
-#define BMI160_D_TAP_INT_BIT        4
-#define BMI160_S_TAP_INT_BIT        5
-#define BMI160_NOMOTION_INT_BIT     7
-#define BMI160_FFULL_INT_BIT        5
-#define BMI160_DRDY_INT_BIT         4
-#define BMI160_LOW_G_INT_BIT        3
-#define BMI160_HIGH_G_INT_BIT       2
-
-#define BMI160_TAP_SIGN_BIT         7
-#define BMI160_TAP_1ST_Z_BIT        6
-#define BMI160_TAP_1ST_Y_BIT        5
-#define BMI160_TAP_1ST_X_BIT        4
-
-#define BMI160_ANYMOTION_SIGN_BIT   3
-#define BMI160_ANYMOTION_1ST_Z_BIT  2
-#define BMI160_ANYMOTION_1ST_Y_BIT  1
-#define BMI160_ANYMOTION_1ST_X_BIT  0
-
-#define BMI160_HIGH_G_SIGN_BIT      3
-#define BMI160_HIGH_G_1ST_Z_BIT     2
-#define BMI160_HIGH_G_1ST_Y_BIT     1
-#define BMI160_HIGH_G_1ST_X_BIT     0
-
-#define BMI160_RA_INT_STATUS_0      0x1C
-#define BMI160_RA_INT_STATUS_1      0x1D
-#define BMI160_RA_INT_STATUS_2      0x1E
-#define BMI160_RA_INT_STATUS_3      0x1F
-
-#define BMI160_RA_TEMP_L            0x20
-#define BMI160_RA_TEMP_H            0x21
-
-#define BMI160_RA_FIFO_LENGTH_0     0x22
-#define BMI160_RA_FIFO_LENGTH_1     0x23
-
-#define BMI160_FIFO_DATA_INVALID    0x80
-#define BMI160_RA_FIFO_DATA         0x24
-
-#define BMI160_ACCEL_RATE_SEL_BIT    0
-#define BMI160_ACCEL_RATE_SEL_LEN    4
-
-#define BMI160_RA_ACCEL_CONF        0x40
-#define BMI160_RA_ACCEL_RANGE       0x41
-
-#define BMI160_RA_GYRO_CONF         0x42
-#define BMI160_RA_GYRO_RANGE        0x43
-
-#define BMI160_FIFO_HEADER_EN_BIT   4
-#define BMI160_FIFO_MAG_EN_BIT      5
-#define BMI160_FIFO_ACC_EN_BIT      6
-#define BMI160_FIFO_GYR_EN_BIT      7
-
-#define BMI160_RA_FIFO_CONFIG_0     0x46
-#define BMI160_RA_FIFO_CONFIG_1     0x47
-
-#define BMI160_ANYMOTION_EN_BIT     0
-#define BMI160_ANYMOTION_EN_LEN     3
-#define BMI160_D_TAP_EN_BIT         4
-#define BMI160_S_TAP_EN_BIT         5
-#define BMI160_NOMOTION_EN_BIT      0
-#define BMI160_NOMOTION_EN_LEN      3
-#define BMI160_LOW_G_EN_BIT         3
-#define BMI160_LOW_G_EN_LEN         1
-#define BMI160_HIGH_G_EN_BIT        0
-#define BMI160_HIGH_G_EN_LEN        3
-
-#define BMI160_STEP_EN_BIT          3
-#define BMI160_DRDY_EN_BIT          4
-#define BMI160_FFULL_EN_BIT         5
-
-#define BMI160_RA_INT_EN_0          0x50
-#define BMI160_RA_INT_EN_1          0x51
-#define BMI160_RA_INT_EN_2          0x52
-
-#define BMI160_INT1_EDGE_CTRL       0
-#define BMI160_INT1_LVL             1
-#define BMI160_INT1_OD              2
-#define BMI160_INT1_OUTPUT_EN       3
-
-#define BMI160_RA_INT_OUT_CTRL      0x53
-
-#define BMI160_LATCH_MODE_BIT       0
-#define BMI160_LATCH_MODE_LEN       4
-
-#define BMI160_RA_INT_LATCH         0x54
-#define BMI160_RA_INT_MAP_0         0x55
-#define BMI160_RA_INT_MAP_1         0x56
-#define BMI160_RA_INT_MAP_2         0x57
-
-#define BMI160_ANYMOTION_DUR_BIT    0
-#define BMI160_ANYMOTION_DUR_LEN    2
-#define BMI160_NOMOTION_DUR_BIT     2
-#define BMI160_NOMOTION_DUR_LEN     6
-
-#define BMI160_NOMOTION_SEL_BIT     0
-#define BMI160_NOMOTION_SEL_LEN     1
-
-#define BMI160_RA_INT_LOWHIGH_0     0x5A
-#define BMI160_RA_INT_LOWHIGH_1     0x5B
-#define BMI160_RA_INT_LOWHIGH_2     0x5C
-#define BMI160_RA_INT_LOWHIGH_3     0x5D
-#define BMI160_RA_INT_LOWHIGH_4     0x5E
-
-#define BMI160_RA_INT_MOTION_0      0x5F
-#define BMI160_RA_INT_MOTION_1      0x60
-#define BMI160_RA_INT_MOTION_2      0x61
-#define BMI160_RA_INT_MOTION_3      0x62
-
-#define BMI160_TAP_DUR_BIT          0
-#define BMI160_TAP_DUR_LEN          3
-#define BMI160_TAP_SHOCK_BIT        6
-#define BMI160_TAP_QUIET_BIT        7
-#define BMI160_TAP_THRESH_BIT       0
-#define BMI160_TAP_THRESH_LEN       5
-
-#define BMI160_RA_INT_TAP_0         0x63
-#define BMI160_RA_INT_TAP_1         0x64
-
-#define BMI160_FOC_ACC_Z_BIT        0
-#define BMI160_FOC_ACC_Z_LEN        2
-#define BMI160_FOC_ACC_Y_BIT        2
-#define BMI160_FOC_ACC_Y_LEN        2
-#define BMI160_FOC_ACC_X_BIT        4
-#define BMI160_FOC_ACC_X_LEN        2
-#define BMI160_FOC_GYR_EN           6
-
-#define BMI160_RA_FOC_CONF          0x69
-
-#define BMI160_GYR_OFFSET_X_MSB_BIT 0
-#define BMI160_GYR_OFFSET_X_MSB_LEN 2
-#define BMI160_GYR_OFFSET_Y_MSB_BIT 2
-#define BMI160_GYR_OFFSET_Y_MSB_LEN 2
-#define BMI160_GYR_OFFSET_Z_MSB_BIT 4
-#define BMI160_GYR_OFFSET_Z_MSB_LEN 2
-#define BMI160_ACC_OFFSET_EN        6
-#define BMI160_GYR_OFFSET_EN        7
-
-#define BMI160_RA_OFFSET_0          0x71
-#define BMI160_RA_OFFSET_1          0x72
-#define BMI160_RA_OFFSET_2          0x73
-#define BMI160_RA_OFFSET_3          0x74
-#define BMI160_RA_OFFSET_4          0x75
-#define BMI160_RA_OFFSET_5          0x76
-#define BMI160_RA_OFFSET_6          0x77
-
-#define BMI160_RA_STEP_CNT_L        0x78
-#define BMI160_RA_STEP_CNT_H        0x79
-
-#define BMI160_STEP_BUF_MIN_BIT     0
-#define BMI160_STEP_BUF_MIN_LEN     3
-#define BMI160_STEP_CNT_EN_BIT      3
-
-#define BMI160_STEP_TIME_MIN_BIT    0
-#define BMI160_STEP_TIME_MIN_LEN    3
-#define BMI160_STEP_THRESH_MIN_BIT  3
-#define BMI160_STEP_THRESH_MIN_LEN  2
-#define BMI160_STEP_ALPHA_BIT       5
-#define BMI160_STEP_ALPHA_LEN       3
-
-#define BMI160_RA_STEP_CONF_0       0x7A
-#define BMI160_RA_STEP_CONF_1       0x7B
-
-#define BMI160_RA_STEP_CONF_0_NOR   0x15
-#define BMI160_RA_STEP_CONF_0_SEN   0x2D
-#define BMI160_RA_STEP_CONF_0_ROB   0x1D
-#define BMI160_RA_STEP_CONF_1_NOR   0x03
-#define BMI160_RA_STEP_CONF_1_SEN   0x00
-#define BMI160_RA_STEP_CONF_1_ROB   0x07
-
-
-#define BMI160_GYRO_RANGE_SEL_BIT   0
-#define BMI160_GYRO_RANGE_SEL_LEN   3
-
-#define BMI160_GYRO_RATE_SEL_BIT    0
-#define BMI160_GYRO_RATE_SEL_LEN    4
-
-#define BMI160_GYRO_DLPF_SEL_BIT    4
-#define BMI160_GYRO_DLPF_SEL_LEN    2
-
-#define BMI160_ACCEL_DLPF_SEL_BIT   4
-#define BMI160_ACCEL_DLPF_SEL_LEN   3
-
-#define BMI160_ACCEL_RANGE_SEL_BIT  0
-#define BMI160_ACCEL_RANGE_SEL_LEN  4
-
-#define BMI160_CMD_START_FOC        0x03
-#define BMI160_CMD_ACC_MODE_NORMAL  0x11
-#define BMI160_CMD_GYR_MODE_NORMAL  0x15
-#define BMI160_CMD_FIFO_FLUSH       0xB0
-#define BMI160_CMD_INT_RESET        0xB1
-#define BMI160_CMD_STEP_CNT_CLR     0xB2
-#define BMI160_CMD_SOFT_RESET       0xB6
-
-#define BMI160_RA_CMD               0x7E
-
-                                                               // mode   parm ext
-#define BMI160_FIFO_HEADER_CTL_SKIP_FRAME               0x40   // 0b01 0 000  00
-#define BMI160_FIFO_HEADER_CTL_SENSOR_TIME              0x44   // 0b01 0 001  00
-#define BMI160_FIFO_HEADER_CTL_INPUT_CONFIG             0x48   // 0b01 0 010  00
-#define BMI160_FIFO_HEADER_DATA_FRAME_BASE              0x80   // 0b10 0 000  00
-#define BMI160_FIFO_HEADER_DATA_FRAME_FLAG_M            1 << 4 // 0b00 0 100  00
-#define BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G            1 << 3 // 0b00 0 010  00
-#define BMI160_FIFO_HEADER_DATA_FRAME_FLAG_A            1 << 2 // 0b00 0 001  00
-#define BMI160_FIFO_HEADER_DATA_FRAME_MASK_HAS_DATA \
-    (BMI160_FIFO_HEADER_DATA_FRAME_FLAG_M |\
-    BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G |\
-    BMI160_FIFO_HEADER_DATA_FRAME_FLAG_A)
-
-#define BMI160_FIFO_SKIP_FRAME_LEN 1
-#define BMI160_FIFO_INPUT_CONFIG_LEN 1
-#define BMI160_FIFO_SENSOR_TIME_LEN 3
-#define BMI160_FIFO_M_LEN 8
-#define BMI160_FIFO_G_LEN 6
-#define BMI160_FIFO_A_LEN 6
-
-#define BMI160_RA_ERR                        0x02
-#define BMI160_ERR_MASK_MAG_DRDY_ERR         0b10000000
-#define BMI160_ERR_MASK_DROP_CMD_ERR         0b01000000
-// datasheet is unclear - reserved or i2c_fail_err?
-// #define BMI160_ERR_MASK_I2C_FAIL             0b00100000
-#define BMI160_ERR_MASK_ERROR_CODE           0b00011110
-#define BMI160_ERR_MASK_CHIP_NOT_OPERABLE    0b00000001
-
-/**
- * Interrupt Latch Mode options
- * @see setInterruptLatch()
- */
-typedef enum {
-    BMI160_LATCH_MODE_NONE = 0, /**< Non-latched */
-    BMI160_LATCH_MODE_312_5_US, /**< Temporary, 312.50 microseconds */
-    BMI160_LATCH_MODE_625_US,   /**< Temporary, 625.00 microseconds */
-    BMI160_LATCH_MODE_1_25_MS,  /**< Temporary,   1.25 milliseconds */
-    BMI160_LATCH_MODE_2_5_MS,   /**< Temporary,   2.50 milliseconds */
-    BMI160_LATCH_MODE_5_MS,     /**< Temporary,   5.00 milliseconds */
-    BMI160_LATCH_MODE_10_MS,    /**< Temporary,  10.00 milliseconds */
-    BMI160_LATCH_MODE_20_MS,    /**< Temporary,  20.00 milliseconds */
-    BMI160_LATCH_MODE_40_MS,    /**< Temporary,  40.00 milliseconds */
-    BMI160_LATCH_MODE_80_MS,    /**< Temporary,  80.00 milliseconds */
-    BMI160_LATCH_MODE_160_MS,   /**< Temporary, 160.00 milliseconds */
-    BMI160_LATCH_MODE_320_MS,   /**< Temporary, 320.00 milliseconds */
-    BMI160_LATCH_MODE_640_MS,   /**< Temporary, 640.00 milliseconds */
-    BMI160_LATCH_MODE_1_28_S,   /**< Temporary,   1.28 seconds      */
-    BMI160_LATCH_MODE_2_56_S,   /**< Temporary,   2.56 seconds      */
-    BMI160_LATCH_MODE_LATCH,    /**< Latched, @see resetInterrupt() */
-} BMI160InterruptLatchMode;
-
-/**
- * Digital Low-Pass Filter Mode options
- * @see setGyroDLPFMode()
- * @see setAccelDLPFMode()
- */
-typedef enum {
-    BMI160_DLPF_MODE_NORM = 0x2,
-    BMI160_DLPF_MODE_OSR2 = 0x1,
-    BMI160_DLPF_MODE_OSR4 = 0x0,
-} BMI160DLPFMode;
-
-/**
- * Accelerometer Sensitivity Range options
- * @see setFullScaleAccelRange()
- */
-typedef enum {
-    BMI160_ACCEL_RANGE_2G  = 0x03, /**<  +/-  2g range */
-    BMI160_ACCEL_RANGE_4G  = 0x05, /**<  +/-  4g range */
-    BMI160_ACCEL_RANGE_8G  = 0x08, /**<  +/-  8g range */
-    BMI160_ACCEL_RANGE_16G = 0x0C, /**<  +/- 16g range */
-} BMI160AccelRange;
-
-/**
- * Gyroscope Sensitivity Range options
- * @see setFullScaleGyroRange()
- */
-typedef enum {
-    BMI160_GYRO_RANGE_2000 = 0, /**<  +/- 2000 degrees/second */
-    BMI160_GYRO_RANGE_1000,     /**<  +/- 1000 degrees/second */
-    BMI160_GYRO_RANGE_500,      /**<  +/-  500 degrees/second */
-    BMI160_GYRO_RANGE_250,      /**<  +/-  250 degrees/second */
-    BMI160_GYRO_RANGE_125,      /**<  +/-  125 degrees/second */
-} BMI160GyroRange;
-
-/**
- * Accelerometer Output Data Rate options
- * @see setAccelRate()
- */
-typedef enum {
-    BMI160_ACCEL_RATE_25_2HZ = 5,  /**<   25/2  Hz */
-    BMI160_ACCEL_RATE_25HZ,        /**<   25    Hz */
-    BMI160_ACCEL_RATE_50HZ,        /**<   50    Hz */
-    BMI160_ACCEL_RATE_100HZ,       /**<  100    Hz */
-    BMI160_ACCEL_RATE_200HZ,       /**<  200    Hz */
-    BMI160_ACCEL_RATE_400HZ,       /**<  400    Hz */
-    BMI160_ACCEL_RATE_800HZ,       /**<  800    Hz */
-    BMI160_ACCEL_RATE_1600HZ,      /**< 1600    Hz */
-} BMI160AccelRate;
-
-/**
- * Gyroscope Output Data Rate options
- * @see setGyroRate()
- */
-typedef enum {
-    BMI160_GYRO_RATE_25HZ = 6,     /**<   25    Hz */
-    BMI160_GYRO_RATE_50HZ,         /**<   50    Hz */
-    BMI160_GYRO_RATE_100HZ,        /**<  100    Hz */
-    BMI160_GYRO_RATE_200HZ,        /**<  200    Hz */
-    BMI160_GYRO_RATE_400HZ,        /**<  400    Hz */
-    BMI160_GYRO_RATE_800HZ,        /**<  800    Hz */
-    BMI160_GYRO_RATE_1600HZ,       /**< 1600    Hz */
-    BMI160_GYRO_RATE_3200HZ,       /**< 3200    Hz */
-} BMI160GyroRate;
-
-/**
- * Step Detection Mode options
- * @see setStepDetectionMode()
- */
-typedef enum {
-    BMI160_STEP_MODE_NORMAL = 0,
-    BMI160_STEP_MODE_SENSITIVE,
-    BMI160_STEP_MODE_ROBUST,
-    BMI160_STEP_MODE_UNKNOWN,
-} BMI160StepMode;
-
-/**
- * Tap Detection Shock Duration options
- * @see setTapShockDuration()
- */
-typedef enum {
-    BMI160_TAP_SHOCK_DURATION_50MS = 0,
-    BMI160_TAP_SHOCK_DURATION_75MS,
-} BMI160TapShockDuration;
-
-/**
- * Tap Detection Quiet Duration options
- * @see setTapQuietDuration()
- */
-typedef enum {
-    BMI160_TAP_QUIET_DURATION_30MS = 0,
-    BMI160_TAP_QUIET_DURATION_20MS,
-} BMI160TapQuietDuration;
-
-/**
- * Double-Tap Detection Duration options
- * @see setDoubleTapDetectionDuration()
- */
-typedef enum {
-    BMI160_DOUBLE_TAP_DURATION_50MS = 0,
-    BMI160_DOUBLE_TAP_DURATION_100MS,
-    BMI160_DOUBLE_TAP_DURATION_150MS,
-    BMI160_DOUBLE_TAP_DURATION_200MS,
-    BMI160_DOUBLE_TAP_DURATION_250MS,
-    BMI160_DOUBLE_TAP_DURATION_375MS,
-    BMI160_DOUBLE_TAP_DURATION_500MS,
-    BMI160_DOUBLE_TAP_DURATION_700MS,
-} BMI160DoubleTapDuration;
-
-/**
- * Zero-Motion Detection Duration options
- * @see setZeroMotionDetectionDuration()
- */
-typedef enum {
-    BMI160_ZERO_MOTION_DURATION_1_28S   = 0x00, /**<   1.28 seconds */
-    BMI160_ZERO_MOTION_DURATION_2_56S,          /**<   2.56 seconds */
-    BMI160_ZERO_MOTION_DURATION_3_84S,          /**<   3.84 seconds */
-    BMI160_ZERO_MOTION_DURATION_5_12S,          /**<   5.12 seconds */
-    BMI160_ZERO_MOTION_DURATION_6_40S,          /**<   6.40 seconds */
-    BMI160_ZERO_MOTION_DURATION_7_68S,          /**<   7.68 seconds */
-    BMI160_ZERO_MOTION_DURATION_8_96S,          /**<   8.96 seconds */
-    BMI160_ZERO_MOTION_DURATION_10_24S,         /**<  10.24 seconds */
-    BMI160_ZERO_MOTION_DURATION_11_52S,         /**<  11.52 seconds */
-    BMI160_ZERO_MOTION_DURATION_12_80S,         /**<  12.80 seconds */
-    BMI160_ZERO_MOTION_DURATION_14_08S,         /**<  14.08 seconds */
-    BMI160_ZERO_MOTION_DURATION_15_36S,         /**<  15.36 seconds */
-    BMI160_ZERO_MOTION_DURATION_16_64S,         /**<  16.64 seconds */
-    BMI160_ZERO_MOTION_DURATION_17_92S,         /**<  17.92 seconds */
-    BMI160_ZERO_MOTION_DURATION_19_20S,         /**<  19.20 seconds */
-    BMI160_ZERO_MOTION_DURATION_20_48S,         /**<  20.48 seconds */
-    BMI160_ZERO_MOTION_DURATION_25_60S  = 0x10, /**<  25.60 seconds */
-    BMI160_ZERO_MOTION_DURATION_30_72S,         /**<  30.72 seconds */
-    BMI160_ZERO_MOTION_DURATION_35_84S,         /**<  35.84 seconds */
-    BMI160_ZERO_MOTION_DURATION_40_96S,         /**<  40.96 seconds */
-    BMI160_ZERO_MOTION_DURATION_46_08S,         /**<  46.08 seconds */
-    BMI160_ZERO_MOTION_DURATION_51_20S,         /**<  51.20 seconds */
-    BMI160_ZERO_MOTION_DURATION_56_32S,         /**<  56.32 seconds */
-    BMI160_ZERO_MOTION_DURATION_61_44S,         /**<  61.44 seconds */
-    BMI160_ZERO_MOTION_DURATION_66_56S,         /**<  66.56 seconds */
-    BMI160_ZERO_MOTION_DURATION_71_68S,         /**<  71.68 seconds */
-    BMI160_ZERO_MOTION_DURATION_76_80S,         /**<  76.80 seconds */
-    BMI160_ZERO_MOTION_DURATION_81_92S,         /**<  81.92 seconds */
-    BMI160_ZERO_MOTION_DURATION_87_04S,         /**<  87.04 seconds */
-    BMI160_ZERO_MOTION_DURATION_92_16S,         /**<  92.16 seconds */
-    BMI160_ZERO_MOTION_DURATION_97_28S,         /**<  97.28 seconds */
-    BMI160_ZERO_MOTION_DURATION_102_40S,        /**< 102.40 seconds */
-    BMI160_ZERO_MOTION_DURATION_112_64S = 0x20, /**< 112.64 seconds */
-    BMI160_ZERO_MOTION_DURATION_122_88S,        /**< 122.88 seconds */
-    BMI160_ZERO_MOTION_DURATION_133_12S,        /**< 133.12 seconds */
-    BMI160_ZERO_MOTION_DURATION_143_36S,        /**< 143.36 seconds */
-    BMI160_ZERO_MOTION_DURATION_153_60S,        /**< 153.60 seconds */
-    BMI160_ZERO_MOTION_DURATION_163_84S,        /**< 163.84 seconds */
-    BMI160_ZERO_MOTION_DURATION_174_08S,        /**< 174.08 seconds */
-    BMI160_ZERO_MOTION_DURATION_184_32S,        /**< 184.32 seconds */
-    BMI160_ZERO_MOTION_DURATION_194_56S,        /**< 194.56 seconds */
-    BMI160_ZERO_MOTION_DURATION_204_80S,        /**< 204.80 seconds */
-    BMI160_ZERO_MOTION_DURATION_215_04S,        /**< 215.04 seconds */
-    BMI160_ZERO_MOTION_DURATION_225_28S,        /**< 225.28 seconds */
-    BMI160_ZERO_MOTION_DURATION_235_52S,        /**< 235.52 seconds */
-    BMI160_ZERO_MOTION_DURATION_245_76S,        /**< 245.76 seconds */
-    BMI160_ZERO_MOTION_DURATION_256_00S,        /**< 256.00 seconds */
-    BMI160_ZERO_MOTION_DURATION_266_24S,        /**< 266.24 seconds */
-    BMI160_ZERO_MOTION_DURATION_276_48S,        /**< 276.48 seconds */
-    BMI160_ZERO_MOTION_DURATION_286_72S,        /**< 286.72 seconds */
-    BMI160_ZERO_MOTION_DURATION_296_96S,        /**< 296.96 seconds */
-    BMI160_ZERO_MOTION_DURATION_307_20S,        /**< 307.20 seconds */
-    BMI160_ZERO_MOTION_DURATION_317_44S,        /**< 317.44 seconds */
-    BMI160_ZERO_MOTION_DURATION_327_68S,        /**< 327.68 seconds */
-    BMI160_ZERO_MOTION_DURATION_337_92S,        /**< 337.92 seconds */
-    BMI160_ZERO_MOTION_DURATION_348_16S,        /**< 348.16 seconds */
-    BMI160_ZERO_MOTION_DURATION_358_40S,        /**< 358.40 seconds */
-    BMI160_ZERO_MOTION_DURATION_368_64S,        /**< 368.64 seconds */
-    BMI160_ZERO_MOTION_DURATION_378_88S,        /**< 378.88 seconds */
-    BMI160_ZERO_MOTION_DURATION_389_12S,        /**< 389.12 seconds */
-    BMI160_ZERO_MOTION_DURATION_399_36S,        /**< 399.36 seconds */
-    BMI160_ZERO_MOTION_DURATION_409_60S,        /**< 409.60 seconds */
-    BMI160_ZERO_MOTION_DURATION_419_84S,        /**< 419.84 seconds */
-    BMI160_ZERO_MOTION_DURATION_430_08S,        /**< 430.08 seconds */
-} BMI160ZeroMotionDuration;
-
-class BMI160 {
-    public:
-        BMI160();
-        void initialize(
-            uint8_t addr,
-            BMI160GyroRate gyroRate = BMI160_GYRO_RATE_800HZ,
-            BMI160GyroRange gyroRange = BMI160_GYRO_RANGE_500,
-            BMI160DLPFMode gyroFilterMode = BMI160_DLPF_MODE_NORM,
-            BMI160AccelRate accelRate = BMI160_ACCEL_RATE_800HZ,
-            BMI160AccelRange accelRange = BMI160_ACCEL_RANGE_4G,
-            BMI160DLPFMode accelFilterMode = BMI160_DLPF_MODE_OSR4
-        );
-        bool testConnection();
-
-        uint8_t getGyroRate();
-        void setGyroRate(uint8_t rate);
-
-        uint8_t getAccelRate();
-        void setAccelRate(uint8_t rate);
-
-        uint8_t getGyroDLPFMode();
-        void setGyroDLPFMode(uint8_t bandwidth);
-
-        uint8_t getAccelDLPFMode();
-        void setAccelDLPFMode(uint8_t bandwidth);
-
-        uint8_t getFullScaleGyroRange();
-        void setFullScaleGyroRange(uint8_t range);
-        uint8_t getFullScaleAccelRange();
-        void setFullScaleAccelRange(uint8_t range);
-
-        void autoCalibrateGyroOffset();
-        bool getGyroOffsetEnabled();
-        void setGyroOffsetEnabled(bool enabled);
-
-        int16_t getXGyroOffset();
-        void setXGyroOffset(int16_t offset);
-        int16_t getYGyroOffset();
-        void setYGyroOffset(int16_t offset);
-        int16_t getZGyroOffset();
-        void setZGyroOffset(int16_t offset);
-
-        void autoCalibrateXAccelOffset(int target);
-        void autoCalibrateYAccelOffset(int target);
-        void autoCalibrateZAccelOffset(int target);
-        bool getAccelOffsetEnabled();
-        void setAccelOffsetEnabled(bool enabled);
-
-        int8_t getXAccelOffset();
-        void setXAccelOffset(int8_t offset);
-        int8_t getYAccelOffset();
-        void setYAccelOffset(int8_t offset);
-        int8_t getZAccelOffset();
-        void setZAccelOffset(int8_t offset);
-
-        uint8_t getFreefallDetectionThreshold();
-        void setFreefallDetectionThreshold(uint8_t threshold);
-
-        uint8_t getFreefallDetectionDuration();
-        void setFreefallDetectionDuration(uint8_t duration);
-
-        uint8_t getShockDetectionThreshold();
-        void setShockDetectionThreshold(uint8_t threshold);
-
-        uint8_t getShockDetectionDuration();
-        void setShockDetectionDuration(uint8_t duration);
-
-        uint8_t getMotionDetectionThreshold();
-        void setMotionDetectionThreshold(uint8_t threshold);
-
-        uint8_t getMotionDetectionDuration();
-        void setMotionDetectionDuration(uint8_t duration);
-
-        uint8_t getZeroMotionDetectionThreshold();
-        void setZeroMotionDetectionThreshold(uint8_t threshold);
-
-        uint8_t getZeroMotionDetectionDuration();
-        void setZeroMotionDetectionDuration(uint8_t duration);
-
-        uint8_t getTapDetectionThreshold();
-        void setTapDetectionThreshold(uint8_t threshold);
-
-        bool getTapShockDuration();
-        void setTapShockDuration(bool duration);
-
-        bool getTapQuietDuration();
-        void setTapQuietDuration(bool duration);
-
-        uint8_t getDoubleTapDetectionDuration();
-        void setDoubleTapDetectionDuration(uint8_t duration);
-
-        uint8_t getStepDetectionMode();
-        void setStepDetectionMode(BMI160StepMode mode);
-        bool getStepCountEnabled();
-        void setStepCountEnabled(bool enabled);
-        uint16_t getStepCount();
-        void resetStepCount();
-
-        bool getIntFreefallEnabled();
-        void setIntFreefallEnabled(bool enabled);
-        bool getIntShockEnabled();
-        void setIntShockEnabled(bool enabled);
-        bool getIntStepEnabled();
-        void setIntStepEnabled(bool enabled);
-        bool getIntMotionEnabled();
-        void setIntMotionEnabled(bool enabled);
-        bool getIntZeroMotionEnabled();
-        void setIntZeroMotionEnabled(bool enabled);
-        bool getIntTapEnabled();
-        void setIntTapEnabled(bool enabled);
-        bool getIntDoubleTapEnabled();
-        void setIntDoubleTapEnabled(bool enabled);
-
-        bool getGyroFIFOEnabled();
-        void setGyroFIFOEnabled(bool enabled);
-        bool getAccelFIFOEnabled();
-        void setAccelFIFOEnabled(bool enabled);
-        bool getMagFIFOEnabled();
-        void setMagFIFOEnabled(bool enabled);
-
-        bool getIntFIFOBufferFullEnabled();
-        void setIntFIFOBufferFullEnabled(bool enabled);
-        bool getIntDataReadyEnabled();
-        void setIntDataReadyEnabled(bool enabled);
-
-        uint8_t getIntStatus0();
-        uint8_t getIntStatus1();
-        uint8_t getIntStatus2();
-        uint8_t getIntStatus3();
-        bool getIntFreefallStatus();
-        bool getIntShockStatus();
-        bool getIntStepStatus();
-        bool getIntMotionStatus();
-        bool getIntZeroMotionStatus();
-        bool getIntTapStatus();
-        bool getIntDoubleTapStatus();
-        bool getIntFIFOBufferFullStatus();
-        bool getIntDataReadyStatus();
-
-        void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
-        void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
-        int16_t getAccelerationX();
-        int16_t getAccelerationY();
-        int16_t getAccelerationZ();
-
-        void getMagnetometer(int16_t* mx, int16_t* my, int16_t* mz);
-        void getMagnetometerXYZBuffer(uint8_t* data);
-
-        bool getTemperature(int16_t* out);
-
-        void getRotation(int16_t* x, int16_t* y, int16_t* z);
-        int16_t getRotationX();
-        int16_t getRotationY();
-        int16_t getRotationZ();
-
-        bool getXNegShockDetected();
-        bool getXPosShockDetected();
-        bool getYNegShockDetected();
-        bool getYPosShockDetected();
-        bool getZNegShockDetected();
-        bool getZPosShockDetected();
-
-        bool getXNegMotionDetected();
-        bool getXPosMotionDetected();
-        bool getYNegMotionDetected();
-        bool getYPosMotionDetected();
-        bool getZNegMotionDetected();
-        bool getZPosMotionDetected();
-
-        bool getXNegTapDetected();
-        bool getXPosTapDetected();
-        bool getYNegTapDetected();
-        bool getYPosTapDetected();
-        bool getZNegTapDetected();
-        bool getZPosTapDetected();
-
-        bool getFIFOHeaderModeEnabled();
-        void setFIFOHeaderModeEnabled(bool enabled);
-        void resetFIFO();
-
-        bool getFIFOCount(uint16_t* outCount);
-        bool getFIFOBytes(uint8_t *data, uint16_t length);
-
-        uint8_t getDeviceID();
-
-        uint8_t getRegister(uint8_t reg);
-        void setRegister(uint8_t reg, uint8_t data);
-
-        bool getIntEnabled();
-        void setIntEnabled(bool enabled);
-        bool getInterruptMode();
-        void setInterruptMode(bool mode);
-        bool getInterruptDrive();
-        void setInterruptDrive(bool drive);
-        uint8_t getInterruptLatch();
-        void setInterruptLatch(uint8_t latch);
-        void resetInterrupt();
-        
-        bool getGyroDrdy();
-        void waitForGyroDrdy();
-        void waitForAccelDrdy();
-        void waitForMagDrdy();
-        bool getSensorTime(uint32_t *v_sensor_time_u32);
-        void setMagDeviceAddress(uint8_t addr);
-        void setMagRegister(uint8_t addr, uint8_t value);
-
-        bool getErrReg(uint8_t* out);
-    private:
-        uint8_t buffer[14];
-        uint8_t devAddr;
-};
-
-#endif /* _BMI160_H_ */

lib/bno080/BNO080.cpp

@@ -12,7 +12,6 @@
 
   This library handles the initialization of the BNO080 and is able to query the sensor
   for different readings.
-
   https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library
 
   Development environment specifics:
@@ -43,14 +42,14 @@
 
 //Attempt communication with the device
 //Return true if we got a 'Polo' back from Marco
-boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, uint8_t intPin)
+boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, PinInterface* intPin)
 {
 	_deviceAddress = deviceAddress; //If provided, store the I2C address from user
 	_i2cPort = &wirePort;			//Grab which port the user wants us to use
 	_int = intPin;					//Get the pin that the user wants to use for interrupts. By default, it's 255 and we'll not use it in dataAvailable() function.
-	if (_int != 255)
+	if (_int != nullptr)
 	{
-		pinMode(_int, INPUT_PULLUP);
+		_int->pinMode(INPUT_PULLUP);
 	}
 
 	//We expect caller to begin their I2C port, with the speed of their choice external to the library
@@ -99,7 +98,7 @@ boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, uint8_t intPin)
 	return tBoardInfoReceived;
 }
 
-boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_INTPin, uint8_t user_RSTPin, uint32_t spiPortSpeed, SPIClass &spiPort)
+boolean BNO080::beginSPI(PinInterface* user_CSPin, PinInterface* user_WAKPin, PinInterface* user_INTPin, PinInterface* user_RSTPin, uint32_t spiPortSpeed, SPIClass &spiPort)
 {
 	_i2cPort = NULL; //This null tells the send/receive functions to use SPI
 
@@ -114,18 +113,18 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I
 	_int = user_INTPin;
 	_rst = user_RSTPin;
 
-	pinMode(_cs, OUTPUT);
-	pinMode(_wake, OUTPUT);
-	pinMode(_int, INPUT_PULLUP);
-	pinMode(_rst, OUTPUT);
+	_cs->pinMode(OUTPUT);
+	_wake->pinMode(OUTPUT);
+	_int->pinMode(INPUT_PULLUP);
+	_rst->pinMode(OUTPUT);
 
-	digitalWrite(_cs, HIGH); //Deselect BNO080
+	_cs->digitalWrite(HIGH); //Deselect BNO080
 
 	//Configure the BNO080 for SPI communication
-	digitalWrite(_wake, HIGH); //Before boot up the PS0/WAK pin must be high to enter SPI mode
-	digitalWrite(_rst, LOW);   //Reset BNO080
+	_wake->digitalWrite(HIGH); //Before boot up the PS0/WAK pin must be high to enter SPI mode
+	_rst->digitalWrite(LOW);   //Reset BNO080
 	delay(2);				   //Min length not specified in datasheet?
-	digitalWrite(_rst, HIGH);  //Bring out of reset
+	_rst->digitalWrite(HIGH);  //Bring out of reset
 
 	//Wait for first assertion of INT before using WAK pin. Can take ~104ms
 	waitForSPI();
@@ -203,9 +202,9 @@ uint16_t BNO080::getReadings(void)
 	//If we have an interrupt pin connection available, check if data is available.
 	//If int pin is not set, then we'll rely on receivePacket() to timeout
 	//See issue 13: https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/issues/13
-	if (_int != 255)
+	if (_int != nullptr)
 	{
-		if (digitalRead(_int) == HIGH)
+		if (_int->digitalRead() == HIGH)
 			return 0;
 	}
 
@@ -256,6 +255,13 @@ uint16_t BNO080::parseCommandReport(void)
 		if (command == COMMAND_ME_CALIBRATE)
 		{
 			calibrationStatus = shtpData[5 + 0]; //R0 - Status (0 = success, non-zero = fail)
+			_calibrationResponseStatus = shtpData[5 + 0];
+			_accelCalEnabled = shtpData[6 + 0];
+			_gyroCalEnabled = shtpData[7 + 0];
+			_magCalEnabled = shtpData[8 + 0];
+			_planarAccelCalEnabled = shtpData[9 + 0];
+			_onTableCalEnabled = shtpData[10 + 0];
+			_hasNewCalibrationStatus = true;
 		}
 		return shtpData[0];
 	}
@@ -278,12 +284,13 @@ uint16_t BNO080::parseCommandReport(void)
 //shtpData[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector)
 //shtpData[5 + 1]: Sequence number (See 6.5.18.2)
 //shtpData[5 + 2]: Status
-//shtpData[3]: Delay
-//shtpData[4:5]: i/accel x/gyro x/etc
-//shtpData[6:7]: j/accel y/gyro y/etc
-//shtpData[8:9]: k/accel z/gyro z/etc
-//shtpData[10:11]: real/gyro temp/etc
-//shtpData[12:13]: Accuracy estimate
+//shtpData[5 + 3]: Delay
+//shtpData[5 + 4:5]: i/accel x/gyro x/etc
+//shtpData[5 + 6:7]: j/accel y/gyro y/etc
+//shtpData[5 + 8:9]: k/accel z/gyro z/etc
+//shtpData[5 + 10:11]: real/gyro temp/etc
+//shtpData[5 + 12:13]: Accuracy estimate: Raw Accel/Gyro/Mag Timestap part1
+//shtpData[5 + 14:15]: Raw Accel/Gyro/Mag Timestap part2
 uint16_t BNO080::parseInputReport(void)
 {
 	//Calculate the number of data bytes in this packet
@@ -304,7 +311,7 @@ uint16_t BNO080::parseInputReport(void)
 		rawFastGyroX = (uint16_t)shtpData[9] << 8 | shtpData[8];
 		rawFastGyroY = (uint16_t)shtpData[11] << 8 | shtpData[10];
 		rawFastGyroZ = (uint16_t)shtpData[13] << 8 | shtpData[12];
-
+		hasNewFastGyro_ = true;
 		return SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR;
 	}
 
@@ -314,6 +321,7 @@ uint16_t BNO080::parseInputReport(void)
 	uint16_t data3 = (uint16_t)shtpData[5 + 9] << 8 | shtpData[5 + 8];
 	uint16_t data4 = 0;
 	uint16_t data5 = 0; //We would need to change this to uin32_t to capture time stamp value on Raw Accel/Gyro/Mag reports
+	uint32_t memstimeStamp = 0; //Timestamp of MEMS sensor reading
 
 	if (dataLength - 5 > 9)
 	{
@@ -323,6 +331,11 @@ uint16_t BNO080::parseInputReport(void)
 	{
 		data5 = (uint16_t)shtpData[5 + 13] << 8 | shtpData[5 + 12];
 	}
+	//only for Raw Reports 0x14, 0x15, 0x16
+	if (dataLength - 5 >= 15)
+	{
+		memstimeStamp = ((uint32_t)shtpData[5 + 15] << (8 * 3)) | ((uint32_t)shtpData[5 + 14] << (8 * 2)) | ((uint32_t)shtpData[5 + 13] << (8 * 1)) | ((uint32_t)shtpData[5 + 12] << (8 * 0));
+	}
 
 	//Store these generic values to their proper global variable
 	if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER || shtpData[5] == SENSOR_REPORTID_GRAVITY)
@@ -335,6 +348,7 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_LINEAR_ACCELERATION)
 	{
+		hasNewLinAccel_ = true;
 		accelLinAccuracy = status;
 		rawLinAccelX = data1;
 		rawLinAccelY = data2;
@@ -342,6 +356,7 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE)
 	{
+		hasNewGyro_ = true;
 		gyroAccuracy = status;
 		rawGyroX = data1;
 		rawGyroY = data2;
@@ -349,6 +364,7 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_MAGNETIC_FIELD)
 	{
+		hasNewMag_ = true;
 		magAccuracy = status;
 		rawMagX = data1;
 		rawMagY = data2;
@@ -411,21 +427,28 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_RAW_ACCELEROMETER)
 	{
+		hasNewRawAccel_ = true;
 		memsRawAccelX = data1;
 		memsRawAccelY = data2;
 		memsRawAccelZ = data3;
+		memsAccelTimeStamp = memstimeStamp;
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_RAW_GYROSCOPE)
 	{
+		hasNewRawGyro_ = true;
 		memsRawGyroX = data1;
 		memsRawGyroY = data2;
 		memsRawGyroZ = data3;
+		memsRawGyroTemp = data4;
+		memsGyroTimeStamp = memstimeStamp;
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_RAW_MAGNETOMETER)
 	{
+		hasNewRawMag_ = true;
 		memsRawMagX = data1;
 		memsRawMagY = data2;
 		memsRawMagZ = data3;
+		memsMagTimeStamp = memstimeStamp;
 	}
 	else if (shtpData[5] == SHTP_REPORT_COMMAND_RESPONSE)
 	{
@@ -545,6 +568,16 @@ void BNO080::getQuat(float &i, float &j, float &k, float &real, float &radAccura
 	hasNewQuaternion = false;
 }
 
+bool BNO080::getNewQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
+{
+	if (hasNewQuaternion)
+	{
+		getQuat(i, j, k, real, radAccuracy, accuracy);
+		return true;
+	}
+	return false;
+}
+
 void BNO080::getGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy)
 {
 	i = qToFloat(rawGameQuatI, rotationVector_Q1);
@@ -555,6 +588,16 @@ void BNO080::getGameQuat(float &i, float &j, float &k, float &real, uint8_t &acc
 	hasNewGameQuaternion = false;
 }
 
+bool BNO080::getNewGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy)
+{
+	if (hasNewGameQuaternion)
+	{
+		getGameQuat(i, j, k, real, accuracy);
+		return true;
+	}
+	return false;
+}
+
 void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
 {
 	i = qToFloat(rawMagQuatI, rotationVector_Q1);
@@ -566,6 +609,16 @@ void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAcc
 	hasNewMagQuaternion = false;
 }
 
+bool BNO080::getNewMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
+{
+	if (hasNewMagQuaternion)
+	{
+		getMagQuat(i, j, k, real, radAccuracy, accuracy);
+		return true;
+	}
+	return false;
+}
+
 bool BNO080::hasNewQuat() {
 	return hasNewQuaternion;
 }
@@ -634,6 +687,16 @@ void BNO080::getAccel(float &x, float &y, float &z, uint8_t &accuracy)
 	hasNewAccel_ = false;
 }
 
+bool BNO080::getNewAccel(float &x, float &y, float &z, uint8_t &accuracy)
+{
+	if (hasNewAccel_)
+	{
+		getAccel(x, y, z, accuracy);
+		return true;
+	}
+	return false;
+}
+
 //Return the acceleration component
 float BNO080::getAccelX()
 {
@@ -671,6 +734,21 @@ void BNO080::getLinAccel(float &x, float &y, float &z, uint8_t &accuracy)
 	y = qToFloat(rawLinAccelY, linear_accelerometer_Q1);
 	z = qToFloat(rawLinAccelZ, linear_accelerometer_Q1);
 	accuracy = accelLinAccuracy;
+	hasNewLinAccel_ = false;
+}
+
+bool BNO080::getNewLinAccel(float &x, float &y, float &z, uint8_t &accuracy)
+{
+	if (hasNewLinAccel_)
+	{
+		getLinAccel(x, y, z, accuracy);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewLinAccel() {
+	return hasNewLinAccel_;
 }
 
 //Return the acceleration component
@@ -708,6 +786,7 @@ void BNO080::getGyro(float &x, float &y, float &z, uint8_t &accuracy)
 	y = qToFloat(rawGyroY, gyro_Q1);
 	z = qToFloat(rawGyroZ, gyro_Q1);
 	accuracy = gyroAccuracy;
+	hasNewGyro_ = false;
 }
 
 //Return the gyro component
@@ -737,6 +816,10 @@ uint8_t BNO080::getGyroAccuracy()
 	return (gyroAccuracy);
 }
 
+bool BNO080::hasNewGyro() {
+	return hasNewGyro_;
+}
+
 //Gets the full mag vector
 //x,y,z output floats
 void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy)
@@ -745,6 +828,7 @@ void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy)
 	y = qToFloat(rawMagY, magnetometer_Q1);
 	z = qToFloat(rawMagZ, magnetometer_Q1);
 	accuracy = magAccuracy;
+	hasNewMag_=false;
 }
 
 //Return the magnetometer component
@@ -774,6 +858,10 @@ uint8_t BNO080::getMagAccuracy()
 	return (magAccuracy);
 }
 
+bool BNO080::hasNewMag() {
+	return hasNewMag_;
+}
+
 //Gets the full high rate gyro vector
 //x,y,z output floats
 void BNO080::getFastGyro(float &x, float &y, float &z)
@@ -804,6 +892,10 @@ float BNO080::getFastGyroZ()
 	return (gyro);
 }
 
+bool BNO080::hasNewFastGyro() {
+	return hasNewFastGyro_;
+}
+
 //Return the tap detector
 uint8_t BNO080::getTapDetector()
 {
@@ -857,6 +949,29 @@ int16_t BNO080::getRawAccelZ()
 	return (memsRawAccelZ);
 }
 
+void BNO080::getRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	x = memsRawAccelX;
+	y = memsRawAccelY;
+	z = memsRawAccelZ;
+	timeStamp = memsAccelTimeStamp;
+	hasNewRawAccel_ = false;
+}
+
+bool BNO080::getNewRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	if (hasNewRawAccel_)
+	{
+		getRawAccel(x, y, z, timeStamp);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewRawAccel() {
+	return hasNewRawAccel_;
+}
+
 //Return raw mems value for the gyro
 int16_t BNO080::getRawGyroX()
 {
@@ -871,6 +986,42 @@ int16_t BNO080::getRawGyroZ()
 	return (memsRawGyroZ);
 }
 
+// From https://github.com/ceva-dsp/sh2/issues/15
+// Raw gyro temperature for BNO085 uses BMI055 gyro
+// memsRawGyroTemp is in 23°C + 0.5°C/LSB
+float BNO080::getGyroTemp()
+{
+	return (23.0 + (0.5f * memsRawGyroTemp));
+}
+
+void BNO080::resetNewRawGyro()
+{
+	hasNewRawGyro_ = false;
+}
+
+void BNO080::getRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	x = memsRawGyroX;
+	y = memsRawGyroY;
+	z = memsRawGyroZ;
+	timeStamp = memsGyroTimeStamp;
+	hasNewRawGyro_ = false;
+}
+
+bool BNO080::getNewRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	if (hasNewRawGyro_)
+	{
+		getRawGyro(x, y, z, timeStamp);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewRawGyro() {
+	return hasNewRawGyro_;
+}
+
 //Return raw mems value for the mag
 int16_t BNO080::getRawMagX()
 {
@@ -885,6 +1036,29 @@ int16_t BNO080::getRawMagZ()
 	return (memsRawMagZ);
 }
 
+void BNO080::getRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	x = memsRawMagX;
+	y = memsRawMagY;
+	z = memsRawMagZ;
+	timeStamp = memsMagTimeStamp;
+	hasNewRawMag_ = false;
+}
+
+bool BNO080::getNewRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	if (hasNewRawMag_)
+	{
+		getRawMag(x, y, z, timeStamp);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewRawMag() {
+	return hasNewRawMag_;
+}
+
 //Given a record ID, read the Q1 value from the metaData record in the FRS (ya, it's complicated)
 //Q1 is used for all sensor data calculations
 int16_t BNO080::getQ1(uint16_t recordID)
@@ -1381,6 +1555,7 @@ void BNO080::sendCalibrateCommand(uint8_t thingToCalibrate)
 
 	//Make the internal calStatus variable non-zero (operation failed) so that user can test while we wait
 	calibrationStatus = 1;
+	_hasNewCalibrationStatus  = false;
 
 	//Using this shtpData packet, send a command
 	sendCommand(COMMAND_ME_CALIBRATE);
@@ -1404,7 +1579,7 @@ void BNO080::requestCalibrationStatus()
 		shtpData[x] = 0;
 
 	shtpData[6] = 0x01; //P3 - 0x01 - Subcommand: Get ME Calibration
-
+	_hasNewCalibrationStatus  = false;
 	//Using this shtpData packet, send a command
 	sendCommand(COMMAND_ME_CALIBRATE);
 }
@@ -1430,6 +1605,43 @@ void BNO080::saveCalibration()
 	sendCommand(COMMAND_DCD); //Save DCD command
 }
 
+void BNO080::saveCalibrationPeriodically(bool save)
+{
+	/*shtpData[3] = 0; //P0 - Enable/Disable Periodic DCD Save
+	shtpData[4] = 0; //P1 - Reserved
+	shtpData[5] = 0; //P2 - Reserved
+	shtpData[6] = 0; //P3 - Reserved
+	shtpData[7] = 0; //P4 - Reserved
+	shtpData[8] = 0; //P5 - Reserved
+	shtpData[9] = 0; //P6 - Reserved
+	shtpData[10] = 0; //P7 - Reserved
+	shtpData[11] = 0; //P8 - Reserved*/
+
+	for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array
+		shtpData[x] = 0;
+	shtpData[3] = save ? 1 : 0;
+
+	//Using this shtpData packet, send a command
+	sendCommand(COMMAND_DCD_PERIOD_SAVE); //Save DCD command
+}
+
+bool BNO080::hasNewCalibrationStatus()
+{
+	return _hasNewCalibrationStatus;
+}
+
+void BNO080::getCalibrationStatus(uint8_t &calibrationResponseStatus, uint8_t &accelCalEnabled, uint8_t &gyroCalEnabled, uint8_t &magCalEnabled, uint8_t &planarAccelCalEnabled, uint8_t &onTableCalEnabled)
+{
+	_hasNewCalibrationStatus = false;
+	calibrationResponseStatus = _calibrationResponseStatus;
+	accelCalEnabled = _accelCalEnabled;
+	gyroCalEnabled = _gyroCalEnabled;
+	magCalEnabled = _magCalEnabled;
+	planarAccelCalEnabled = _planarAccelCalEnabled;
+	onTableCalEnabled = _onTableCalEnabled;
+}
+
+
 //Wait a certain time for incoming I2C bytes before giving up
 //Returns false if failed
 boolean BNO080::waitForI2C()
@@ -1460,7 +1672,7 @@ boolean BNO080::waitForSPI()
 {
 	for (uint8_t counter = 0; counter < 125; counter++) //Don't got more than 255
 	{
-		if (digitalRead(_int) == LOW)
+		if (_int->digitalRead() == LOW)
 			return (true);
 		if (_printDebug == true)
 			_debugPort->println(F("SPI Wait"));
@@ -1478,7 +1690,7 @@ boolean BNO080::receivePacket(void)
 {
 	if (_i2cPort == NULL) //Do SPI
 	{
-		if (digitalRead(_int) == HIGH)
+		if (_int->digitalRead() == HIGH)
 			return (false); //Data is not available
 
 		//Old way: if (waitForSPI() == false) return (false); //Something went wrong
@@ -1486,7 +1698,7 @@ boolean BNO080::receivePacket(void)
 		//Get first four bytes to find out how much data we need to read
 
 		_spiPort->beginTransaction(SPISettings(_spiPortSpeed, MSBFIRST, SPI_MODE3));
-		digitalWrite(_cs, LOW);
+		_cs->digitalWrite(LOW);
 
 		//Get the first four bytes, aka the packet header
 		uint8_t packetLSB = _spiPort->transfer(0);
@@ -1521,7 +1733,7 @@ boolean BNO080::receivePacket(void)
 				shtpData[dataSpot] = incoming; //Store data into the shtpData array
 		}
 
-		digitalWrite(_cs, HIGH); //Release BNO080
+		_cs->digitalWrite(HIGH); //Release BNO080
 
 		_spiPort->endTransaction();
 		printPacket();
@@ -1627,7 +1839,7 @@ boolean BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
 		//BNO080 has max CLK of 3MHz, MSB first,
 		//The BNO080 uses CPOL = 1 and CPHA = 1. This is mode3
 		_spiPort->beginTransaction(SPISettings(_spiPortSpeed, MSBFIRST, SPI_MODE3));
-		digitalWrite(_cs, LOW);
+		_cs->digitalWrite(LOW);
 
 		//Send the 4 byte packet header
 		_spiPort->transfer(packetLength & 0xFF);			 //Packet length LSB
@@ -1641,7 +1853,7 @@ boolean BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
 			_spiPort->transfer(shtpData[i]);
 		}
 
-		digitalWrite(_cs, HIGH);
+		_cs->digitalWrite(HIGH);
 		_spiPort->endTransaction();
 	}
 	else //Do I2C

lib/bno080/BNO080.h

@@ -49,6 +49,8 @@
 
 #include <Wire.h>
 #include <SPI.h>
+#include <memory>
+#include "PinInterface.h"
 
 //The default I2C address for the BNO080 on the SparkX breakout is 0x4B. 0x4A is also possible.
 #define BNO080_DEFAULT_ADDRESS 0x4B
@@ -150,8 +152,8 @@ struct BNO080Error {
 class BNO080
 {
 public:
-	boolean begin(uint8_t deviceAddress = BNO080_DEFAULT_ADDRESS, TwoWire &wirePort = Wire, uint8_t intPin = 255); //By default use the default I2C addres, and use Wire port, and don't declare an INT pin
-	boolean beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_INTPin, uint8_t user_RSTPin, uint32_t spiPortSpeed = 3000000, SPIClass &spiPort = SPI);
+	boolean begin(uint8_t deviceAddress = BNO080_DEFAULT_ADDRESS, TwoWire &wirePort = Wire, PinInterface* intPin = nullptr); //By default use the default I2C addres, and use Wire port, and don't declare an INT pin
+	boolean beginSPI(PinInterface* user_CSPin, PinInterface* user_WAKPin, PinInterface* user_INTPin, PinInterface* user_RSTPin, uint32_t spiPortSpeed = 3000000, SPIClass &spiPort = SPI);
 
 	void enableDebugging(Stream &debugPort = Serial); //Turn on debug printing. If user doesn't specify then Serial will be used.
 
@@ -199,8 +201,11 @@ public:
 	bool hasNewGameQuat();
 	bool hasNewMagQuat();
 	void getQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy);
+	bool getNewQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy);
 	void getGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy);
+	bool getNewGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy);
 	void getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy);
+	bool getNewMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy);
 	float getQuatI();
 	float getQuatJ();
 	float getQuatK();
@@ -209,6 +214,7 @@ public:
 	uint8_t getQuatAccuracy();
 
 	void getAccel(float &x, float &y, float &z, uint8_t &accuracy);
+	bool getNewAccel(float &x, float &y, float &z, uint8_t &accuracy);
 	float getAccelX();
 	float getAccelY();
 	float getAccelZ();
@@ -216,32 +222,39 @@ public:
 	bool hasNewAccel();
 
 	void getLinAccel(float &x, float &y, float &z, uint8_t &accuracy);
+	bool getNewLinAccel(float &x, float &y, float &z, uint8_t &accuracy);
 	float getLinAccelX();
 	float getLinAccelY();
 	float getLinAccelZ();
 	uint8_t getLinAccelAccuracy();
+	bool hasNewLinAccel();
 
 	void getGyro(float &x, float &y, float &z, uint8_t &accuracy);
 	float getGyroX();
 	float getGyroY();
 	float getGyroZ();
 	uint8_t getGyroAccuracy();
+	bool hasNewGyro();
 
 	void getFastGyro(float &x, float &y, float &z);
 	float getFastGyroX();
 	float getFastGyroY();
 	float getFastGyroZ();
+	bool hasNewFastGyro();
 
 	void getMag(float &x, float &y, float &z, uint8_t &accuracy);
 	float getMagX();
 	float getMagY();
 	float getMagZ();
 	uint8_t getMagAccuracy();
-	
+	bool hasNewMag();
+
 	void endCalibration();
 	void saveCalibration();
 	void requestCalibrationStatus(); //Sends command to get status
-	boolean calibrationComplete();   //Checks ME Cal response for byte 5, R0 - Status
+	bool calibrationComplete();   //Checks ME Cal response for byte 5, R0 - Status
+	bool hasNewCalibrationStatus();
+	void getCalibrationStatus(uint8_t &calibrationResponseStatus, uint8_t &accelCalEnabled, uint8_t &gyroCalEnabled, uint8_t &magCalEnabled, uint8_t &planarAccelCalEnabled, uint8_t &onTableCalEnabled);
 
 	uint8_t getTapDetector();
 	bool getTapDetected();
@@ -253,14 +266,25 @@ public:
 	int16_t getRawAccelX();
 	int16_t getRawAccelY();
 	int16_t getRawAccelZ();
+	void getRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp);
+	bool getNewRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp);
+	bool hasNewRawAccel();
 
 	int16_t getRawGyroX();
 	int16_t getRawGyroY();
 	int16_t getRawGyroZ();
+	float getGyroTemp();
+	void getRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp);
+	bool getNewRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp);
+	bool hasNewRawGyro();
+	void resetNewRawGyro();
 
 	int16_t getRawMagX();
 	int16_t getRawMagY();
 	int16_t getRawMagZ();
+	void getRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp);
+	bool getNewRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp);
+	bool hasNewRawMag();
 
 	float getRoll();
 	float getPitch();
@@ -270,6 +294,7 @@ public:
 	void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig);
 	void sendCommand(uint8_t command);
 	void sendCalibrateCommand(uint8_t thingToCalibrate);
+	void saveCalibrationPeriodically(bool save);
 
 	//Metadata functions
 	int16_t getQ1(uint16_t recordID);
@@ -308,10 +333,10 @@ private:
 
 	SPIClass *_spiPort;			 //The generic connection to user's chosen SPI hardware
 	unsigned long _spiPortSpeed; //Optional user defined port speed
-	uint8_t _cs;				 //Pins needed for SPI
-	uint8_t _wake;
-	uint8_t _int;
-	uint8_t _rst;
+	PinInterface* _cs;				 //Pins needed for SPI
+	PinInterface* _wake;
+	PinInterface* _int;
+	PinInterface* _rst;
 
 	//These are the raw sensor values (without Q applied) pulled from the user requested Input Report
 	uint16_t rawAccelX, rawAccelY, rawAccelZ, accelAccuracy;
@@ -321,7 +346,8 @@ private:
 	uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
 	uint16_t rawGameQuatI, rawGameQuatJ, rawGameQuatK, rawGameQuatReal, quatGameAccuracy;
 	uint16_t rawMagQuatI, rawMagQuatJ, rawMagQuatK, rawMagQuatReal, rawMagQuatRadianAccuracy, quatMagAccuracy;
-	bool hasNewQuaternion, hasNewGameQuaternion, hasNewMagQuaternion, hasNewAccel_;
+	bool hasNewQuaternion, hasNewGameQuaternion, hasNewMagQuaternion, hasNewAccel_, hasNewLinAccel_, hasNewFastGyro_;
+	bool hasNewMag_, hasNewGyro_;
 	uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ;
 	uint8_t tapDetector;
 	bool hasNewTap;
@@ -332,8 +358,12 @@ private:
 	uint8_t *_activityConfidences;						  //Array that store the confidences of the 9 possible activities
 	uint8_t calibrationStatus;							  //Byte R0 of ME Calibration Response
 	uint16_t memsRawAccelX, memsRawAccelY, memsRawAccelZ; //Raw readings from MEMS sensor
-	uint16_t memsRawGyroX, memsRawGyroY, memsRawGyroZ;	//Raw readings from MEMS sensor
+	uint16_t memsRawGyroX, memsRawGyroY, memsRawGyroZ, memsRawGyroTemp;	//Raw readings from MEMS sensor
 	uint16_t memsRawMagX, memsRawMagY, memsRawMagZ;		  //Raw readings from MEMS sensor
+	uint32_t memsAccelTimeStamp, memsGyroTimeStamp, memsMagTimeStamp;  //Timestamp of MEMS sensor reading
+	bool hasNewRawAccel_ = false;
+	bool hasNewRawGyro_= false;
+	bool hasNewRawMag_ = false;
 
 	//These Q values are defined in the datasheet but can also be obtained by querying the meta data records
 	//See the read metadata example for more info
@@ -344,4 +374,7 @@ private:
 	int16_t gyro_Q1 = 9;
 	int16_t magnetometer_Q1 = 4;
 	int16_t angular_velocity_Q1 = 10;
+
+	bool _hasNewCalibrationStatus = false;
+	uint8_t _calibrationResponseStatus, _accelCalEnabled, _gyroCalEnabled, _magCalEnabled, _planarAccelCalEnabled, _onTableCalEnabled;
 };

lib/bno080/PinInterface.h

@@ -1,6 +1,6 @@
 /*
     SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+    Copyright (c) 2024 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
@@ -20,31 +20,18 @@
     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
     THE SOFTWARE.
 */
+#pragma once
 
-#ifndef _MAHONY_H_
-#define _MAHONY_H_
-
-#include "helper_3dmath.h"
-
-template<typename T>
-class Mahony {
-
-    // These are the free parameters in the Mahony filter and fusion scheme,
-    // Kp for proportional feedback, Ki for integral
-    // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
-    static constexpr float Kp = 10.0f;
-    static constexpr float Ki = 0.0f;
+#include <cstdint>
+#include <string>
 
+class PinInterface
+{
 public:
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat);
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat);
-    
-private:
-    T ix = 0.0;
-    T iy = 0.0;
-    T iz = 0.0;
+	virtual bool init() { return true; };
+	virtual int digitalRead() = 0;
+	virtual void pinMode(uint8_t mode) = 0;
+	virtual void digitalWrite(uint8_t val) = 0;
+
+	[[nodiscard]] virtual std::string toString() const = 0;
 };
-
-#include "mahony.hpp"
-
-#endif /* _MAHONY_H_ */

lib/i2cscan/i2cscan.cpp

@@ -5,11 +5,14 @@
 uint8_t portArray[] = {16, 5, 4, 2, 14, 12, 13};
 uint8_t portExclude[] = {LED_PIN};
 String portMap[] = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"};
-// ESP32C3 has not as many ports as the ESP32
 #elif defined(ESP32C3)
 uint8_t portArray[] = {2, 3, 4, 5, 6, 7, 8, 9, 10};
 uint8_t portExclude[] = {18, 19, 20, 21, LED_PIN};
 String portMap[] = {"2", "3", "4", "5", "6", "7", "8", "9", "10"};
+#elif defined(ESP32C6)
+uint8_t portArray[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23};
+String portMap[] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"};
+uint8_t portExclude[] = {12, 13, 16, 17, LED_PIN};
 #elif defined(ESP32)
 uint8_t portArray[] = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
 String portMap[] = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
@@ -18,54 +21,108 @@ uint8_t portExclude[] = {LED_PIN};
 
 namespace I2CSCAN
 {
+    enum class ScanState {
+        IDLE,
+        SCANNING,
+        DONE
+    };
 
-    uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound) {
-        if(I2CSCAN::hasDevOnBus(addr1)) {
-            return addr1;
-        }
-        if(I2CSCAN::hasDevOnBus(addr2)) {
-            return addr2;
-        }
-        if (scanIfNotFound) {
-            Serial.println("[ERR] I2C: Can't find I2C device on provided addresses, scanning for all I2C devices and returning");
-            I2CSCAN::scani2cports();
-        } else {
-            Serial.println("[ERR] I2C: Can't find I2C device on provided addresses");
-        }
-        return 0;
-    }
+    ScanState scanState = ScanState::IDLE;
+    uint8_t currentSDA = 0;
+    uint8_t currentSCL = 0;
+    uint8_t currentAddress = 1;
+    bool found = false;
+    std::vector<uint8_t> validPorts;
 
     void scani2cports()
     {
-        bool found = false;
-        for (uint8_t i = 0; i < sizeof(portArray); i++)
-        {
-            for (uint8_t j = 0; j < sizeof(portArray); j++)
-            {
-                if ((i != j) && !inArray(portArray[i], portExclude, sizeof(portExclude)) && !inArray(portArray[j], portExclude, sizeof(portExclude)))
-                {
-                    if(checkI2C(i, j))
-                        found = true;
-                }
+        if (scanState != ScanState::IDLE) {
+            return;
+        }
+
+        // Filter out excluded ports
+        for (size_t i = 0; i < sizeof(portArray); i++) {
+            if (!inArray(portArray[i], portExclude, sizeof(portExclude))) {
+                validPorts.push_back(portArray[i]);
             }
         }
-        if(!found) {
-            Serial.println("[ERR] I2C: No I2C devices found");
+
+        found = false;
+        currentSDA = 0;
+        currentSCL = 1;
+        currentAddress = 1;
+        scanState = ScanState::SCANNING;
+    }
+
+    bool selectNextPort() {
+        currentSCL++;
+        if(validPorts[currentSCL] == validPorts[currentSDA])
+            currentSCL++;
+        if (currentSCL < validPorts.size()) {
+            Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+            return true;
         }
 
-#if ESP32
-        Wire.end();
-#endif
+        currentSCL = 0;
+        currentSDA++;
 
-        // Reset the I2C interface back to it's original values
-        Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+        if (currentSDA >= validPorts.size()) {
+            if (!found) {
+                Serial.println("[ERR] I2C: No I2C devices found");
+            }
+#ifdef ESP32
+            Wire.end();
+#endif
+            Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+            scanState = ScanState::DONE;
+            return false;
+        }
+
+        Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+        return true;
+    }
+
+    void update()
+    {
+        if (scanState != ScanState::SCANNING) {
+            return;
+        }
+
+        if (currentAddress == 1) {
+#ifdef ESP32
+            Wire.end();
+#endif
+        }
+
+        Wire.beginTransmission(currentAddress);
+        byte error = Wire.endTransmission();
+
+        if (error == 0)
+        {
+            Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n",
+                            portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
+            found = true;
+        }
+        else if (error == 4)
+        {
+            Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n",
+                            portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
+        }
+
+        currentAddress++;
+        if (currentAddress <= 127) {
+            return;
+        }
+
+        currentAddress = 1;
+        selectNextPort();
     }
 
     bool inArray(uint8_t value, uint8_t* array, size_t arraySize)
     {
         for (size_t i = 0; i < arraySize; i++)
         {
-            if (value == array[i]) 
+            if (value == array[i])
             {
                 return true;
             }
@@ -73,43 +130,6 @@ namespace I2CSCAN
 
         return false;
     }
-    
-    bool checkI2C(uint8_t i, uint8_t j)
-    {
-        bool found = false;
-
-#if ESP32
-        Wire.end();
-#endif
-
-        Wire.begin((int)portArray[i], (int)portArray[j]);
-
-        byte error, address;
-        int nDevices;
-        nDevices = 0;
-        for (address = 1; address < 127; address++)
-        {
-            // The i2c_scanner uses the return value of
-            // the Write.endTransmisstion to see if
-            // a device did acknowledge to the address.
-            Wire.beginTransmission(address);
-            error = Wire.endTransmission();
-
-            if (error == 0)
-            {
-                Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n", 
-                                portMap[i].c_str(), portArray[i], portMap[j].c_str(), portArray[j], address);
-                nDevices++;
-                found = true;
-            }
-            else if (error == 4)
-            {
-                Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n",
-                                portMap[i].c_str(), portArray[i], portMap[j].c_str(), portArray[j], address);
-            }
-        }
-        return found;
-    }
 
     bool hasDevOnBus(uint8_t addr) {
         byte error;
@@ -118,7 +138,7 @@ namespace I2CSCAN
         do {
 #endif
             Wire.beginTransmission(addr);
-            error = Wire.endTransmission();
+            error = Wire.endTransmission(); // The return value of endTransmission is used to determine if a device is present
 #if ESP32C3
         }
         while (error != 0 && retries--);
@@ -143,60 +163,55 @@ namespace I2CSCAN
      * NSW Australia, www.forward.com.au
      * This code may be freely used for both private and commerical use
      */
+
     int clearBus(uint8_t SDA, uint8_t SCL) {
         #if defined(TWCR) && defined(TWEN)
-        TWCR &= ~(_BV(TWEN)); //Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly
+        TWCR &= ~(_BV(TWEN)); // Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly
         #endif
 
-        pinMode(SDA, INPUT_PULLUP); // Make SDA (data) and SCL (clock) pins Inputs with pullup.
+        pinMode(SDA, INPUT_PULLUP);
         pinMode(SCL, INPUT_PULLUP);
 
-        boolean SCL_LOW = (digitalRead(SCL) == LOW); // Check is SCL is Low.
-        if (SCL_LOW) { //If it is held low Arduno cannot become the I2C master. 
-            return 1; //I2C bus error. Could not clear SCL clock line held low
+        boolean SCL_LOW = (digitalRead(SCL) == LOW);
+        if (SCL_LOW) {
+            return 1; // I2C bus error. Could not clear SCL, clock line held low.
         }
 
-        boolean SDA_LOW = (digitalRead(SDA) == LOW);  // vi. Check SDA input.
+        boolean SDA_LOW = (digitalRead(SDA) == LOW);
         int clockCount = 20; // > 2x9 clock
 
-        while (SDA_LOW && (clockCount > 0)) { //  vii. If SDA is Low,
+        while (SDA_LOW && (clockCount > 0)) {
             clockCount--;
-        // Note: I2C bus is open collector so do NOT drive SCL or SDA high.
-            pinMode(SCL, INPUT); // release SCL pullup so that when made output it will be LOW
-            pinMode(SCL, OUTPUT); // then clock SCL Low
-            delayMicroseconds(10); //  for >5uS
-            pinMode(SCL, INPUT); // release SCL LOW
-            pinMode(SCL, INPUT_PULLUP); // turn on pullup resistors again
-            // do not force high as slave may be holding it low for clock stretching.
-            delayMicroseconds(10); //  for >5uS
-            // The >5uS is so that even the slowest I2C devices are handled.
-            SCL_LOW = (digitalRead(SCL) == LOW); // Check if SCL is Low.
-            int counter = 20;
-            while (SCL_LOW && (counter > 0)) {  //  loop waiting for SCL to become High only wait 2sec.
-            counter--;
-            delay(100);
+            pinMode(SCL, INPUT);
+            pinMode(SCL, OUTPUT);
+            delayMicroseconds(10);
+            pinMode(SCL, INPUT);
+            pinMode(SCL, INPUT_PULLUP);
+            delayMicroseconds(10);
             SCL_LOW = (digitalRead(SCL) == LOW);
+            int counter = 20;
+            while (SCL_LOW && (counter > 0)) {
+                counter--;
+                delay(100);
+                SCL_LOW = (digitalRead(SCL) == LOW);
             }
-            if (SCL_LOW) { // still low after 2 sec error
-            return 2; // I2C bus error. Could not clear. SCL clock line held low by slave clock stretch for >2sec
+            if (SCL_LOW) {
+                return 2;
             }
-            SDA_LOW = (digitalRead(SDA) == LOW); //   and check SDA input again and loop
+            SDA_LOW = (digitalRead(SDA) == LOW);
         }
-        if (SDA_LOW) { // still low
-            return 3; // I2C bus error. Could not clear. SDA data line held low
+        if (SDA_LOW) {
+            return 3;
         }
 
-        // else pull SDA line low for Start or Repeated Start
-        pinMode(SDA, INPUT); // remove pullup.
-        pinMode(SDA, OUTPUT);  // and then make it LOW i.e. send an I2C Start or Repeated start control.
-        // When there is only one I2C master a Start or Repeat Start has the same function as a Stop and clears the bus.
-        /// A Repeat Start is a Start occurring after a Start with no intervening Stop.
-        delayMicroseconds(10); // wait >5uS
-        pinMode(SDA, INPUT); // remove output low
-        pinMode(SDA, INPUT_PULLUP); // and make SDA high i.e. send I2C STOP control.
-        delayMicroseconds(10); // x. wait >5uS
-        pinMode(SDA, INPUT); // and reset pins as tri-state inputs which is the default state on reset
+        pinMode(SDA, INPUT);
+        pinMode(SDA, OUTPUT);
+        delayMicroseconds(10);
+        pinMode(SDA, INPUT);
+        pinMode(SDA, INPUT_PULLUP);
+        delayMicroseconds(10);
+        pinMode(SDA, INPUT);
         pinMode(SCL, INPUT);
-        return 0; // all ok
+        return 0;
     }
 }

lib/i2cscan/i2cscan.h

@@ -6,6 +6,7 @@
 
 namespace I2CSCAN {
     void scani2cports();
+    void update();
     bool checkI2C(uint8_t i, uint8_t j);
     bool hasDevOnBus(uint8_t addr);
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound);

lib/magneto/madgwick.h

@@ -1,18 +0,0 @@
-#ifndef _MADGWICK_H_
-#define _MADGWICK_H_
-
-#include "helper_3dmath.h"
-
-template<typename T>
-class Madgwick {
-
-    static constexpr float madgwickBeta = 0.1f;
-
-public:
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat);
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat);
-};
-
-#include "madgwick.hpp"
-
-#endif /* _MADGWICK_H_ */

lib/magneto/madgwick.hpp

@@ -1,169 +0,0 @@
-#include "madgwick.h"
-
-template<typename T>
-void Madgwick<T>::update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat)
-{
-    T recipNorm;
-    T s0, s1, s2, s3;
-    T qDot1, qDot2, qDot3, qDot4;
-    T hx, hy;
-    T _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
-
-    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
-    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-        update(q, ax, ay, az, gx, gy, gz, deltat);
-        return;
-    }
-
-    // Rate of change of quaternion from gyroscope
-    qDot1 = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
-    qDot2 = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
-    qDot3 = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
-    qDot4 = 0.5f * (q[0] * gz + q[1] * gy - q[2] * gx);
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-        // Normalise accelerometer measurement
-        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-        ax *= recipNorm;
-        ay *= recipNorm;
-        az *= recipNorm;   
-
-        // Normalise magnetometer measurement
-        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-        mx *= recipNorm;
-        my *= recipNorm;
-        mz *= recipNorm;
-
-        // Auxiliary variables to avoid repeated arithmetic
-        _2q0mx = 2.0f * q[0] * mx;
-        _2q0my = 2.0f * q[0] * my;
-        _2q0mz = 2.0f * q[0] * mz;
-        _2q1mx = 2.0f * q[1] * mx;
-        _2q0 = 2.0f * q[0];
-        _2q1 = 2.0f * q[1];
-        _2q2 = 2.0f * q[2];
-        _2q3 = 2.0f * q[3];
-        _2q0q2 = 2.0f * q[0] * q[2];
-        _2q2q3 = 2.0f * q[2] * q[3];
-        q0q0 = q[0] * q[0];
-        q0q1 = q[0] * q[1];
-        q0q2 = q[0] * q[2];
-        q0q3 = q[0] * q[3];
-        q1q1 = q[1] * q[1];
-        q1q2 = q[1] * q[2];
-        q1q3 = q[1] * q[3];
-        q2q2 = q[2] * q[2];
-        q2q3 = q[2] * q[3];
-        q3q3 = q[3] * q[3];
-
-        // Reference direction of Earth's magnetic field
-        hx = mx * q0q0 - _2q0my * q[3] + _2q0mz * q[2] + mx * q1q1 + _2q1 * my * q[2] + _2q1 * mz * q[3] - mx * q2q2 - mx * q3q3;
-        hy = _2q0mx * q[3] + my * q0q0 - _2q0mz * q[1] + _2q1mx * q[2] - my * q1q1 + my * q2q2 + _2q2 * mz * q[3] - my * q3q3;
-        _2bx = sqrt(hx * hx + hy * hy);
-        _2bz = -_2q0mx * q[2] + _2q0my * q[1] + mz * q0q0 + _2q1mx * q[3] - mz * q1q1 + _2q2 * my * q[3] - mz * q2q2 + mz * q3q3;
-        _4bx = 2.0f * _2bx;
-        _4bz = 2.0f * _2bz;
-
-        // Gradient decent algorithm corrective step
-        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q[2] * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q[3] + _2bz * q[1]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q[2] * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q[1] * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q[3] * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q[2] + _2bz * q[0]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q[3] - _4bz * q[1]) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q[2] * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q[2] - _2bz * q[0]) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q[1] + _2bz * q[3]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q[0] - _4bz * q[2]) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q[3] + _2bz * q[1]) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q[0] + _2bz * q[2]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q[1] * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
-        s0 *= recipNorm;
-        s1 *= recipNorm;
-        s2 *= recipNorm;
-        s3 *= recipNorm;
-
-        // Apply feedback step
-        qDot1 -= madgwickBeta * s0;
-        qDot2 -= madgwickBeta * s1;
-        qDot3 -= madgwickBeta * s2;
-        qDot4 -= madgwickBeta * s3;
-    }
-
-    // Integrate rate of change of quaternion to yield quaternion
-    q[0] += qDot1 * deltat;
-    q[1] += qDot2 * deltat;
-    q[2] += qDot3 * deltat;
-    q[3] += qDot4 * deltat;
-
-    // Normalise quaternion
-    recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
-    q[0] *= recipNorm;
-    q[1] *= recipNorm;
-    q[2] *= recipNorm;
-    q[3] *= recipNorm;
-}
-
-template<typename T>
-void Madgwick<T>::update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat)
-{
-    T recipNorm;
-    T s0, s1, s2, s3;
-    T qDot1, qDot2, qDot3, qDot4;
-    T _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
-
-    // Rate of change of quaternion from gyroscope
-    qDot1 = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
-    qDot2 = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
-    qDot3 = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
-    qDot4 = 0.5f * (q[0] * gz + q[1] * gy - q[2] * gx);
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-        // Normalise accelerometer measurement
-        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-        ax *= recipNorm;
-        ay *= recipNorm;
-        az *= recipNorm;   
-
-        // Auxiliary variables to avoid repeated arithmetic
-        _2q0 = 2.0f * q[0];
-        _2q1 = 2.0f * q[1];
-        _2q2 = 2.0f * q[2];
-        _2q3 = 2.0f * q[3];
-        _4q0 = 4.0f * q[0];
-        _4q1 = 4.0f * q[1];
-        _4q2 = 4.0f * q[2];
-        _8q1 = 8.0f * q[1];
-        _8q2 = 8.0f * q[2];
-        q0q0 = q[0] * q[0];
-        q1q1 = q[1] * q[1];
-        q2q2 = q[2] * q[2];
-        q3q3 = q[3] * q[3];
-
-        // Gradient decent algorithm corrective step
-        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
-        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
-        s2 = 4.0f * q0q0 * q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
-        s3 = 4.0f * q1q1 * q[3] - _2q1 * ax + 4.0f * q2q2 * q[3] - _2q2 * ay;
-        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
-        s0 *= recipNorm;
-        s1 *= recipNorm;
-        s2 *= recipNorm;
-        s3 *= recipNorm;
-
-        // Apply feedback step
-        qDot1 -= madgwickBeta * s0;
-        qDot2 -= madgwickBeta * s1;
-        qDot3 -= madgwickBeta * s2;
-        qDot4 -= madgwickBeta * s3;
-    }
-
-    // Integrate rate of change of quaternion to yield quaternion
-    q[0] += qDot1 * deltat;
-    q[1] += qDot2 * deltat;
-    q[2] += qDot3 * deltat;
-    q[3] += qDot4 * deltat;
-
-    // Normalise quaternion
-    recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
-    q[0] *= recipNorm;
-    q[1] *= recipNorm;
-    q[2] *= recipNorm;
-    q[3] *= recipNorm;
-}

lib/magneto/mahony.hpp

@@ -1,206 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
-
-    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 "mahony.h"
-
-// Mahony orientation filter, assumed World Frame NWU (xNorth, yWest, zUp)
-// Modified from Madgwick version to remove Z component of magnetometer:
-// reference vectors are Up (Acc) and West (Acc cross Mag)
-// sjr 12/2020
-// gx, gy, gz must be in units of radians/second
-template<typename T>
-void Mahony<T>::update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat)
-{
-    // short name local variable for readability
-    T q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
-    T norm;
-    T hx, hy, hz;  //observed West vector W = AxM
-    T ux, uy, uz, wx, wy, wz; //calculated A (Up) and W in body frame
-    T ex, ey, ez;
-    T qa, qb, qc;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    T q1q1 = q1 * q1;
-    T q1q2 = q1 * q2;
-    T q1q3 = q1 * q3;
-    T q1q4 = q1 * q4;
-    T q2q2 = q2 * q2;
-    T q2q3 = q2 * q3;
-    T q2q4 = q2 * q4;
-    T q3q3 = q3 * q3;
-    T q3q4 = q3 * q4;
-    T q4q4 = q4 * q4;
-
-    // Compute feedback only if magnetometer measurement valid (avoids NaN in magnetometer normalisation)
-    T tmp = mx * mx + my * my + mz * mz;
-    if (tmp == 0.0f) {
-        update(q, ax, ay, az, gx, gy, gz, deltat);
-        return;
-    }
-    // Normalise magnetometer
-    norm = invSqrt(tmp);
-    mx *= norm;
-    my *= norm;
-    mz *= norm;
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    tmp = ax * ax + ay * ay + az * az;
-    if (tmp > 0.0f)
-    {
-        // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
-        norm = invSqrt(tmp);
-        ax *= norm;
-        ay *= norm;
-        az *= norm;
-
-        // Measured horizon vector = a x m (in body frame)
-        hx = ay * mz - az * my;
-        hy = az * mx - ax * mz;
-        hz = ax * my - ay * mx;
-
-        // Normalise horizon vector
-        norm = invSqrt(hx * hx + hy * hy + hz * hz);
-        hx *= norm;
-        hy *= norm;
-        hz *= norm;
-
-        // Estimated direction of Up reference vector
-        ux = 2.0f * (q2q4 - q1q3);
-        uy = 2.0f * (q1q2 + q3q4);
-        uz = q1q1 - q2q2 - q3q3 + q4q4;
-
-        // estimated direction of horizon (West) reference vector
-        wx = 2.0f * (q2q3 + q1q4);
-        wy = q1q1 - q2q2 + q3q3 - q4q4;
-        wz = 2.0f * (q3q4 - q1q2);
-
-        // Error is cross product between estimated direction and measured direction of the reference vectors
-        ex = (ay * uz - az * uy) + (hy * wz - hz * wy);
-        ey = (az * ux - ax * uz) + (hz * wx - hx * wz);
-        ez = (ax * uy - ay * ux) + (hx * wy - hy * wx);
-
-        // Compute and apply to gyro term the integral feedback, if enabled
-        if (Ki > 0.0f) {
-            ix += Ki * ex * deltat;  // integral error scaled by Ki
-            iy += Ki * ey * deltat;
-            iz += Ki * ez * deltat;
-            gx += ix;  // apply integral feedback
-            gy += iy;
-            gz += iz;
-        }
-
-        // Apply proportional feedback to gyro term
-        gx += Kp * ex;
-        gy += Kp * ey;
-        gz += Kp * ez;
-    }
-
-    // Integrate rate of change of quaternion
-    // small correction 1/11/2022, see https://github.com/kriswiner/MPU9250/issues/447
-    deltat *= 0.5f;
-    gx *= deltat;   // pre-multiply common factors
-    gy *= deltat;
-    gz *= deltat;
-    qa = q1;
-    qb = q2;
-    qc = q3;
-    q1 += (-qb * gx - qc * gy - q4 * gz);
-    q2 += (qa * gx + qc * gz - q4 * gy);
-    q3 += (qa * gy - qb * gz + q4 * gx);
-    q4 += (qa * gz + qb * gy - qc * gx);
-
-    // Normalise quaternion
-    norm = invSqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-    q[0] = q1 * norm;
-    q[1] = q2 * norm;
-    q[2] = q3 * norm;
-    q[3] = q4 * norm;
-}
-
-template<typename T>
-void Mahony<T>::update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat)
-{
-    // short name local variable for readability
-    T q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
-    T norm;
-    T vx, vy, vz;
-    T ex, ey, ez;  //error terms
-    T qa, qb, qc;
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    T tmp = ax * ax + ay * ay + az * az;
-    if (tmp > 0.0f)
-    {
-        // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
-        norm = invSqrt(tmp);
-        ax *= norm;
-        ay *= norm;
-        az *= norm;
-
-        // Estimated direction of gravity in the body frame (factor of two divided out)
-        vx = q2 * q4 - q1 * q3;
-        vy = q1 * q2 + q3 * q4;
-        vz = q1 * q1 - 0.5f + q4 * q4;
-
-        // Error is cross product between estimated and measured direction of gravity in body frame
-        // (half the actual magnitude)
-        ex = (ay * vz - az * vy);
-        ey = (az * vx - ax * vz);
-        ez = (ax * vy - ay * vx);
-
-        // Compute and apply to gyro term the integral feedback, if enabled
-        if (Ki > 0.0f) {
-            ix += Ki * ex * deltat;  // integral error scaled by Ki
-            iy += Ki * ey * deltat;
-            iz += Ki * ez * deltat;
-            gx += ix;  // apply integral feedback
-            gy += iy;
-            gz += iz;
-        }
-
-        // Apply proportional feedback to gyro term
-        gx += Kp * ex;
-        gy += Kp * ey;
-        gz += Kp * ez;
-    }
-
-    // Integrate rate of change of quaternion, q cross gyro term
-    deltat *= 0.5f;
-    gx *= deltat;   // pre-multiply common factors
-    gy *= deltat;
-    gz *= deltat;
-    qa = q1;
-    qb = q2;
-    qc = q3;
-    q1 += (-qb * gx - qc * gy - q4 * gz);
-    q2 += (qa * gx + qc * gz - q4 * gy);
-    q3 += (qa * gy - qb * gz + q4 * gx);
-    q4 += (qa * gz + qb * gy - qc * gx);
-
-    // normalise quaternion
-    norm = invSqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-    q[0] = q1 * norm;
-    q[1] = q2 * norm;
-    q[2] = q3 * norm;
-    q[3] = q4 * norm;
-}

lib/math/quat.cpp

@@ -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;
+}

lib/math/quat.h

@@ -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);

lib/ota/ota.cpp

@@ -64,12 +64,14 @@ void OTA::otaSetup(const char * const otaPassword) {
 
 void OTA::otaUpdate() {
     if(enabled) {
+        #if USE_OTA_TIMEOUT
         if(bootTime + 60000 < millis()) {
             // Disable OTA 60 seconds after boot as protection measure
             enabled = false;
             Serial.println("[NOTICE] OTA updates disabled by timeout, this is not an error");
             return;
         }
+        #endif
         ArduinoOTA.handle();
     }
 }

lib/vqf/vqf.cpp

@@ -2,7 +2,7 @@
 //
 // SPDX-License-Identifier: MIT
 
-// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
 // Removed batch update functions
 
 #include "vqf.h"
@@ -18,41 +18,6 @@
 
 inline vqf_real_t square(vqf_real_t x) { return x*x; }
 
-
-VQFParams::VQFParams()
-    : tauAcc(3.0)
-    , tauMag(9.0)
-#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-    , motionBiasEstEnabled(true)
-#endif
-    , restBiasEstEnabled(true)
-    , magDistRejectionEnabled(true)
-    , biasSigmaInit(0.5)
-    , biasForgettingTime(100.0)
-    , biasClip(2.0)
-#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-    , biasSigmaMotion(0.1)
-    , biasVerticalForgettingFactor(0.0001)
-#endif
-    , biasSigmaRest(0.03)
-    , restMinT(1.5)
-    , restFilterTau(0.5)
-    , restThGyr(2.0)
-    , restThAcc(0.5)
-    , magCurrentTau(0.05)
-    , magRefTau(20.0)
-    , magNormTh(0.1)
-    , magDipTh(10.0)
-    , magNewTime(20.0)
-    , magNewFirstTime(5.0)
-    , magNewMinGyr(20.0)
-    , magMinUndisturbedTime(0.5)
-    , magMaxRejectionTime(60.0)
-    , magRejectionFactor(2.0)
-{
-
-}
-
 VQF::VQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs)
 {
     coeffs.gyrTs = gyrTs;
@@ -73,7 +38,7 @@ VQF::VQF(const VQFParams &params, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t
     setup();
 }
 
-void VQF::updateGyr(const vqf_real_t gyr[3], double gyrTs)
+void VQF::updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
 {
     // rest detection
     if (params.restBiasEstEnabled || params.magDistRejectionEnabled) {
@@ -537,8 +502,8 @@ void VQF::setTauAcc(vqf_real_t tauAcc)
         return;
     }
     params.tauAcc = tauAcc;
-    double newB[3];
-    double newA[3];
+    vqf_real_t newB[3];
+    vqf_real_t newA[3];
 
     filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA);
     filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState);
@@ -731,15 +696,15 @@ vqf_real_t VQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts)
     }
 }
 
-void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA[])
+void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[], vqf_real_t outA[])
 {
     assert(tau > 0);
     assert(Ts > 0);
     // second order Butterworth filter based on https://stackoverflow.com/a/52764064
-    double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response
-    double C = tan(M_PI*fc*double(Ts));
-    double D = C*C + sqrt(2)*C + 1;
-    double b0 = C*C/D;
+    vqf_real_t fc = (M_SQRT2 / (2.0*M_PI))/vqf_real_t(tau); // time constant of dampened, non-oscillating part of step response
+    vqf_real_t C = tan(M_PI*fc*vqf_real_t(Ts));
+    vqf_real_t D = C*C + sqrt(2)*C + 1;
+    vqf_real_t b0 = C*C/D;
     outB[0] = b0;
     outB[1] = 2*b0;
     outB[2] = b0;
@@ -748,7 +713,7 @@ void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA
     outA[1] = (1-sqrt(2)*C+C*C)/D; // a2
 }
 
-void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[])
+void VQF::filterInitialState(vqf_real_t x0, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t out[])
 {
     // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter
     // update equation)
@@ -756,9 +721,9 @@ void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2]
     out[1] = x0*(b[2] - a[1]);
 }
 
-void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[],
-                                         const double a_old[], const double b_new[],
-                                         const double a_new[], double state[])
+void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const vqf_real_t b_old[],
+                                         const vqf_real_t a_old[], const vqf_real_t b_new[],
+                                         const vqf_real_t a_new[], vqf_real_t state[])
 {
     if (isnan(state[0])) {
         return;
@@ -769,18 +734,18 @@ void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const do
     }
 }
 
-vqf_real_t VQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2])
+vqf_real_t VQF::filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2])
 {
     // difference equations based on scipy.signal.lfilter documentation
     // assumes that a0 == 1.0
-    double y = b[0]*x + state[0];
+    vqf_real_t y = b[0]*x + state[0];
     state[0] = b[1]*x - a[0]*y + state[1];
     state[1] = b[2]*x - a[1]*y;
     return y;
 }
 
-void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3],
-                    const double a[2], double state[], vqf_real_t out[])
+void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const vqf_real_t b[3],
+                    const vqf_real_t a[2], vqf_real_t state[], vqf_real_t out[])
 {
     assert(N>=2);
 
@@ -873,17 +838,17 @@ void VQF::matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2
 bool VQF::matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9])
 {
     // in = [a b c; d e f; g h i]
-    double A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
-    double D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
-    double G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
-    double B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
-    double E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
-    double H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
-    double C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
-    double F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
-    double I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
+    vqf_real_t A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
+    vqf_real_t D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
+    vqf_real_t G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
+    vqf_real_t B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
+    vqf_real_t E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
+    vqf_real_t H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
+    vqf_real_t C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
+    vqf_real_t F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
+    vqf_real_t I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
 
-    double det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
+    vqf_real_t det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
 
     if (det >= -EPS && det <= EPS) {
         std::fill(out, out+9, 0);
@@ -917,16 +882,7 @@ void VQF::setup()
 
     coeffs.biasP0 = square(params.biasSigmaInit*100.0);
     // the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds
-    coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime;
-
-#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-    vqf_real_t pMotion = square(params.biasSigmaMotion*100.0);
-    coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion;
-    coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10));
-#endif
-
-    vqf_real_t pRest = square(params.biasSigmaRest*100.0);
-    coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
+    updateBiasForgettingTime(params.biasForgettingTime);
 
     filterCoeffs(params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA);
     filterCoeffs(params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA);
@@ -941,3 +897,16 @@ void VQF::setup()
 
     resetState();
 }
+
+void VQF::updateBiasForgettingTime(float biasForgettingTime) {
+    coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime;
+
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    vqf_real_t pMotion = square(params.biasSigmaMotion*100.0);
+    coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion;
+    coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10));
+#endif
+
+    vqf_real_t pRest = square(params.biasSigmaRest*100.0);
+    coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
+}

platformio-tools.ini

@@ -2,6 +2,8 @@
 lib_deps=
   https://github.com/SlimeVR/CmdParser.git
   https://github.com/SlimeVR/base64_arduino.git
+  https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library.git
+  https://github.com/hideakitai/PCA9547.git
 monitor_speed = 115200
 framework = arduino
 build_flags =
@@ -15,26 +17,75 @@ build_unflags =
 [env:BOARD_SLIMEVR]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+build_flags =
+  ${env.build_flags}
+  -D BOARD=BOARD_SLIMEVR
+  -D VENDOR_NAME='"SlimeVR"'
+  -D VENDOR_URL='"https://slimevr.dev"'
+  -D PRODUCT_NAME='"SlimeVR Tracker"'
+  -D UPDATE_ADDRESS='"SlimeVR/SlimeVR-Tracker-ESP"'
+  -D UPDATE_NAME='"BOARD_SLIMEVR-firmware"'
+
+[env:BOARD_SLIMEVR_V1_2]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+build_flags =
+  ${env.build_flags}
+  -D BOARD=BOARD_SLIMEVR_V1_2
+  -D VENDOR_NAME='"SlimeVR"'
+  -D VENDOR_URL='"https://slimevr.dev"'
+  -D PRODUCT_NAME='"SlimeVR Tracker v1.2"'
+  -D UPDATE_ADDRESS='"SlimeVR/SlimeVR-Tracker-ESP"'
+  -D UPDATE_NAME='"BOARD_SLIMEVR_V1_2-firmware"'
 
 [env:BOARD_SLIMEVR_DEV]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+build_flags =
+  ${env.build_flags}
+  -D BOARD=BOARD_SLIMEVR_DEV
+  -D VENDOR_NAME='"SlimeVR"'
+  -D PRODUCT_NAME='"SlimeVR Tracker (dev)"'
+
+[env:BOARD_GLOVE_IMU_SLIMEVR_DEV]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+  -D BOARD=BOARD_GLOVE_IMU_SLIMEVR_DEV
+  -D PRODUCT_NAME='"SlimeVR Glove (dev)"'
+board = lolin_c3_mini
 
 [env:BOARD_NODEMCU]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_NODEMCU
 
 [env:BOARD_WEMOSD1MINI]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_WEMOSD1MINI
 
 [env:BOARD_TTGO_TBASE]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_TTGO_TBASE
 
 [env:BOARD_WEMOSWROOM02]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_NODEMCU
 
 [env:BOARD_WROOM32]
 platform = espressif32 @ 6.7.0
@@ -42,6 +93,9 @@ platform_packages =
   framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
   framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
 board = esp32dev
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_WROOM32
 
 [env:BOARD_ESP01]
 platform = espressif32 @ 6.7.0
@@ -49,6 +103,9 @@ platform_packages =
   framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
   framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
 board = esp32dev
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_ESP01
 
 [env:BOARD_LOLIN_C3_MINI]
 platform = espressif32 @ 6.7.0
@@ -58,6 +115,7 @@ platform_packages =
 build_flags = 
   ${env.build_flags}
   -DESP32C3
+  -D BOARD=BOARD_LOLIN_C3_MINI
 board = lolin_c3_mini
 
 [env:BOARD_BEETLE32C3]
@@ -68,9 +126,10 @@ platform_packages =
 build_flags = 
   ${env.build_flags}
   -DESP32C3
+  -D BOARD=BOARD_BEETLE32C3
 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
@@ -78,8 +137,17 @@ platform_packages =
 build_flags = 
   ${env.build_flags}
   -DESP32C3
+  -D BOARD=BOARD_ESP32C3DEVKITM1
 board = esp32-c3-devkitm-1
 
+[env:BOARD_ESP32C6DEVKITC1]
+platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
+build_flags =
+  ${env.build_flags}
+  -DESP32C6
+  -D BOARD=BOARD_ESP32C6DEVKITC1
+board = esp32-c6-devkitc-1
+
 [env:BOARD_XIAO_ESP32C3]
 platform = espressif32 @ 6.7.0
 platform_packages =
@@ -88,4 +156,21 @@ platform_packages =
 build_flags = 
   ${env.build_flags}
   -DESP32C3
+  -D BOARD=BOARD_XIAO_ESP32C3
 board = seeed_xiao_esp32c3
+
+[env:BOARD_ESP32S3_SUPERMINI]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+build_flags = 
+  ${env.build_flags}
+  -DARDUINO_USB_MODE=1
+  -DESP32S3
+  -D BOARD=BOARD_ESP32S3_SUPERMINI
+board = esp32s3_supermini
+board_upload.use_1200bps_touch = 1
+board_upload.wait_for_upload_port = 1
+board_upload.require_upload_port = 1
+upload_speed = 921600

platformio.ini

@@ -18,6 +18,8 @@ build_cache_dir = cache
 lib_deps=
   https://github.com/SlimeVR/CmdParser.git
   https://github.com/SlimeVR/base64_arduino.git
+  https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library.git
+  https://github.com/hideakitai/PCA9547.git
 monitor_speed = 115200
 monitor_echo = yes
 monitor_filters = colorize
@@ -66,6 +68,14 @@ board = esp12e
 ; Comment out this line below if you have any trouble uploading the firmware
 ; and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed
 upload_speed = 921600
+build_flags =
+  ${env.build_flags}
+  -D BOARD=BOARD_SLIMEVR_V1_2
+  -D VENDOR_NAME='"SlimeVR"'
+  -D VENDOR_URL='"https://slimevr.dev"'
+  -D PRODUCT_NAME='"SlimeVR Tracker v1.2"'
+  -D UPDATE_ADDRESS='"SlimeVR/SlimeVR-Tracker-ESP"'
+  -D UPDATE_NAME='"BOARD_SLIMEVR_V1_2-firmware"'
 
 ; Uncomment below if you want to build for ESP-01
 ;[env:esp01_1m]
@@ -109,3 +119,29 @@ upload_speed = 921600
 ;  ${env.build_flags}
 ;  -DESP32C3
 ;board = lolin_c3_mini
+;monitor_filters = colorize, esp32_exception_decoder
+
+; If you want to use a ESP32C6, you can use this (experimental)
+;[env:esp32c6]
+;platform =  https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
+;board = esp32-c6-devkitc-1
+;build_flags =
+;  ${env.build_flags}
+;  -DESP32C6
+;  -DARDUINO_USB_MODE=1
+;  -DARDUINO_USB_CDC_ON_BOOT=1
+
+;[env:BOARD_ESP32S3_SUPERMINI]
+;platform = espressif32 @ 6.7.0
+;platform_packages =
+;  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+;  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+;build_flags = 
+;  ${env.build_flags}
+;  -DARDUINO_USB_MODE=1
+;  -DESP32S3
+;board = esp32s3_supermini
+;board_upload.use_1200bps_touch = 1
+;board_upload.wait_for_upload_port = 1
+;board_upload.require_upload_port = 1
+;upload_speed = 921600

scripts/get_git_commit.py

@@ -9,11 +9,38 @@ if not env_rev is None and env_rev != "":
 else:
     try:
         revision = (
-            subprocess.check_output(["git", "rev-parse", "HEAD"])
+            subprocess.check_output(["git", "rev-parse", "--short", "HEAD"])
             .strip()
             .decode("utf-8")
         )
     except Exception:
         revision = "NOT_GIT"
 
-print(f"'-DGIT_REV=\"{revision}\"'")
+tag = ""
+try:
+	tag = subprocess.check_output(["git", "--no-pager", "tag", "--sort", "-taggerdate", "--points-at" , "HEAD"]).strip().decode("utf-8")
+	if tag.startswith("v"):
+		tag = tag[1:]
+except Exception:
+	tag = ""
+
+branch = ""
+try:
+	branch = (
+		subprocess.check_output(["git", "symbolic-ref", "--short", "-q", "HEAD"])
+			.strip()
+			.decode("utf-8")
+	)
+except Exception:
+	branch = ""
+
+output = f"-DGIT_REV='\"{revision}\"'"
+
+if tag != "":
+	output += f" -DFIRMWARE_VERSION='\"{tag}\"'"
+elif branch != "":
+	output += f" -DFIRMWARE_VERSION='\"{branch}\"'"
+else:
+	output += f" -DFIRMWARE_VERSION='\"git-{revision}\"'"
+
+print(output)

src/FSHelper.cpp

@@ -25,8 +25,7 @@
 
 #include <functional>
 
-namespace SlimeVR {
-namespace Utils {
+namespace SlimeVR::Utils {
 SlimeVR::Logging::Logger m_Logger("FSHelper");
 
 bool ensureDirectory(const char* directory) {
@@ -88,5 +87,4 @@ void forEachFile(const char* directory, std::function<void(File file)> callback)
 	}
 #endif
 }
-}  // namespace Utils
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Utils

src/FSHelper.h

@@ -29,8 +29,7 @@
 
 #include <functional>
 
-namespace SlimeVR {
-namespace Utils {
+namespace SlimeVR::Utils {
 
 class File {
 public:
@@ -62,7 +61,6 @@ bool ensureDirectory(const char* directory);
 File openFile(const char* path, const char* mode);
 
 void forEachFile(const char* directory, std::function<void(File file)> callback);
-}  // namespace Utils
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Utils
 
 #endif

src/GlobalVars.h

@@ -20,19 +20,17 @@
 	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 	THE SOFTWARE.
 */
-
-#ifndef GLOBALVARS_H
-#define GLOBALVARS_H
+#pragma once
 
 #include <arduino-timer.h>
 
-#include "LEDManager.h"
+#include "batterymonitor.h"
 #include "configuration/Configuration.h"
 #include "network/connection.h"
 #include "network/manager.h"
 #include "sensors/SensorManager.h"
+#include "status/LEDManager.h"
 #include "status/StatusManager.h"
-#include "batterymonitor.h"
 
 extern Timer<> globalTimer;
 extern SlimeVR::LEDManager ledManager;
@@ -42,5 +40,3 @@ extern SlimeVR::Sensors::SensorManager sensorManager;
 extern SlimeVR::Network::Manager networkManager;
 extern SlimeVR::Network::Connection networkConnection;
 extern BatteryMonitor battery;
-
-#endif

src/LEDManager.cpp

@@ -1,212 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
-
-    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 "LEDManager.h"
-#include "GlobalVars.h"
-#include "status/Status.h"
-
-namespace SlimeVR
-{
-    void LEDManager::setup()
-    {
-#if ENABLE_LEDS
-        pinMode(m_Pin, OUTPUT);
-#endif
-
-        // Do the initial pull of the state
-        update();
-    }
-
-    void LEDManager::on()
-    {
-#if ENABLE_LEDS
-        digitalWrite(m_Pin, LED__ON);
-#endif
-    }
-
-    void LEDManager::off()
-    {
-#if ENABLE_LEDS
-        digitalWrite(m_Pin, LED__OFF);
-#endif
-    }
-
-    void LEDManager::blink(unsigned long time)
-    {
-        on();
-        delay(time);
-        off();
-    }
-
-    void LEDManager::pattern(unsigned long timeon, unsigned long timeoff, int times)
-    {
-        for (int i = 0; i < times; i++)
-        {
-            blink(timeon);
-            delay(timeoff);
-        }
-    }
-
-    void LEDManager::update()
-    {
-        unsigned long time = millis();
-        unsigned long diff = time - m_LastUpdate;
-
-        // Don't tick the LEDManager *too* often
-        if (diff < 10)
-        {
-            return;
-        }
-
-        m_LastUpdate = time;
-
-        unsigned int length = 0;
-        unsigned int count = 0;
-
-        if (statusManager.hasStatus(Status::LOW_BATTERY))
-        {
-            count = LOW_BATTERY_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = LOW_BATTERY_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = LOW_BATTERY_INTERVAL;
-                break;
-            }
-        }
-        else if (statusManager.hasStatus(Status::IMU_ERROR))
-        {
-            count = IMU_ERROR_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = IMU_ERROR_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = IMU_ERROR_INTERVAL;
-                break;
-            }
-        }
-        else if (statusManager.hasStatus(Status::WIFI_CONNECTING))
-        {
-            count = WIFI_CONNECTING_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = WIFI_CONNECTING_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = WIFI_CONNECTING_INTERVAL;
-                break;
-            }
-        }
-        else if (statusManager.hasStatus(Status::SERVER_CONNECTING))
-        {
-            count = SERVER_CONNECTING_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = SERVER_CONNECTING_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = SERVER_CONNECTING_INTERVAL;
-                break;
-            }
-        }
-        else
-        {
-#if defined(LED_INTERVAL_STANDBY) && LED_INTERVAL_STANDBY > 0
-            count = 1;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = STANDBUY_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = LED_INTERVAL_STANDBY;
-                break;
-            }
-#else
-            return;
-#endif
-        }
-
-        if (m_CurrentStage == OFF || m_Timer + diff >= length)
-        {
-            m_Timer = 0;
-            // Advance stage
-            switch (m_CurrentStage)
-            {
-            case OFF:
-                on();
-                m_CurrentStage = ON;
-                m_CurrentCount = 0;
-                break;
-            case ON:
-                off();
-                m_CurrentCount++;
-                if (m_CurrentCount >= count)
-                {
-                    m_CurrentCount = 0;
-                    m_CurrentStage = INTERVAL;
-                }
-                else
-                {
-                    m_CurrentStage = GAP;
-                }
-                break;
-            case GAP:
-            case INTERVAL:
-                on();
-                m_CurrentStage = ON;
-                break;
-            }
-        }
-        else
-        {
-            m_Timer += diff;
-        }
-    }
-}

src/LEDManager.h

@@ -1,103 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
-
-    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.
-*/
-#ifndef SLIMEVR_LEDMANAGER_H
-#define SLIMEVR_LEDMANAGER_H
-
-#include <Arduino.h>
-#include "globals.h"
-#include "logging/Logger.h"
-
-#define DEFAULT_LENGTH 300
-#define DEFAULT_GAP 500
-#define DEFAULT_INTERVAL 3000
-
-#define STANDBUY_LENGTH DEFAULT_LENGTH
-#define IMU_ERROR_LENGTH DEFAULT_LENGTH
-#define IMU_ERROR_INTERVAL 1000
-#define IMU_ERROR_COUNT 5
-#define LOW_BATTERY_LENGTH DEFAULT_LENGTH
-#define LOW_BATTERY_INTERVAL 300
-#define LOW_BATTERY_COUNT 1
-#define WIFI_CONNECTING_LENGTH DEFAULT_LENGTH
-#define WIFI_CONNECTING_INTERVAL 3000
-#define WIFI_CONNECTING_COUNT 3
-#define SERVER_CONNECTING_LENGTH DEFAULT_LENGTH
-#define SERVER_CONNECTING_INTERVAL 3000
-#define SERVER_CONNECTING_COUNT 2
-
-namespace SlimeVR
-{
-    enum LEDStage
-    {
-        OFF,
-        ON,
-        GAP,
-        INTERVAL
-    };
-
-    class LEDManager
-    {
-    public:
-        LEDManager(uint8_t pin) : m_Pin(pin) {}
-
-        void setup();
-
-        /*!
-         *  @brief Turns the LED on
-         */
-        void on();
-
-        /*!
-         *  @brief Turns the LED off
-         */
-        void off();
-
-        /*!
-         *  @brief Blink the LED for [time]ms. *Can* cause lag
-         *  @param time Amount of ms to turn the LED on
-         */
-        void blink(unsigned long time);
-
-        /*!
-         *  @brief Show a pattern on the LED. *Can* cause lag
-         *  @param timeon Amount of ms to turn the LED on
-         *  @param timeoff Amount of ms to turn the LED off
-         *  @param times Amount of times to display the pattern
-         */
-        void pattern(unsigned long timeon, unsigned long timeoff, int times);
-
-        void update();
-
-    private:
-        uint8_t m_CurrentCount = 0;
-        unsigned long m_Timer = 0;
-        LEDStage m_CurrentStage = OFF;
-        unsigned long m_LastUpdate = millis();
-
-        uint8_t m_Pin;
-
-        Logging::Logger m_Logger = Logging::Logger("LEDManager");
-    };
-}
-
-#endif

src/batterymonitor.cpp

@@ -1,138 +1,132 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 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:
+	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 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.
+	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 "batterymonitor.h"
+
 #include "GlobalVars.h"
 
-#if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
+#if ESP8266 \
+	&& (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
 ADC_MODE(ADC_VCC);
 #endif
 
-void BatteryMonitor::Setup()
-{
+void BatteryMonitor::Setup() {
 #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-    for (uint8_t i = 0x48; i < 0x4F; i++)
-    {
-        if (I2CSCAN::hasDevOnBus(i))
-        {
-            address = i;
-            break;
-        }
-    }
-    if (address == 0)
-    {
-        m_Logger.error("MCP3021 not found on I2C bus");
-    }
+	for (uint8_t i = 0x48; i < 0x4F; i++) {
+		if (I2CSCAN::hasDevOnBus(i)) {
+			address = i;
+			break;
+		}
+	}
+	if (address == 0) {
+		m_Logger.error("MCP3021 not found on I2C bus");
+	}
 #endif
 }
 
-void BatteryMonitor::Loop()
-{
-    #if BATTERY_MONITOR == BAT_EXTERNAL || BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-        auto now_ms = millis();
-        if (now_ms - last_battery_sample >= batterySampleRate)
-        {
-            last_battery_sample = now_ms;
-            voltage = -1;
-            #if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
-                // Find out what your max measurement is (voltage_3_3).
-                // Take the max measurement and check if it was less than 50mV
-                // if yes output 5.0V
-                // if no output 3.3V - dropvoltage + 0.1V
-                auto ESPmV = ESP.getVcc();
-                if (ESPmV > voltage_3_3)
-                {
-                    voltage_3_3 = ESPmV;
-                }
-                else
-                {
-                    //Calculate drop in mV
-                    ESPmV = voltage_3_3 - ESPmV;
-                    if (ESPmV < 50)
-                    {
-                        voltage = 5.0F;
-                    }
-                    else
-                    {
-                        voltage = 3.3F - ((float)ESPmV / 1000.0F) + 0.1F; //we assume 100mV drop on the linear converter
-                    }
-                }
-            #endif
-            #if ESP8266 && BATTERY_MONITOR == BAT_EXTERNAL
-                voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * ADCVoltageMax / ADCResolution * ADCMultiplier;
-            #endif
-            #if ESP32 && BATTERY_MONITOR == BAT_EXTERNAL
-                voltage = ((float)analogReadMilliVolts(PIN_BATTERY_LEVEL)) / 1000 * ADCMultiplier;
-            #endif
-            #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-                if (address > 0)
-                {
-                    Wire.beginTransmission(address);
-                    Wire.requestFrom(address, (uint8_t)2);
-                    auto MSB = Wire.read();
-                    auto LSB = Wire.read();
-                    auto status = Wire.endTransmission();
-                    if (status == 0)
-                    {
-                        float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2));
-                        v *= ADCMultiplier;
-                        voltage = (voltage > 0) ? min(voltage, v) : v;
-                    }
-                }
-            #endif
-            if (voltage > 0) //valid measurement
-            {
-                // Estimate battery level, 3.2V is 0%, 4.17V is 100% (1.0)
-                if (voltage > 3.975f)
-                    level = (voltage - 2.920f) * 0.8f;
-                else if (voltage > 3.678f)
-                    level = (voltage - 3.300f) * 1.25f;
-                else if (voltage > 3.489f)
-                    level = (voltage - 3.400f) * 1.7f;
-                else if (voltage > 3.360f)
-                    level = (voltage - 3.300f) * 0.8f;
-                else
-                    level = (voltage - 3.200f) * 0.3f;
+void BatteryMonitor::Loop() {
+#if BATTERY_MONITOR == BAT_EXTERNAL || BATTERY_MONITOR == BAT_INTERNAL \
+	|| BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
+	auto now_ms = millis();
+	if (now_ms - last_battery_sample >= batterySampleRate) {
+		last_battery_sample = now_ms;
+		voltage = -1;
+#if ESP8266 \
+	&& (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
+		// Find out what your max measurement is (voltage_3_3).
+		// Take the max measurement and check if it was less than 50mV
+		// if yes output 5.0V
+		// if no output 3.3V - dropvoltage + 0.1V
+		auto ESPmV = ESP.getVcc();
+		if (ESPmV > voltage_3_3) {
+			voltage_3_3 = ESPmV;
+		} else {
+			// Calculate drop in mV
+			ESPmV = voltage_3_3 - ESPmV;
+			if (ESPmV < 50) {
+				voltage = 5.0F;
+			} else {
+				voltage = 3.3F - ((float)ESPmV / 1000.0F)
+						+ 0.1F;  // we assume 100mV drop on the linear converter
+			}
+		}
+#endif
+#if ESP8266 && BATTERY_MONITOR == BAT_EXTERNAL
+		voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * ADCVoltageMax / ADCResolution
+				* ADCMultiplier;
+#endif
+#if defined(ESP32) && BATTERY_MONITOR == BAT_EXTERNAL
+		voltage
+			= ((float)analogReadMilliVolts(PIN_BATTERY_LEVEL)) / 1000 * ADCMultiplier;
+#endif
+#if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
+		if (address > 0) {
+			Wire.beginTransmission(address);
+			Wire.requestFrom(address, (uint8_t)2);
+			auto MSB = Wire.read();
+			auto LSB = Wire.read();
+			auto status = Wire.endTransmission();
+			if (status == 0) {
+				float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2));
+				v *= ADCMultiplier;
+				voltage = (voltage > 0) ? min(voltage, v) : v;
+			}
+		}
+#endif
+		if (voltage > 0)  // valid measurement
+		{
+			// Estimate battery level, 3.2V is 0%, 4.17V is 100% (1.0)
+			if (voltage > 3.975f) {
+				level = (voltage - 2.920f) * 0.8f;
+			} else if (voltage > 3.678f) {
+				level = (voltage - 3.300f) * 1.25f;
+			} else if (voltage > 3.489f) {
+				level = (voltage - 3.400f) * 1.7f;
+			} else if (voltage > 3.360f) {
+				level = (voltage - 3.300f) * 0.8f;
+			} else {
+				level = (voltage - 3.200f) * 0.3f;
+			}
 
-                level = (level - 0.05f) / 0.95f; // Cut off the last 5% (3.36V)
+			level = (level - 0.05f) / 0.95f;  // Cut off the last 5% (3.36V)
 
-                if (level > 1)
-                    level = 1;
-                else if (level < 0)
-                    level = 0;
-                networkConnection.sendBatteryLevel(voltage, level);
-                #ifdef BATTERY_LOW_POWER_VOLTAGE
-                    if (voltage < BATTERY_LOW_POWER_VOLTAGE)
-                    {
-                        #if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP
-                            ESP.deepSleep(0);
-                        #else
-                            statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true);
-                        #endif
-                    } else {
-                        statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false);
-                    }
-                #endif
-            }
-        }
-    #endif
+			if (level > 1) {
+				level = 1;
+			} else if (level < 0) {
+				level = 0;
+			}
+			networkConnection.sendBatteryLevel(voltage, level);
+#ifdef BATTERY_LOW_POWER_VOLTAGE
+			if (voltage < BATTERY_LOW_POWER_VOLTAGE) {
+#if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP
+				ESP.deepSleep(0);
+#else
+				statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true);
+#endif
+			} else {
+				statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false);
+			}
+#endif
+		}
+	}
+#endif
 }

src/batterymonitor.h

@@ -1,92 +1,95 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 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:
+	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 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.
+	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.
 */
 #ifndef SLIMEVR_BATTERYMONITOR_H_
 #define SLIMEVR_BATTERYMONITOR_H_
 
 #include <Arduino.h>
-#include "globals.h"
-#include <i2cscan.h>
 #include <I2Cdev.h>
+#include <i2cscan.h>
+
+#include "globals.h"
 #include "logging/Logger.h"
 
 #if ESP8266
-    #define ADCResolution 1023.0  // ESP8266 has 10bit ADC
-    #define ADCVoltageMax 1.0     // ESP8266 input is 1.0 V = 1023.0
+#define ADCResolution 1023.0  // ESP8266 has 10bit ADC
+#define ADCVoltageMax 1.0  // ESP8266 input is 1.0 V = 1023.0
 #endif
 #ifndef ADCResolution
-    #define ADCResolution 1023.0
+#define ADCResolution 1023.0
 #endif
 #ifndef ADCVoltageMax
-    #define ADCVoltageMax 1.0
+#define ADCVoltageMax 1.0
 #endif
 
 #ifndef BATTERY_SHIELD_RESISTANCE
-    #define BATTERY_SHIELD_RESISTANCE 180.0
+#define BATTERY_SHIELD_RESISTANCE 180.0
 #endif
 #ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 100.0
+#define BATTERY_SHIELD_R1 100.0
 #endif
 #ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 220.0
+#define BATTERY_SHIELD_R2 220.0
 #endif
 
 #if BATTERY_MONITOR == BAT_EXTERNAL
-    #ifndef PIN_BATTERY_LEVEL
-        #error Internal ADC enabled without pin! Please select a pin.
-    #endif
-    // Wemos D1 Mini has an internal Voltage Divider with R1=100K and R2=220K > this means, 3.3V analogRead input voltage results in 1023.0
-    // Wemos D1 Mini with Wemos Battery Shield v1.2.0 or higher: Battery Shield with J2 closed, has an additional 130K resistor. So the resulting Voltage Divider is R1=220K+100K=320K and R2=100K > this means, 4.5V analogRead input voltage results in 1023.0
-    // ESP32 Boards may have not the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can be changed.
-    // Diagramm:
-    //   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2] ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
-    // SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0
-  #define ADCMultiplier (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) / BATTERY_SHIELD_R1
+// Wemos D1 Mini has an internal Voltage Divider with R1=100K and R2=220K > this
+// means, 3.3V analogRead input voltage results in 1023.0 Wemos D1 Mini with Wemos
+// Battery Shield v1.2.0 or higher: Battery Shield with J2 closed, has an additional
+// 130K resistor. So the resulting Voltage Divider is R1=220K+100K=320K and R2=100K >
+// this means, 4.5V analogRead input voltage results in 1023.0 ESP32 Boards may have not
+// the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can
+// be changed. Diagramm:
+//   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2]
+//   ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
+// SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0
+#define ADCMultiplier                                                   \
+	(BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) \
+		/ BATTERY_SHIELD_R1
 #elif BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-  // Default recommended resistors are 9.1k and 5.1k
-  #define ADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1
+// Default recommended resistors are 9.1k and 5.1k
+#define ADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1
 #endif
 
-class BatteryMonitor
-{
+class BatteryMonitor {
 public:
-    void Setup();
-    void Loop();
+	void Setup();
+	void Loop();
 
-    float getVoltage() const { return voltage; }
-    float getLevel() const { return level; }
+	float getVoltage() const { return voltage; }
+	float getLevel() const { return level; }
 
 private:
-    unsigned long last_battery_sample = 0;
+	unsigned long last_battery_sample = 0;
 #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-    uint8_t address = 0;
+	uint8_t address = 0;
 #endif
 #if BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-    uint16_t voltage_3_3 = 3000;
+	uint16_t voltage_3_3 = 3000;
 #endif
-    float voltage = -1;
-    float level = -1;
+	float voltage = -1;
+	float level = -1;
 
-    SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor");
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor");
 };
 
-#endif // SLIMEVR_BATTERYMONITOR_H_
+#endif  // SLIMEVR_BATTERYMONITOR_H_

src/boards/boards_default.h

@@ -0,0 +1,258 @@
+/*
+ * LED configuration:
+ * Configuration Priority 1 = Highest:
+ * 1. LED_PIN
+ * 2. LED_BUILTIN
+ *
+ *   LED_PIN
+ *     - Number or Symbol (D1,..) of the Output
+ *     - To turn off the LED, set LED_PIN to LED_OFF
+ *   LED_INVERTED
+ *     - false for output 3.3V on high
+ *     - true for pull down to GND on high
+ */
+
+/*
+ * D1 Mini boards with ESP8266 have internal resistors. For these boards you only have
+ * to adjust BATTERY_SHIELD_RESISTANCE. For other boards you can now adjust the other
+ * resistor values. The diagram looks like this:
+ *   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2]
+ *   ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
+ * BATTERY_SHIELD_R(180)
+ * 130k BatteryShield, 180k SlimeVR or fill in
+ * external resistor value in kOhm BATTERY_R1(100)
+ * Board voltage divider resistor Ain to GND in kOhm BATTERY_R2(220)
+ * Board voltage divider resistor Ain to INPUT_BOARD in kOhm
+ */
+
+#include "defines_helpers.h"
+
+// Board-specific configurations
+#if BOARD == BOARD_SLIMEVR
+
+SDA(14)
+SCL(12)
+INT(16)
+INT2(13)
+BATTERY(17)
+LED(2)
+INVERTED_LED(true)
+BATTERY_SHIELD_R(0)
+BATTERY_R1(10)
+BATTERY_R2(40.2)
+
+#elif BOARD == BOARD_SLIMEVR_V1_2
+
+SDA(4)
+SCL(5)
+INT(2)
+INT2(16)
+BATTERY(17)
+LED(2)
+INVERTED_LED(true)
+BATTERY_SHIELD_R(0)
+BATTERY_R1(10)
+BATTERY_R2(40.2)
+
+#elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV
+
+SDA(4)
+SCL(5)
+INT(10)
+INT2(13)
+BATTERY(17)
+LED(2)
+INVERTED_LED(true)
+BATTERY_SHIELD_R(0)
+BATTERY_R1(10)
+BATTERY_R2(40.2)
+
+#elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI
+
+SDA(D2)
+SCL(D1)
+INT(D5)
+INT2(D6)
+BATTERY(A0)
+BATTERY_SHIELD_R(180)
+BATTERY_R1(100)
+BATTERY_R2(220)
+
+#elif BOARD == BOARD_ESP01
+
+SDA(2)
+SCL(0)
+INT(255)
+INT2(255)
+BATTERY(255)
+LED(LED_OFF)
+INVERTED_LED(false)
+
+#elif BOARD == BOARD_TTGO_TBASE
+
+SDA(5)
+SCL(4)
+INT(14)
+INT2(13)
+BATTERY(A0)
+
+#elif BOARD == BOARD_CUSTOM
+
+// Define pins by the examples above
+
+#elif BOARD == BOARD_WROOM32
+
+SDA(21)
+SCL(22)
+INT(23)
+INT2(25)
+BATTERY(36)
+
+#elif BOARD == BOARD_LOLIN_C3_MINI
+
+SDA(5)
+SCL(4)
+INT(6)
+INT2(8)
+BATTERY(3)
+LED(7)
+
+#elif BOARD == BOARD_BEETLE32C3
+
+SDA(8)
+SCL(9)
+INT(6)
+INT2(7)
+BATTERY(3)
+LED(10)
+INVERTED_LED(false)
+
+#elif BOARD == BOARD_ESP32C3DEVKITM1 || BOARD == BOARD_ESP32C6DEVKITC1
+
+SDA(5)
+SCL(4)
+INT(6)
+INT2(7)
+BATTERY(3)
+LED(LED_OFF)
+
+#elif BOARD == BOARD_WEMOSWROOM02
+
+SDA(2)
+SCL(14)
+INT(0)
+INT2(4)
+BATTERY(A0)
+LED(16)
+INVERTED_LED(true)
+
+#elif BOARD == BOARD_XIAO_ESP32C3
+
+SDA(6)
+SCL(7)  // D5
+INT(5)  // D3
+INT2(10)  // D10
+LED(4)  // D2
+INVERTED_LED(false)
+BATTERY(2)  // D0 / A0
+BATTERY_SHIELD_R(0)
+BATTERY_R1(100)
+BATTERY_R2(100)
+
+#elif BOARD == BOARD_GLOVE_IMU_SLIMEVR_DEV
+
+SDA(1)
+SCL(0)
+#define PCA_ADDR 0x70
+INT(16)
+INT2(13)
+BATTERY(3)
+LED(2)
+INVERTED_LED(true)
+BATTERY_SHIELD_R(0)
+BATTERY_R1(10)
+BATTERY_R2(40.2)
+
+#elif BOARD == BOARD_ESP32S3_SUPERMINI
+
+SDA(7)
+SCL(6)
+INT(5)
+INT2(4)
+BATTERY(A2)  // IO3
+BATTERY_SHIELD_R(0)
+BATTERY_R1(10)
+BATTERY_R2(40.2)
+LED(LED_BUILTIN)
+
+#endif
+
+// Default IMU pinouts and definitions for default tracker types
+
+#if BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV
+// Defaunlt definitions for normal 2-sensor trackers
+#ifndef MAX_SENSORS_COUNT
+#define MAX_SENSORS_COUNT 2
+#endif
+#ifndef TRACKER_TYPE
+#define TRACKER_TYPE TrackerType::TRACKER_TYPE_SVR_ROTATION
+#endif
+
+// Axis mapping example
+/*
+#include "sensors/axisremap.h"
+#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN,
+AXIS_REMAP_USE_Z, \ AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)
+
+SENSOR_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL,
+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_PIN(15),                              \
+		IMU_ROTATION,                                \
+		DIRECT_SPI(24'000'000, MSBFIRST, SPI_MODE3), \
+		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,                                   \
+		PRIMARY_IMU_ADDRESS_ONE,               \
+		IMU_ROTATION,                          \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \
+		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                                      \
+	)
+#endif
+#endif
+#else  // BOARD == BOARD_GLOVE_IMU_SLIMEVR_DEV
+
+#include "glove_default.h"
+
+#endif  // BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV

src/boards/defines_helpers.cpp

@@ -0,0 +1,33 @@
+/*
+	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 "defines_helpers.h"
+
+#include "../consts.h"
+
+#ifndef LED_BUILTIN
+#define LED_BUILTIN LED_OFF
+#endif
+
+extern const uint8_t __attribute__((weak)) LED_PIN = LED_BUILTIN;
+extern const bool __attribute__((weak)) LED_INVERTED = true;

src/boards/defines_helpers.h

@@ -0,0 +1,96 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+
+#include "pins_arduino.h"
+
+#ifndef PIN_IMU_SDA
+#define SDA(pin) constexpr uint8_t PIN_IMU_SDA = pin;
+#else
+#define SDA(pin)
+#endif
+
+#ifndef PIN_IMU_SCL
+#define SCL(pin) constexpr uint8_t PIN_IMU_SCL = pin;
+#else
+#define SCL(pin)
+#endif
+
+#ifndef PIN_IMU_INT
+#define INT(pin) constexpr uint8_t PIN_IMU_INT = pin;
+#else
+#define INT(pin)
+#endif
+
+#ifndef PIN_IMU_INT_2
+#define INT2(pin) constexpr uint8_t PIN_IMU_INT_2 = pin;
+#else
+#define INT2(pin)
+#endif
+
+#ifndef PIN_BATTERY_LEVEL
+#define BATTERY(pin) constexpr uint8_t PIN_BATTERY_LEVEL = pin;
+#else
+#define BATTERY(pin)
+#endif
+
+#ifndef LED_PIN
+#define LED(pin) const uint8_t LED_PIN = pin;
+#else
+#define LED(pin)
+#endif
+
+#ifndef LED_PIN
+extern const uint8_t __attribute__((weak)) LED_PIN;
+#endif
+
+#ifndef BATTERY_SHIELD_RESISTANCE
+#define BATTERY_SHIELD_R(value) constexpr float BATTERY_SHIELD_RESISTANCE = value;
+#else
+#define BATTERY_SHIELD_R(value)
+#endif
+
+#ifndef BATTERY_SHIELD_R1
+#define BATTERY_R1(value) constexpr float BATTERY_SHIELD_R1 = value;
+#else
+#define BATTERY_R1(value)
+#endif
+
+#ifndef BATTERY_SHIELD_R2
+#define BATTERY_R2(value) constexpr float BATTERY_SHIELD_R2 = value;
+#else
+#define BATTERY_R2(value)
+#endif
+
+#ifndef LED_INVERTED
+#define INVERTED_LED(value) const bool LED_INVERTED = value;
+#else
+#define INVERTED_LED(value)
+#endif
+
+#ifndef LED_INVERTED
+extern const bool __attribute__((weak)) LED_INVERTED;
+#endif

src/boards/glove_default.h

@@ -0,0 +1,140 @@
+// default definitions for the GLOVE
+#ifndef MAX_SENSORS_COUNT
+#define MAX_SENSORS_COUNT 10
+#endif
+#ifndef TRACKER_TYPE
+#define TRACKER_TYPE TrackerType::TRACKER_TYPE_SVR_GLOVE_LEFT
+#endif
+#ifndef GLOVE_SIDE
+#define GLOVE_SIDE GLOVE_LEFT
+#endif
+#ifndef PRIMARY_IMU_ADDRESS_ONE
+#define PRIMARY_IMU_ADDRESS_ONE 0x4a
+#endif
+#ifndef SECONDARY_IMU_ADDRESS_TWO
+#define SECONDARY_IMU_ADDRESS_TWO 0x4b
+#endif
+
+#ifndef SENSOR_DESC_LIST
+#define SENSOR_DESC_LIST                                 \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		(PRIMARY_IMU_ADDRESS_ONE ^ 0x02),                \
+		IMU_ROTATION,                                    \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA),           \
+		false,                                           \
+		MCP_PIN(MCP_GPA6),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		(SECONDARY_IMU_ADDRESS_TWO ^ 0x02),              \
+		IMU_ROTATION,                                    \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA),           \
+		true,                                            \
+		MCP_PIN(MCP_GPA5),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		PRIMARY_IMU_ADDRESS_ONE,                         \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 0), \
+		true,                                            \
+		MCP_PIN(MCP_GPB0),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		SECONDARY_IMU_ADDRESS_TWO,                       \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 0), \
+		true,                                            \
+		MCP_PIN(MCP_GPB1),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		PRIMARY_IMU_ADDRESS_ONE,                         \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 1), \
+		true,                                            \
+		MCP_PIN(MCP_GPB2),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		SECONDARY_IMU_ADDRESS_TWO,                       \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 1), \
+		true,                                            \
+		MCP_PIN(MCP_GPB3),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		PRIMARY_IMU_ADDRESS_ONE,                         \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 2), \
+		true,                                            \
+		MCP_PIN(MCP_GPB4),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		SECONDARY_IMU_ADDRESS_TWO,                       \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 2), \
+		true,                                            \
+		MCP_PIN(MCP_GPB5),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		PRIMARY_IMU_ADDRESS_ONE,                         \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 3), \
+		true,                                            \
+		MCP_PIN(MCP_GPB6),                               \
+		0                                                \
+	)                                                    \
+	SENSOR_DESC_ENTRY(                                   \
+		IMU,                                             \
+		SECONDARY_IMU_ADDRESS_TWO,                       \
+		IMU_ROTATION,                                    \
+		PCA_WIRE(PIN_IMU_SCL, PIN_IMU_SDA, PCA_ADDR, 3), \
+		true,                                            \
+		MCP_PIN(MCP_GPA1),                               \
+		0                                                \
+	)
+#endif
+
+#ifndef SENSOR_INFO_LIST
+#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 == GLOVE_RIGHT
+#define SENSOR_INFO_LIST                                                     \
+	SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_RIGHT_HAND)                \
+	SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_RIGHT_LITTLE_INTERMEDIATE) \
+	SENSOR_INFO_ENTRY(2, SensorPosition::POSITION_RIGHT_RING_INTERMEDIATE)   \
+	SENSOR_INFO_ENTRY(3, SensorPosition::POSITION_RIGHT_RING_DISTAL)         \
+	SENSOR_INFO_ENTRY(4, SensorPosition::POSITION_RIGHT_MIDDLE_INTERMEDIATE) \
+	SENSOR_INFO_ENTRY(5, SensorPosition::POSITION_RIGHT_MIDDLE_DISTAL)       \
+	SENSOR_INFO_ENTRY(6, SensorPosition::POSITION_RIGHT_INDEX_INTERMEDIATE)  \
+	SENSOR_INFO_ENTRY(7, SensorPosition::POSITION_RIGHT_INDEX_DISTAL)        \
+	SENSOR_INFO_ENTRY(8, SensorPosition::POSITION_RIGHT_THUMB_PROXIMAL)      \
+	SENSOR_INFO_ENTRY(9, SensorPosition::POSITION_RIGHT_THUMB_DISTAL)
+#else  // GLOVE_SIDE
+#error "Glove side not defined"
+#endif  // GLOVE_SIDE
+#endif

src/calibration.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 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:
+	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 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.
+	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.
 */
 #ifndef SLIMEVR_CALIBRATION_H_
 #define SLIMEVR_CALIBRATION_H_
@@ -31,4 +31,4 @@
 #define CALIBRATION_TYPE_EXTERNAL_ACCEL 6
 #define CALIBRATION_TYPE_EXTERNAL_MAG 7
 
-#endif // SLIMEVR_CALIBRATION_H_
+#endif  // SLIMEVR_CALIBRATION_H_

src/configuration/Configuration.cpp

@@ -1,327 +1,444 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    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:
+	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 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.
+	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 "Configuration.h"
+
 #include <LittleFS.h>
 
-#include "Configuration.h"
+#include "../FSHelper.h"
 #include "consts.h"
 #include "utils.h"
-#include "../FSHelper.h"
 
 #define DIR_CALIBRATIONS "/calibrations"
 #define DIR_TEMPERATURE_CALIBRATIONS "/tempcalibrations"
+#define DIR_TOGGLES "/toggles"
 
-namespace SlimeVR {
-    namespace Configuration {
-        void Configuration::setup() {
-            if (m_Loaded) {
-                return;
-            }
+namespace SlimeVR::Configuration {
+void Configuration::setup() {
+	if (m_Loaded) {
+		return;
+	}
 
-            bool status = LittleFS.begin();
-            if (!status) {
-                this->m_Logger.warn("Could not mount LittleFS, formatting");
+	bool status = LittleFS.begin();
+	if (!status) {
+		this->m_Logger.warn("Could not mount LittleFS, formatting");
 
-                status = LittleFS.format();
-                if (!status) {
-                    this->m_Logger.warn("Could not format LittleFS, aborting");
-                    return;
-                }
+		status = LittleFS.format();
+		if (!status) {
+			this->m_Logger.warn("Could not format LittleFS, aborting");
+			return;
+		}
 
-                status = LittleFS.begin();
-                if (!status) {
-                    this->m_Logger.error("Could not mount LittleFS, aborting");
-                    return;
-                }
-            }
+		status = LittleFS.begin();
+		if (!status) {
+			this->m_Logger.error("Could not mount LittleFS, aborting");
+			return;
+		}
+	}
 
-            if (LittleFS.exists("/config.bin")) {
-                m_Logger.trace("Found configuration file");
+	if (LittleFS.exists("/config.bin")) {
+		m_Logger.trace("Found configuration file");
 
-                auto file = LittleFS.open("/config.bin", "r");
+		auto file = LittleFS.open("/config.bin", "r");
 
-                file.read((uint8_t*)&m_Config.version, sizeof(int32_t));
+		file.read((uint8_t*)&m_Config.version, sizeof(int32_t));
 
-                if (m_Config.version < CURRENT_CONFIGURATION_VERSION) {
-                    m_Logger.debug("Configuration is outdated: v%d < v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION);
+		if (m_Config.version < CURRENT_CONFIGURATION_VERSION) {
+			m_Logger.debug(
+				"Configuration is outdated: v%d < v%d",
+				m_Config.version,
+				CURRENT_CONFIGURATION_VERSION
+			);
 
-                    if (!runMigrations(m_Config.version)) {
-                        m_Logger.error("Failed to migrate configuration from v%d to v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION);
-                        return;
-                    }
-                } else {
-                    m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
-                }
+			if (!runMigrations(m_Config.version)) {
+				m_Logger.error(
+					"Failed to migrate configuration from v%d to v%d",
+					m_Config.version,
+					CURRENT_CONFIGURATION_VERSION
+				);
+				return;
+			}
+		} else {
+			m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
+		}
 
-                file.seek(0);
-                file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
-                file.close();
-            } else {
-                m_Logger.info("No configuration file found, creating new one");
-                m_Config.version = CURRENT_CONFIGURATION_VERSION;
-                save();
-            }
+		file.seek(0);
+		file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
+		file.close();
+	} else {
+		m_Logger.info("No configuration file found, creating new one");
+		m_Config.version = CURRENT_CONFIGURATION_VERSION;
+		save();
+	}
 
-            loadSensors();
+	loadSensors();
 
-            m_Loaded = true;
+	m_Loaded = true;
 
-            m_Logger.info("Loaded configuration");
+	m_Logger.info("Loaded configuration");
 
 #ifdef DEBUG_CONFIGURATION
-            print();
+	print();
 #endif
-        }
-
-        void Configuration::save() {
-            for (size_t i = 0; i < m_Sensors.size(); i++) {
-                SensorConfig config = m_Sensors[i];
-                if (config.type == SensorConfigType::NONE) {
-                    continue;
-                }
-
-                char path[17];
-                sprintf(path, DIR_CALIBRATIONS"/%d", i);
-
-                m_Logger.trace("Saving sensor config data for %d", i);
-
-                File file = LittleFS.open(path, "w");
-                file.write((uint8_t*)&config, sizeof(SensorConfig));
-                file.close();
-            }
-
-            {
-                File file = LittleFS.open("/config.bin", "w");
-                file.write((uint8_t*)&m_Config, sizeof(DeviceConfig));
-                file.close();
-            }
-
-            m_Logger.debug("Saved configuration");
-        }
-
-        void Configuration::reset() {
-            LittleFS.format();
-
-            m_Sensors.clear();
-            m_Config.version = 1;
-            save();
-
-            m_Logger.debug("Reset configuration");
-        }
-
-        int32_t Configuration::getVersion() const {
-            return m_Config.version;
-        }
-
-        size_t Configuration::getSensorCount() const {
-            return m_Sensors.size();
-        }
-
-        SensorConfig Configuration::getSensor(size_t sensorID) const {
-            if (sensorID >= m_Sensors.size()) {
-                return {};
-            }
-
-            return m_Sensors.at(sensorID);
-        }
-
-        void Configuration::setSensor(size_t sensorID, const SensorConfig& config) {
-            size_t currentSensors = m_Sensors.size();
-
-            if (sensorID >= currentSensors) {
-                m_Sensors.resize(sensorID + 1);
-            }
-
-            m_Sensors[sensorID] = config;
-        }
-
-		void Configuration::loadSensors() {
-			SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
-				SensorConfig sensorConfig;
-				f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig));
-
-				uint8_t sensorId = strtoul(f.name(), nullptr, 10);
-				m_Logger.debug(
-					"Found sensor calibration for %s at index %d",
-					calibrationConfigTypeToString(sensorConfig.type),
-					sensorId
-				);
-
-				setSensor(sensorId, sensorConfig);
-			});
-		}
-
-		bool Configuration::loadTemperatureCalibration(
-			uint8_t sensorId,
-			GyroTemperatureCalibrationConfig& config
-		) {
-			if (!SlimeVR::Utils::ensureDirectory(DIR_TEMPERATURE_CALIBRATIONS)) {
-				return false;
-			}
-
-			char path[32];
-			sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
-
-			if (!LittleFS.exists(path)) {
-				return false;
-			}
-
-			auto f = SlimeVR::Utils::openFile(path, "r");
-			if (f.isDirectory()) {
-				return false;
-			}
-
-			if (f.size() != sizeof(GyroTemperatureCalibrationConfig)) {
-				m_Logger.debug(
-					"Found incompatible sensor temperature calibration (size mismatch) "
-					"sensorId:%d, skipping",
-					sensorId
-				);
-				return false;
-			}
-
-			SensorConfigType storedConfigType;
-			f.read((uint8_t*)&storedConfigType, sizeof(SensorConfigType));
-
-			if (storedConfigType != config.type) {
-				m_Logger.debug(
-					"Found incompatible sensor temperature calibration (expected %s, "
-					"found %s) sensorId:%d, skipping",
-					calibrationConfigTypeToString(config.type),
-					calibrationConfigTypeToString(storedConfigType),
-					sensorId
-				);
-				return false;
-			}
-
-			f.seek(0);
-			f.read((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
-			m_Logger.debug(
-				"Found sensor temperature calibration for %s sensorId:%d",
-				calibrationConfigTypeToString(config.type),
-				sensorId
-			);
-			return true;
-		}
-
-        bool Configuration::saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config) {
-            if (config.type == SensorConfigType::NONE) {
-                return false;
-            }
-
-            char path[32];
-            sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId);
-
-            m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId);
-
-            File file = LittleFS.open(path, "w");
-            file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
-            file.close();
-
-            m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId);
-            return true;
-        }
-
-        bool Configuration::runMigrations(int32_t version) {
-            return true;
-        }
-
-        void Configuration::print() {
-            m_Logger.info("Configuration:");
-            m_Logger.info("  Version: %d", m_Config.version);
-            m_Logger.info("  %d Sensors:", m_Sensors.size());
-
-            for (size_t i = 0; i < m_Sensors.size(); i++) {
-                const SensorConfig& c = m_Sensors[i];
-                m_Logger.info("    - [%3d] %s", i, calibrationConfigTypeToString(c.type));
-
-                switch (c.type) {
-                case SensorConfigType::NONE:
-                    break;
-
-                case SensorConfigType::BMI160:
-                    m_Logger.info("            A_B        : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B));
-
-                    m_Logger.info("            A_Ainv     :");
-                    for (uint8_t i = 0; i < 3; i++) {
-                        m_Logger.info("                         %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_Ainv[i]));
-                    }
-
-                    m_Logger.info("            G_off      : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.G_off));
-                    m_Logger.info("            Temperature: %f", c.data.bmi160.temperature);
-
-                    break;
-
-                case SensorConfigType::SFUSION:
-                    m_Logger.info("            A_B        : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B));
-
-                    m_Logger.info("            A_Ainv     :");
-                    for (uint8_t i = 0; i < 3; i++) {
-                        m_Logger.info("                         %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_Ainv[i]));
-                    }
-
-                    m_Logger.info("            G_off      : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.G_off));
-                    m_Logger.info("            Temperature: %f", c.data.sfusion.temperature);
-                    break;
-
-                case SensorConfigType::ICM20948:
-                    m_Logger.info("            G: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.G));
-                    m_Logger.info("            A: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.A));
-                    m_Logger.info("            C: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.C));
-
-                    break;
-
-                case SensorConfigType::MPU9250:
-                    m_Logger.info("            A_B   : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B));
-
-                    m_Logger.info("            A_Ainv:");
-                    for (uint8_t i = 0; i < 3; i++) {
-                        m_Logger.info("                    %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_Ainv[i]));
-                    }
-
-                    m_Logger.info("            M_B   : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B));
-
-                    m_Logger.info("            M_Ainv:");
-                    for (uint8_t i = 0; i < 3; i++) {
-                        m_Logger.info("                    %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_Ainv[i]));
-                    }
-
-                    m_Logger.info("            G_off  : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off));
-
-                    break;
-
-                case SensorConfigType::MPU6050:
-                    m_Logger.info("            A_B  : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B));
-                    m_Logger.info("            G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off));
-
-                    break;
-
-				case SensorConfigType::BNO0XX:
-					m_Logger.info("            magEnabled: %d", c.data.bno0XX.magEnabled);
-
-					break;
-                }
-            }
-        }
-    }
 }
+
+void Configuration::save() {
+	for (size_t i = 0; i < m_Sensors.size(); i++) {
+		SensorConfig config = m_Sensors[i];
+		if (config.type == SensorConfigType::NONE) {
+			continue;
+		}
+
+		char path[17];
+		sprintf(path, DIR_CALIBRATIONS "/%zu", i);
+
+		m_Logger.trace("Saving sensor config data for %d", i);
+
+		File file = LittleFS.open(path, "w");
+		file.write((uint8_t*)&config, sizeof(SensorConfig));
+		file.close();
+
+		sprintf(path, DIR_TOGGLES "/%zu", i);
+
+		m_Logger.trace("Saving sensor toggle state for %d", i);
+
+		file = LittleFS.open(path, "w");
+		file.write((uint8_t*)&m_SensorToggles[i], sizeof(SensorToggleState));
+		file.close();
+	}
+
+	{
+		File file = LittleFS.open("/config.bin", "w");
+		file.write((uint8_t*)&m_Config, sizeof(DeviceConfig));
+		file.close();
+	}
+
+	m_Logger.debug("Saved configuration");
+}
+
+void Configuration::reset() {
+	LittleFS.format();
+
+	m_Sensors.clear();
+	m_SensorToggles.clear();
+	m_Config.version = 1;
+	save();
+
+	m_Logger.debug("Reset configuration");
+}
+
+int32_t Configuration::getVersion() const { return m_Config.version; }
+
+size_t Configuration::getSensorCount() const { return m_Sensors.size(); }
+
+SensorConfig Configuration::getSensor(size_t sensorID) const {
+	if (sensorID >= m_Sensors.size()) {
+		return {};
+	}
+
+	return m_Sensors.at(sensorID);
+}
+
+void Configuration::setSensor(size_t sensorID, const SensorConfig& config) {
+	size_t currentSensors = m_Sensors.size();
+
+	if (sensorID >= currentSensors) {
+		m_Sensors.resize(sensorID + 1);
+	}
+
+	m_Sensors[sensorID] = config;
+}
+
+SensorToggleState Configuration::getSensorToggles(size_t sensorId) const {
+	if (sensorId >= m_SensorToggles.size()) {
+		return {};
+	}
+
+	return m_SensorToggles.at(sensorId);
+}
+
+void Configuration::setSensorToggles(size_t sensorId, SensorToggleState state) {
+	size_t currentSensors = m_SensorToggles.size();
+
+	if (sensorId >= currentSensors) {
+		m_SensorToggles.resize(sensorId + 1);
+	}
+
+	m_SensorToggles[sensorId] = state;
+}
+
+void Configuration::eraseSensors() {
+	m_Sensors.clear();
+
+	SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
+		char path[17];
+		sprintf(path, DIR_CALIBRATIONS "/%s", f.name());
+
+		f.close();
+
+		LittleFS.remove(path);
+	});
+
+	save();
+}
+
+void Configuration::loadSensors() {
+	SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
+		SensorConfig sensorConfig;
+		f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig));
+
+		uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+		m_Logger.debug(
+			"Found sensor calibration for %s at index %d",
+			calibrationConfigTypeToString(sensorConfig.type),
+			sensorId
+		);
+
+		if (sensorConfig.type == SensorConfigType::BNO0XX) {
+			SensorToggleState toggles;
+			toggles.setToggle(
+				SensorToggles::MagEnabled,
+				sensorConfig.data.bno0XX.magEnabled
+			);
+			setSensorToggles(sensorId, toggles);
+		}
+
+		setSensor(sensorId, sensorConfig);
+	});
+
+	SlimeVR::Utils::forEachFile(DIR_TOGGLES, [&](SlimeVR::Utils::File f) {
+		SensorToggleState sensorToggleState;
+		f.read((uint8_t*)&sensorToggleState, sizeof(SensorToggleState));
+
+		uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+		m_Logger.debug("Found sensor toggle state at index %d", sensorId);
+
+		setSensorToggles(sensorId, sensorToggleState);
+	});
+}
+
+bool Configuration::loadTemperatureCalibration(
+	uint8_t sensorId,
+	GyroTemperatureCalibrationConfig& config
+) {
+	if (!SlimeVR::Utils::ensureDirectory(DIR_TEMPERATURE_CALIBRATIONS)) {
+		return false;
+	}
+
+	char path[32];
+	sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
+
+	if (!LittleFS.exists(path)) {
+		return false;
+	}
+
+	auto f = SlimeVR::Utils::openFile(path, "r");
+	if (f.isDirectory()) {
+		return false;
+	}
+
+	if (f.size() != sizeof(GyroTemperatureCalibrationConfig)) {
+		m_Logger.debug(
+			"Found incompatible sensor temperature calibration (size mismatch) "
+			"sensorId:%d, skipping",
+			sensorId
+		);
+		return false;
+	}
+
+	SensorConfigType storedConfigType;
+	f.read((uint8_t*)&storedConfigType, sizeof(SensorConfigType));
+
+	if (storedConfigType != config.type) {
+		m_Logger.debug(
+			"Found incompatible sensor temperature calibration (expected %s, "
+			"found %s) sensorId:%d, skipping",
+			calibrationConfigTypeToString(config.type),
+			calibrationConfigTypeToString(storedConfigType),
+			sensorId
+		);
+		return false;
+	}
+
+	f.seek(0);
+	f.read((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
+	m_Logger.debug(
+		"Found sensor temperature calibration for %s sensorId:%d",
+		calibrationConfigTypeToString(config.type),
+		sensorId
+	);
+	return true;
+}
+
+bool Configuration::saveTemperatureCalibration(
+	uint8_t sensorId,
+	const GyroTemperatureCalibrationConfig& config
+) {
+	if (config.type == SensorConfigType::NONE) {
+		return false;
+	}
+
+	char path[32];
+	sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
+
+	m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId);
+
+	File file = LittleFS.open(path, "w");
+	file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
+	file.close();
+
+	m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId);
+	return true;
+}
+
+bool Configuration::runMigrations(int32_t version) { return true; }
+
+void Configuration::print() {
+	m_Logger.info("Configuration:");
+	m_Logger.info("  Version: %d", m_Config.version);
+	m_Logger.info("  %d Sensors:", m_Sensors.size());
+
+	for (size_t i = 0; i < m_Sensors.size(); i++) {
+		const SensorConfig& c = m_Sensors[i];
+		m_Logger.info("    - [%3d] %s", i, calibrationConfigTypeToString(c.type));
+
+		switch (c.type) {
+			case SensorConfigType::NONE:
+				break;
+
+			case SensorConfigType::BMI160:
+				m_Logger.info(
+					"            A_B        : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B)
+				);
+
+				m_Logger.info("            A_Ainv     :");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                         %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.bmi160.A_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            G_off      : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.bmi160.G_off)
+				);
+				m_Logger.info("            Temperature: %f", c.data.bmi160.temperature);
+
+				break;
+
+			case SensorConfigType::SFUSION:
+				m_Logger.info(
+					"            A_B        : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B)
+				);
+
+				m_Logger.info("            A_Ainv     :");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                         %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.sfusion.A_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            G_off      : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.sfusion.G_off)
+				);
+				m_Logger.info(
+					"            Temperature: %f",
+					c.data.sfusion.temperature
+				);
+				break;
+
+			case SensorConfigType::ICM20948:
+				m_Logger.info(
+					"            G: %d, %d, %d",
+					UNPACK_VECTOR_ARRAY(c.data.icm20948.G)
+				);
+				m_Logger.info(
+					"            A: %d, %d, %d",
+					UNPACK_VECTOR_ARRAY(c.data.icm20948.A)
+				);
+				m_Logger.info(
+					"            C: %d, %d, %d",
+					UNPACK_VECTOR_ARRAY(c.data.icm20948.C)
+				);
+
+				break;
+
+			case SensorConfigType::MPU9250:
+				m_Logger.info(
+					"            A_B   : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B)
+				);
+
+				m_Logger.info("            A_Ainv:");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                    %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            M_B   : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B)
+				);
+
+				m_Logger.info("            M_Ainv:");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                    %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            G_off  : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off)
+				);
+
+				break;
+
+			case SensorConfigType::MPU6050:
+				m_Logger.info(
+					"            A_B  : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)
+				);
+				m_Logger.info(
+					"            G_off: %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)
+				);
+
+				break;
+
+			case SensorConfigType::BNO0XX:
+				m_Logger.info("            magEnabled: %d", c.data.bno0XX.magEnabled);
+
+				break;
+		}
+	}
+}
+}  // namespace SlimeVR::Configuration

src/configuration/Configuration.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    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:
+	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 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.
+	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.
 */
 
 #ifndef SLIMEVR_CONFIGURATION_CONFIGURATION_H
@@ -26,42 +26,51 @@
 
 #include <vector>
 
+#include "../motionprocessing/GyroTemperatureCalibrator.h"
+#include "../sensors/SensorToggles.h"
 #include "DeviceConfig.h"
 #include "logging/Logger.h"
-#include "../motionprocessing/GyroTemperatureCalibrator.h"
 
-namespace SlimeVR {
-    namespace Configuration {
-        class Configuration {
-        public:
-            void setup();
+namespace SlimeVR::Configuration {
+class Configuration {
+public:
+	void setup();
 
-            void save();
-            void reset();
+	void save();
+	void reset();
 
-            void print();
+	void print();
 
-            int32_t getVersion() const;
+	int32_t getVersion() const;
 
-            size_t getSensorCount() const;
-            SensorConfig getSensor(size_t sensorID) const;
-            void setSensor(size_t sensorID, const SensorConfig& config);
+	size_t getSensorCount() const;
+	SensorConfig getSensor(size_t sensorID) const;
+	void setSensor(size_t sensorID, const SensorConfig& config);
+	SensorToggleState getSensorToggles(size_t sensorId) const;
+	void setSensorToggles(size_t sensorId, SensorToggleState state);
+	void eraseSensors();
 
-            bool loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config);
-            bool saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config);
+	bool loadTemperatureCalibration(
+		uint8_t sensorId,
+		GyroTemperatureCalibrationConfig& config
+	);
+	bool saveTemperatureCalibration(
+		uint8_t sensorId,
+		const GyroTemperatureCalibrationConfig& config
+	);
 
-        private:
-            void loadSensors();
-            bool runMigrations(int32_t version);
+private:
+	void loadSensors();
+	bool runMigrations(int32_t version);
 
-            bool m_Loaded = false;
+	bool m_Loaded = false;
 
-            DeviceConfig m_Config{};
-            std::vector<SensorConfig> m_Sensors;
+	DeviceConfig m_Config{};
+	std::vector<SensorConfig> m_Sensors;
+	std::vector<SensorToggleState> m_SensorToggles;
 
-            Logging::Logger m_Logger = Logging::Logger("Configuration");
-        };
-    }
-}
+	Logging::Logger m_Logger = Logging::Logger("Configuration");
+};
+}  // namespace SlimeVR::Configuration
 
 #endif

src/configuration/SensorConfig.cpp

@@ -23,8 +23,7 @@
 
 #include "SensorConfig.h"
 
-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 const char* calibrationConfigTypeToString(SensorConfigType type) {
 	switch (type) {
 		case SensorConfigType::NONE:
@@ -41,27 +40,23 @@ const char* calibrationConfigTypeToString(SensorConfigType type) {
 			return "SoftFusion (common)";
 		case SensorConfigType::BNO0XX:
 			return "BNO0XX";
+		case SensorConfigType::RUNTIME_CALIBRATION:
+			return "SoftFusion (runtime calibration)";
 		default:
 			return "UNKNOWN";
 	}
 }
 
-// 1st bit specifies if magnetometer is enabled or disabled
-// 2nd bit specifies if magnetometer is supported
-uint16_t configDataToNumber(SensorConfig* sensorConfig, bool magSupported) {
-	uint16_t data = 0;
-	data += magSupported << 1;
-	switch (sensorConfig->type) {
-		case SensorConfigType::BNO0XX: {
-			auto config = &sensorConfig->data.bno0XX;
-			data += config->magEnabled;
-			break;
-		}
-		case SensorConfigType::NONE:
-		default:
-			break;
-	}
-	return data;
+bool SensorConfigBits::operator==(const SensorConfigBits& rhs) const {
+	return magEnabled == rhs.magEnabled && magSupported == rhs.magSupported
+		&& calibrationEnabled == rhs.calibrationEnabled
+		&& calibrationSupported == rhs.calibrationSupported
+		&& tempGradientCalibrationEnabled == rhs.tempGradientCalibrationEnabled
+		&& tempGradientCalibrationSupported == rhs.tempGradientCalibrationSupported;
 }
-}  // namespace Configuration
-}  // namespace SlimeVR
+
+bool SensorConfigBits::operator!=(const SensorConfigBits& rhs) const {
+	return !(*this == rhs);
+}
+
+}  // namespace SlimeVR::Configuration

src/configuration/SensorConfig.h

@@ -28,8 +28,7 @@
 
 #include "consts.h"
 
-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 struct BMI160SensorConfig {
 	// accelerometer offsets and correction matrix
 	float A_B[3];
@@ -47,7 +46,7 @@ struct BMI160SensorConfig {
 };
 
 struct SoftFusionSensorConfig {
-	ImuID ImuType;
+	SensorTypeID ImuType;
 	uint16_t MotionlessDataLen;
 
 	// accelerometer offsets and correction matrix
@@ -73,6 +72,32 @@ struct SoftFusionSensorConfig {
 	float G_Sens[3];
 
 	uint8_t MotionlessData[60];
+
+	// temperature sampling rate (placed at the end to not break existing configs)
+	float T_Ts;
+};
+
+struct RuntimeCalibrationSensorConfig {
+	SensorTypeID ImuType;
+	uint16_t MotionlessDataLen;
+
+	bool sensorTimestepsCalibrated;
+	float A_Ts;
+	float G_Ts;
+	float M_Ts;
+	float T_Ts;
+
+	bool motionlessCalibrated;
+	uint8_t MotionlessData[60];
+
+	uint8_t gyroPointsCalibrated;
+	float gyroMeasurementTemperature1;
+	float G_off1[3];
+	float gyroMeasurementTemperature2;
+	float G_off2[3];
+
+	bool accelCalibrated[3];
+	float A_off[3];
 };
 
 struct MPU6050SensorConfig {
@@ -131,7 +156,8 @@ enum class SensorConfigType {
 	MPU9250,
 	ICM20948,
 	SFUSION,
-	BNO0XX
+	BNO0XX,
+	RUNTIME_CALIBRATION,
 };
 
 const char* calibrationConfigTypeToString(SensorConfigType type);
@@ -146,11 +172,29 @@ struct SensorConfig {
 		MPU9250SensorConfig mpu9250;
 		ICM20948SensorConfig icm20948;
 		BNO0XXSensorConfig bno0XX;
+		RuntimeCalibrationSensorConfig runtimeCalibration;
 	} data;
 };
 
-uint16_t configDataToNumber(SensorConfig* sensorConfig, bool magSupported);
-}  // namespace Configuration
-}  // namespace SlimeVR
+struct SensorConfigBits {
+	bool magEnabled : 1;
+	bool magSupported : 1;
+	bool calibrationEnabled : 1;
+	bool calibrationSupported : 1;
+	bool tempGradientCalibrationEnabled : 1;
+	bool tempGradientCalibrationSupported : 1;
+
+	// Remove if the above fields exceed a byte, necessary to make the struct 16
+	// bit
+	uint8_t padding;
+
+	bool operator==(const SensorConfigBits& rhs) const;
+	bool operator!=(const SensorConfigBits& rhs) const;
+};
+
+// If this fails, you forgot to do the above
+static_assert(sizeof(SensorConfigBits) == 2);
+
+}  // namespace SlimeVR::Configuration
 
 #endif

src/consts.h

@@ -1,50 +1,55 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 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:
+	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 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.
+	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.
 */
 #ifndef SLIMEVR_CONSTS_H_
 #define SLIMEVR_CONSTS_H_
 
 // List of constants used in other places
 
-enum class ImuID {
-    Unknown = 0,
-    MPU9250,
-    MPU6500,
-    BNO080,
-    BNO085,
-    BNO055,
-    MPU6050,
-    BNO086,
-    BMI160,
-    ICM20948,
-    ICM42688,
-    BMI270,
-    LSM6DS3TRC,
-    LSM6DSV,
-    LSM6DSO,
-    LSM6DSR,
-    Empty = 255
+#include <cstdint>
+enum class SensorTypeID : uint8_t {
+	Unknown = 0,
+	MPU9250,
+	MPU6500,
+	BNO080,
+	BNO085,
+	BNO055,
+	MPU6050,
+	BNO086,
+	BMI160,
+	ICM20948,
+	ICM42688,
+	BMI270,
+	LSM6DS3TRC,
+	LSM6DSV,
+	LSM6DSO,
+	LSM6DSR,
+	ICM45686,
+	ICM45605,
+	ADC_RESISTANCE,
+	Empty = 255
 };
 
+#define IMU_AUTO SensorAuto
 #define IMU_UNKNOWN ErroneousSensor
 #define IMU_MPU9250 MPU9250Sensor
 #define IMU_MPU6500 MPU6050Sensor
@@ -53,7 +58,7 @@ enum class ImuID {
 #define IMU_BNO055 BNO055Sensor
 #define IMU_MPU6050 MPU6050Sensor
 #define IMU_BNO086 BNO086Sensor
-#define IMU_BMI160 BMI160Sensor
+#define IMU_BMI160 SoftFusionBMI160
 #define IMU_ICM20948 ICM20948Sensor
 #define IMU_ICM42688 SoftFusionICM42688
 #define IMU_BMI270 SoftFusionBMI270
@@ -62,29 +67,36 @@ enum class ImuID {
 #define IMU_LSM6DSO SoftFusionLSM6DSO
 #define IMU_LSM6DSR SoftFusionLSM6DSR
 #define IMU_MPU6050_SF SoftFusionMPU6050
+#define IMU_ICM45686 SoftFusionICM45686
+#define IMU_ICM45605 SoftFusionICM45605
 
-#define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+#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_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_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_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+#define BOARD_HARITORA 18  // Used by Haritora/SlimeTora
+#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_ESP32S3_SUPERMINI 23
+#define BOARD_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
 
 #define BAT_EXTERNAL 1
 #define BAT_INTERNAL 2
@@ -93,25 +105,28 @@ enum class ImuID {
 
 #define LED_OFF 255
 
-#define POWER_SAVING_LEGACY 0 // No sleeping, but PS enabled
-#define POWER_SAVING_NONE 1 // No sleeping, no PS => for connection issues
-#define POWER_SAVING_MINIMUM 2 // Sleeping and PS => default
-#define POWER_SAVING_MODERATE 3 // Sleeping and better PS => might miss broadcasts, use at own risk
-#define POWER_SAVING_MAXIMUM 4 // Actual CPU sleeping, currently has issues with disconnecting
+#define POWER_SAVING_LEGACY 0  // No sleeping, but PS enabled
+#define POWER_SAVING_NONE 1  // No sleeping, no PS => for connection issues
+#define POWER_SAVING_MINIMUM 2  // Sleeping and PS => default
+#define POWER_SAVING_MODERATE \
+	3  // Sleeping and better PS => might miss broadcasts, use at own risk
+#define POWER_SAVING_MAXIMUM \
+	4  // Actual CPU sleeping, currently has issues with disconnecting
 
 // Send rotation/acceleration data as separate frames.
 // PPS: 1470 @ 5+1, 1960 @ 5+3
 #define PACKET_BUNDLING_DISABLED 0
 // Less packets. Pack data per sensor and send asap.
-// Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3 setups.
-// PPS: 680 @ 5+1, 740 @ 5+3
+// Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3
+// setups. PPS: 680 @ 5+1, 740 @ 5+3
 #define PACKET_BUNDLING_LOWLATENCY 1
-// Even less packets, if more than 1 sensor - wait for data from all sensors or until timeout, then send.
-// Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for 5+1, by ~15% for 5+3 setups.
-// PPS: 650 @ 5+1, 650 @ 5+3
+// Even less packets, if more than 1 sensor - wait for data from all sensors or until
+// timeout, then send. Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for
+// 5+1, by ~15% for 5+3 setups. PPS: 650 @ 5+1, 650 @ 5+3
 #define PACKET_BUNDLING_BUFFERED 2
 
-// Get radian for a given angle from 0° to 360° (2*PI*r, solve for r given an angle, range -180° to 180°)
+// Get radian for a given angle from 0° to 360° (2*PI*r, solve for r given an angle,
+// range -180° to 180°)
 #define DEG_X(deg) ((((deg) < 180.0f ? 0 : 360.0f) - (deg)) * PI / 180.0f)
 
 #define DEG_0 DEG_X(0.0f)
@@ -131,22 +146,36 @@ enum class ImuID {
 #define MCU_UNKNOWN 0
 #define MCU_ESP8266 1
 #define MCU_ESP32 2
-#define MCU_OWOTRACK_ANDROID 3 // Only used by owoTrack mobile app
-#define MCU_WRANGLER 4 // Only used by wrangler app
-#define MCU_OWOTRACK_IOS 5 // Only used by owoTrack mobile app
+#define MCU_OWOTRACK_ANDROID 3  // Only used by owoTrack mobile app
+#define MCU_WRANGLER 4  // Only used by wrangler app
+#define MCU_OWOTRACK_IOS 5  // Only used by owoTrack mobile app
 #define MCU_ESP32_C3 6
-#define MCU_MOCOPI 7 // Used by mocopi/moslime
-#define MCU_HARITORA 8 // Used by Haritora/SlimeTora
-#define MCU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+#define MCU_MOCOPI 7  // Used by mocopi/moslime
+#define MCU_HARITORA 8  // Used by Haritora/SlimeTora
+#define MCU_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
+
+enum class SensorDataType : uint8_t {
+	SENSOR_DATATYPE_ROTATION = 0,
+	SENSOR_DATATYPE_FLEX_RESISTANCE,
+	SENSOR_DATATYPE_FLEX_ANGLE
+};
+
+enum class TrackerType : uint8_t {
+	TRACKER_TYPE_SVR_ROTATION = 0,
+	TRACKER_TYPE_SVR_GLOVE_LEFT,
+	TRACKER_TYPE_SVR_GLOVE_RIGHT
+};
 
 #ifdef ESP8266
-  #define HARDWARE_MCU MCU_ESP8266
+#define HARDWARE_MCU MCU_ESP8266
 #elif defined(ESP32)
-  #define HARDWARE_MCU MCU_ESP32
+#define HARDWARE_MCU MCU_ESP32
 #else
-  #define HARDWARE_MCU MCU_UNKNOWN
+#define HARDWARE_MCU MCU_UNKNOWN
 #endif
 
 #define CURRENT_CONFIGURATION_VERSION 1
 
-#endif // SLIMEVR_CONSTS_H_
+#include "sensors/sensorposition.h"
+
+#endif  // SLIMEVR_CONSTS_H_

src/credentials.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    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:
+	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 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.
+	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.
 */
 #ifndef SLIMEVR_CREDENTIALS_H_
 #define SLIMEVR_CREDENTIALS_H_
@@ -30,6 +30,7 @@
 // firmware. We don't have any hardware buttons for the user to confirm
 // OTA update, so this is the best way we have.
 // OTA is allowed only for the first 60 seconds after device startup.
-const char *otaPassword = "SlimeVR-OTA"; // YOUR OTA PASSWORD HERE, LEAVE EMPTY TO DISABLE OTA UPDATES
+const char* otaPassword
+	= "SlimeVR-OTA";  // YOUR OTA PASSWORD HERE, LEAVE EMPTY TO DISABLE OTA UPDATES
 
-#endif // SLIMEVR_CREDENTIALS_H_
+#endif  // SLIMEVR_CREDENTIALS_H_

src/debug.h

@@ -1,50 +1,55 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    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:
+	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 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.
+	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.
 */
 #ifndef SLIMEVR_DEBUG_H_
 #define SLIMEVR_DEBUG_H_
 #include "consts.h"
 #include "logging/Level.h"
 
-#define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration
-#define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs
-#define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently)
-#define LOAD_BIAS true // Loads the bias values from NVS on start
-#define SAVE_BIAS true // Periodically saves bias calibration data to NVS
-#define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only)
-#define ENABLE_TAP false // monitor accel for (triple) tap events and send them. Uses more cpu, disable if problems. Server does nothing with value so disabled atm
-#define SEND_ACCELERATION true // send linear acceleration to the server
+#define IMU_MPU6050_RUNTIME_CALIBRATION  // Comment to revert to
+										 // startup/traditional-calibration
+#define BNO_USE_ARVR_STABILIZATION \
+	true  // Set to false to disable stabilization for BNO085+ IMUs
+#define USE_6_AXIS \
+	true  // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently)
+#define LOAD_BIAS true  // Loads the bias values from NVS on start
+#define SAVE_BIAS true  // Periodically saves bias calibration data to NVS
+#define BIAS_DEBUG false  // Printing BIAS Variables to serial (ICM20948 only)
+#define ENABLE_TAP \
+	false  // monitor accel for (triple) tap events and send them. Uses more cpu,
+		   // disable if problems. Server does nothing with value so disabled atm
+#define SEND_ACCELERATION true  // send linear acceleration to the server
 
-//Debug information
+// Debug information
 
 #define LOG_LEVEL LOG_LEVEL_DEBUG
 
 #if LOG_LEVEL == LOG_LEVEL_TRACE
-    #define DEBUG_SENSOR
-    #define DEBUG_NETWORK
-    #define DEBUG_CONFIGURATION
+#define DEBUG_SENSOR
+#define DEBUG_NETWORK
+#define DEBUG_CONFIGURATION
 #endif
 
-#define serialDebug false // Set to true to get Serial output for debugging
+#define serialDebug false  // Set to true to get Serial output for debugging
 #define serialBaudRate 115200
 #define LED_INTERVAL_STANDBY 10000
 #define PRINT_STATE_EVERY_MS 60000
@@ -55,7 +60,7 @@
 // Sleeping options
 #define POWERSAVING_MODE POWER_SAVING_LEGACY  // Minimum causes sporadic data pauses
 #if POWERSAVING_MODE >= POWER_SAVING_MINIMUM
-    #define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000)
+#define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000)
 #endif
 
 // Packet bundling/aggregation
@@ -69,7 +74,7 @@
 // Battery configuration
 #define batterySampleRate 10000
 #define BATTERY_LOW_VOLTAGE_DEEP_SLEEP false
-#define BATTERY_LOW_POWER_VOLTAGE 3.3f // Voltage to raise error
+#define BATTERY_LOW_POWER_VOLTAGE 3.3f  // Voltage to raise error
 
 // Send updates over network only when changes are substantial
 // If "false" updates are sent at the sensor update rate (usually 100 TPS)
@@ -80,7 +85,7 @@
 #define I2C_SPEED 400000
 
 #define COMPLIANCE_MODE true
-#define USE_ATTENUATION COMPLIANCE_MODE && ESP8266
+#define USE_ATTENUATION COMPLIANCE_MODE&& ESP8266
 #define ATTENUATION_N 10.0 / 4.0
 #define ATTENUATION_G 14.0 / 4.0
 #define ATTENUATION_B 40.0 / 4.0
@@ -89,7 +94,24 @@
 // Not recommended for production
 #define ENABLE_INSPECTION false
 
-#define PROTOCOL_VERSION 18
-#define FIRMWARE_VERSION "0.5.0"
+#define PROTOCOL_VERSION 21
 
-#endif // SLIMEVR_DEBUG_H_
+#ifndef FIRMWARE_VERSION
+#define FIRMWARE_VERSION "UNKNOWN"
+#endif
+
+#ifndef USE_RUNTIME_CALIBRATION
+#define USE_RUNTIME_CALIBRATION true
+#endif
+
+#define DEBUG_MEASURE_SENSOR_TIME_TAKEN false
+
+#ifndef DEBUG_MEASURE_SENSOR_TIME_TAKEN
+#define DEBUG_MEASURE_SENSOR_TIME_TAKEN false
+#endif
+
+#ifndef USE_OTA_TIMEOUT
+#define USE_OTA_TIMEOUT false
+#endif
+
+#endif  // SLIMEVR_DEBUG_H_

src/debugging/TimeTaken.cpp

@@ -0,0 +1,55 @@
+/*
+	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 "TimeTaken.h"
+
+namespace SlimeVR::Debugging {
+
+TimeTakenMeasurer::TimeTakenMeasurer(const char* name)
+	: name{name} {}
+
+void TimeTakenMeasurer::before() { startMicros = micros(); }
+
+void TimeTakenMeasurer::after() {
+	uint64_t elapsedMicros = micros() - startMicros;
+	timeTakenMicros += elapsedMicros;
+
+	uint64_t sinceLastReportMillis = millis() - lastTimeTakenReportMillis;
+
+	if (sinceLastReportMillis < static_cast<uint64_t>(SecondsBetweenReports * 1e3)) {
+		return;
+	}
+
+	float usedPercentage = static_cast<float>(timeTakenMicros) / 1e3f
+						 / static_cast<float>(sinceLastReportMillis) * 100;
+
+	m_Logger.info(
+		"%s: %.2f%% of the last period taken (%.2f/%lld millis)",
+		name,
+		usedPercentage,
+		timeTakenMicros / 1e3f,
+		sinceLastReportMillis
+	);
+
+	timeTakenMicros = 0;
+	lastTimeTakenReportMillis = millis();
+}
+
+}  // namespace SlimeVR::Debugging

src/debugging/TimeTaken.h

@@ -0,0 +1,58 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+
+#include "logging/Logger.h"
+
+namespace SlimeVR::Debugging {
+
+/*
+ * Usage:
+ *
+ * TimeTakenMeasurer measurer{"Some event"};
+ *
+ * ...
+ *
+ * measurer.before();
+ * thing to measure
+ * measurer.after();
+ */
+class TimeTakenMeasurer {
+public:
+	explicit TimeTakenMeasurer(const char* name);
+	void before();
+	void after();
+
+private:
+	static constexpr float SecondsBetweenReports = 1.0f;
+
+	const char* name;
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("TimeTaken");
+
+	uint64_t lastTimeTakenReportMillis = 0;
+	uint64_t timeTakenMicros = 0;
+
+	uint64_t startMicros = 0;
+};
+
+}  // namespace SlimeVR::Debugging

src/defines.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    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:
+	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 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.
+	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.
 */
 // ================================================
 // See docs for configuration options and examples:
@@ -26,184 +26,53 @@
 // ================================================
 
 // Set parameters of IMU and board used
-#define IMU IMU_BNO085
-#define SECOND_IMU IMU
-#define BOARD BOARD_SLIMEVR
+#ifndef IMU
+#define IMU IMU_AUTO
+#endif
+#ifndef SECOND_IMU
+#define SECOND_IMU IMU_AUTO
+#endif
+#ifndef BOARD
+#define BOARD BOARD_SLIMEVR_V1_2
+#endif
+#ifndef IMU_ROTATION
 #define IMU_ROTATION DEG_270
+#endif
+#ifndef SECOND_IMU_ROTATION
 #define SECOND_IMU_ROTATION DEG_270
-
-#define PRIMARY_IMU_OPTIONAL false
-#define SECONDARY_IMU_OPTIONAL true
-
-#define MAX_IMU_COUNT 2
-
-// Axis mapping example
-/*
-#include "sensors/axisremap.h"
-#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN, AXIS_REMAP_USE_Z, \
-                                       AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)
-
-IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
-*/
-
-#ifndef IMU_DESC_LIST
-#define IMU_DESC_LIST \
-        IMU_DESC_ENTRY(IMU,        PRIMARY_IMU_ADDRESS_ONE,   IMU_ROTATION,        PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL,   PIN_IMU_INT) \
-        IMU_DESC_ENTRY(SECOND_IMU, SECONDARY_IMU_ADDRESS_TWO, SECOND_IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, SECONDARY_IMU_OPTIONAL, PIN_IMU_INT_2)
 #endif
 
+#ifndef PRIMARY_IMU_OPTIONAL
+#define PRIMARY_IMU_OPTIONAL false
+#endif
+#ifndef SECONDARY_IMU_OPTIONAL
+#define SECONDARY_IMU_OPTIONAL true
+#endif
+
+// Set I2C address here or directly in IMU_DESC_ENTRY for each IMU used
+// If not set, default address is used based on the IMU and Sensor ID
+// #define PRIMARY_IMU_ADDRESS_ONE 0x4a
+// #define SECONDARY_IMU_ADDRESS_TWO 0x4b
+
+#ifndef BATTERY_MONITOR
 // Battery monitoring options (comment to disable):
 //   BAT_EXTERNAL for ADC pin,
 //   BAT_INTERNAL for internal - can detect only low battery,
 //   BAT_MCP3021 for external ADC connected over I2C
 #define BATTERY_MONITOR BAT_EXTERNAL
-
-// BAT_EXTERNAL definition override
-// D1 Mini boards with ESP8266 have internal resistors. For these boards you only have to adjust BATTERY_SHIELD_RESISTANCE.
-// For other boards you can now adjust the other resistor values.
-// The diagram looks like this:
-//   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2] ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
-// #define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in external resistor value in kOhm
-// #define BATTERY_SHIELD_R1 100 // Board voltage divider resistor Ain to GND in kOhm
-// #define BATTERY_SHIELD_R2 220 // Board voltage divider resistor Ain to INPUT_BOARD in kOhm
-
-// LED configuration:
-// Configuration Priority 1 = Highest:
-// 1. LED_PIN
-// 2. LED_BUILTIN
-//
-//   LED_PIN
-//     - Number or Symbol (D1,..) of the Output
-//     - To turn off the LED, set LED_PIN to LED_OFF
-//   LED_INVERTED
-//     - false for output 3.3V on high
-//     - true for pull down to GND on high
-
-// Board-specific configurations
-#if BOARD == BOARD_SLIMEVR
-  #define PIN_IMU_SDA 14
-  #define PIN_IMU_SCL 12
-  #define PIN_IMU_INT 16
-  #define PIN_IMU_INT_2 13
-  #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
-  #define PIN_IMU_INT 10
-  #define PIN_IMU_INT_2 13
-  #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_NODEMCU || BOARD == BOARD_WEMOSD1MINI
-  #define PIN_IMU_SDA D2
-  #define PIN_IMU_SCL D1
-  #define PIN_IMU_INT D5
-  #define PIN_IMU_INT_2 D6
-  #define PIN_BATTERY_LEVEL A0
-//  #define LED_PIN 2
-//  #define LED_INVERTED true
-  #ifndef BATTERY_SHIELD_RESISTANCE
-    #define BATTERY_SHIELD_RESISTANCE 180
-  #endif
-  #ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 100
-  #endif
-  #ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 220
-  #endif
-#elif BOARD == BOARD_ESP01
-  #define PIN_IMU_SDA 2
-  #define PIN_IMU_SCL 0
-  #define PIN_IMU_INT 255
-  #define PIN_IMU_INT_2 255
-  #define PIN_BATTERY_LEVEL 255
-  #define LED_PIN LED_OFF
-  #define LED_INVERTED false
-#elif BOARD == BOARD_TTGO_TBASE
-  #define PIN_IMU_SDA 5
-  #define PIN_IMU_SCL 4
-  #define PIN_IMU_INT 14
-  #define PIN_IMU_INT_2 13
-  #define PIN_BATTERY_LEVEL A0
-//  #define LED_PIN 2
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_CUSTOM
-  // Define pins by the examples above
-#elif BOARD == BOARD_WROOM32
-  #define PIN_IMU_SDA 21
-  #define PIN_IMU_SCL 22
-  #define PIN_IMU_INT 23
-  #define PIN_IMU_INT_2 25
-  #define PIN_BATTERY_LEVEL 36
-//  #define LED_PIN 2
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_LOLIN_C3_MINI
-  #define PIN_IMU_SDA 5
-  #define PIN_IMU_SCL 4
-  #define PIN_IMU_INT 6
-  #define PIN_IMU_INT_2 8
-  #define PIN_BATTERY_LEVEL 3
-  #define LED_PIN 7
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_BEETLE32C3
-  #define PIN_IMU_SDA 8
-  #define PIN_IMU_SCL 9
-  #define PIN_IMU_INT 6
-  #define PIN_IMU_INT_2 7
-  #define PIN_BATTERY_LEVEL 3
-  #define LED_PIN 10
-  #define LED_INVERTED false
-#elif BOARD == BOARD_ES32C3DEVKITM1
-  #define PIN_IMU_SDA 5
-  #define PIN_IMU_SCL 4
-  #define PIN_IMU_INT 6
-  #define PIN_IMU_INT_2 7
-  #define PIN_BATTERY_LEVEL 3
-  #define LED_PIN LED_OFF  // RGB LED Protocol would need to be implementetet did not brother for the test, because the board ideal for tracker ifself
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_WEMOSWROOM02
-  #define PIN_IMU_SDA 2
-  #define PIN_IMU_SCL 14
-  #define PIN_IMU_INT 0
-  #define PIN_IMU_INT_2 4
-  #define PIN_BATTERY_LEVEL A0
-  #define LED_PIN 16
-  #define LED_INVERTED true
-#elif BOARD == BOARD_XIAO_ESP32C3
-  #define PIN_IMU_SDA 6 // D4
-  #define PIN_IMU_SCL 7 // D5
-  #define PIN_IMU_INT 5 // D3
-  #define PIN_IMU_INT_2 10 // D10
-  #define LED_PIN 4 // D2
-  #define LED_INVERTED false
-  #define PIN_BATTERY_LEVEL 2 // D0 / A0
-  #ifndef BATTERY_SHIELD_RESISTANCE
-    #define BATTERY_SHIELD_RESISTANCE 0
-  #endif
-  #ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 100
-  #endif
-  #ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 100
-  #endif
 #endif
+
+// --- OVERRIDES FOR DEFAULT PINS
+
+// #define PIN_IMU_SDA 14
+// #define PIN_IMU_SCL 12
+// #define PIN_IMU_INT 16
+// #define PIN_IMU_INT_2 13
+// #define PIN_BATTERY_LEVEL 17
+// #define LED_PIN 2
+// #define LED_INVERTED true
+// #define BATTERY_SHIELD_RESISTANCE 0
+// #define BATTERY_SHIELD_R1 10
+// #define BATTERY_SHIELD_R2 40.2
+
+// ------------------------------

src/defines_bmi160.h

@@ -1,76 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 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.
-*/
-#ifndef BMI160_DEFINES_H
-#define BMI160_DEFINES_H
-
-// BMI160 magnetometer type, applies to both main and aux trackers, mixed types are not supported currently.
-// If only 1 out of 2 trackers has a mag, tracker without a mag should still function normally.
-// NOT USED if USE_6_AXIS == true
-// Pick one:
-#define BMI160_MAG_TYPE BMI160_MAG_TYPE_HMC
-// #define BMI160_MAG_TYPE BMI160_MAG_TYPE_QMC
-
-// Use VQF instead of mahony sensor fusion.
-// Features: rest bias estimation, magnetic distortion rejection.
-#define BMI160_USE_VQF true
-
-// Use BasicVQF instead of VQF (if BMI160_USE_VQF == true).
-// Disables the features above.
-#define BMI160_USE_BASIC_VQF false
-
-// Use temperature calibration.
-#define BMI160_USE_TEMPCAL true
-
-// How long to run gyro calibration for.
-// Disables this calibration step if value is 0.
-// Default: 5
-#define BMI160_CALIBRATION_GYRO_SECONDS 5
-
-// Calibration method options:
-// - Skip: disable this calibration step;
-// - Rotation: rotate the device in hand;
-// - 6 point: put the device in 6 unique orientations.
-// Default: ACCEL_CALIBRATION_METHOD_6POINT
-// #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_SKIP
-// #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_ROTATION
-#define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_6POINT
-
-// How long to run magnetometer calibration for, if enabled and you have added a magnetometer.
-// Magnetometer not be used until you calibrate it.
-// Disables this calibration step if value is 0.
-// NOT USED if USE_6_AXIS == true
-// Default: 20
-#define BMI160_CALIBRATION_MAG_SECONDS 20
-
-// Send temperature to the server as AXXYY,
-// where XX is calibration progress from 0 to 60, and YY is temperature,
-// A is 1: not in calibration mode or 2: calibration in progress.
-#define BMI160_TEMPCAL_DEBUG false
-
-// Print debug info every second.
-#define BMI160_DEBUG false
-
-// Use sensitivity calibration.
-#define BMI160_USE_SENSCAL true
-
-#endif

src/defines_sensitivity.h

@@ -1,60 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 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.
-*/
-
-#ifndef DEFINES_SENSITIVITY_H
-#define DEFINES_SENSITIVITY_H
-
-// ==========================
-// Sensitivity calibration
-// Only for: BMI160
-// ==========================
-
-// This type of calibration attempts to reduce "extra degrees measures",
-// useful if you do 360 spins
-
-// Trackers are identified by sensor id and MAC address of the device,
-// which is displayed in serial when you upload firmware
-
-// Number of spins determines calibration accuracy
-
-// 1. Put the axis you want to calibrate vertically
-// 2. Put the tracker into an orientation that you can repeat later
-// 3. Do a full reset, rotation must now be 0 0 0
-// 4. Spin the tracker clockwise <.spins> times around the axis
-// 5. Put the tracker back into the reset position
-// 6. Write down the resulting Y (yaw) value into the table below
-// Reflash, do the same rotation and check if the resulting angle is now 0
-
-#define SENSORID_PRIMARY 0
-#define SENSORID_AUX 1
-struct SensitivityOffsetXYZ { const char* mac; unsigned char sensorId; double spins; double x; double y; double z; };
-const SensitivityOffsetXYZ sensitivityOffsets[] = {
-    // example values
-    { .mac = "A4:E5:7C:B6:00:01", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 },
-    { .mac = "A4:E5:7C:B6:00:02", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 },
-    { .mac = "A4:E5:7C:B6:00:03", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 11, .y =  2.2, .z =  -1 },
-    { .mac = "A4:E5:7C:B6:00:04", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 },
-    { .mac = "A4:E5:7C:B6:00:05", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -10.63, .y = -8.25, .z = -18.6 },
-};
-
-#endif

src/globals.h

@@ -1,34 +1,36 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    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:
+	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 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.
+	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.
 */
-#ifndef SLIMEVR_GLOBALS_H_
-#define SLIMEVR_GLOBALS_H_
+#pragma once
 
 #include <Arduino.h>
+
 #include "consts.h"
 #include "debug.h"
 #include "defines.h"
-#include "defines_bmi160.h"
-#include "defines_sensitivity.h"
+
+// clang-format off
+#include "boards/boards_default.h"
+// clang-format on
 
 #ifndef SECOND_IMU
 #define SECOND_IMU IMU
@@ -42,37 +44,31 @@
 #define BATTERY_MONITOR BAT_INTERNAL
 #endif
 
-// If LED_PIN is not defined in "defines.h" take the default pin from "pins_arduino.h" framework.
-// If there is no pin defined for the board, use LED_PIN 255 and disable LED
-#if defined(LED_PIN)
-    // LED_PIN is defined
-    #if (LED_PIN < 0) || (LED_PIN >= LED_OFF)
-        #define ENABLE_LEDS false    
-    #else
-        #define ENABLE_LEDS true
-    #endif
-#else
-    // LED_PIN is not defined
-    #if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0)
-        #define LED_PIN LED_BUILTIN
-        #define ENABLE_LEDS true
-    #else
-        #define LED_PIN LED_OFF
-        #define ENABLE_LEDS false
-    #endif
+#ifndef SENSOR_INFO_LIST
+#define SENSOR_INFO_LIST
 #endif
 
-#if !defined(LED_INVERTED)
-    // default is inverted for SlimeVR / ESP-12E
-    #define LED_INVERTED true
+// Experimental features
+#ifndef EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION
+#define EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION true
 #endif
 
-#if LED_INVERTED
-#define LED__ON LOW
-#define LED__OFF HIGH
-#else
-#define LED__ON HIGH
-#define LED__OFF LOW
+#ifndef VENDOR_NAME
+#define VENDOR_NAME "Unknown"
 #endif
 
-#endif // SLIMEVR_GLOBALS_H_
+#ifndef VENDOR_URL
+#define VENDOR_URL ""
+#endif
+
+#ifndef PRODUCT_NAME
+#define PRODUCT_NAME "SlimeVR Tracker"
+#endif
+
+#ifndef UPDATE_ADDRESS
+#define UPDATE_ADDRESS ""
+#endif
+
+#ifndef UPDATE_NAME
+#define UPDATE_NAME ""
+#endif

src/logging/Level.cpp

@@ -1,28 +1,22 @@
 #include "Level.h"
 
-namespace SlimeVR
-{
-  namespace Logging
-  {
-    const char *levelToString(Level level)
-    {
-      switch (level)
-      {
-      case TRACE:
-        return "TRACE";
-      case DEBUG:
-        return "DEBUG";
-      case INFO:
-        return "INFO";
-      case WARN:
-        return "WARN";
-      case ERROR:
-        return "ERROR";
-      case FATAL:
-        return "FATAL";
-      default:
-        return "UNKNOWN";
-      }
-    }
-  }
+namespace SlimeVR::Logging {
+const char* levelToString(Level level) {
+	switch (level) {
+		case TRACE:
+			return "TRACE";
+		case DEBUG:
+			return "DEBUG";
+		case INFO:
+			return "INFO";
+		case WARN:
+			return "WARN";
+		case ERROR:
+			return "ERROR";
+		case FATAL:
+			return "FATAL";
+		default:
+			return "UNKNOWN";
+	}
 }
+}  // namespace SlimeVR::Logging

src/logging/Level.h

@@ -7,23 +7,18 @@
 #define LOG_LEVEL_ERROR 4
 #define LOG_LEVEL_FATAL 5
 
-namespace SlimeVR
-{
-  namespace Logging
-  {
-    enum Level
-    {
-      TRACE = LOG_LEVEL_TRACE,
-      DEBUG = LOG_LEVEL_DEBUG,
-      INFO = LOG_LEVEL_INFO,
-      WARN = LOG_LEVEL_WARN,
-      ERROR = LOG_LEVEL_ERROR,
-      FATAL = LOG_LEVEL_FATAL
-    };
+namespace SlimeVR::Logging {
+enum Level {
+	TRACE = LOG_LEVEL_TRACE,
+	DEBUG = LOG_LEVEL_DEBUG,
+	INFO = LOG_LEVEL_INFO,
+	WARN = LOG_LEVEL_WARN,
+	ERROR = LOG_LEVEL_ERROR,
+	FATAL = LOG_LEVEL_FATAL
+};
 
-    const char *levelToString(Level level);
-  }
-}
+const char* levelToString(Level level);
+}  // namespace SlimeVR::Logging
 
 #define LOGGING_LEVEL_H
 #endif

src/logging/Logger.cpp

@@ -1,82 +1,68 @@
 #include "Logger.h"
 
-namespace SlimeVR
-{
-  namespace Logging
-  {
-    void Logger::setTag(const char *tag)
-    {
-      m_Tag = (char *)malloc(strlen(tag) + 1);
-      strcpy(m_Tag, tag);
-    }
-
-    void Logger::trace(const char *format, ...)
-    {
-      va_list args;
-      va_start(args, format);
-      log(TRACE, format, args);
-      va_end(args);
-    }
-
-    void Logger::debug(const char *format, ...)
-    {
-      va_list args;
-      va_start(args, format);
-      log(DEBUG, format, args);
-      va_end(args);
-    }
-
-    void Logger::info(const char *format, ...)
-    {
-      va_list args;
-      va_start(args, format);
-      log(INFO, format, args);
-      va_end(args);
-    }
-
-    void Logger::warn(const char *format, ...)
-    {
-      va_list args;
-      va_start(args, format);
-      log(WARN, format, args);
-      va_end(args);
-    }
-
-    void Logger::error(const char *format, ...)
-    {
-      va_list args;
-      va_start(args, format);
-      log(ERROR, format, args);
-      va_end(args);
-    }
-
-    void Logger::fatal(const char *format, ...)
-    {
-      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)
-    {
-      if (level < LOG_LEVEL)
-      {
-        return;
-      }
-
-      char buffer[256];
-      vsnprintf(buffer, 256, format, args);
-
-      char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2];
-      strcpy(buf, m_Prefix);
-      if (m_Tag != nullptr)
-      {
-        strcat(buf, ":");
-        strcat(buf, m_Tag);
-      }
-
-      Serial.printf("[%-5s] [%s] %s\n", levelToString(level), buf, buffer);
-    }
-  }
+namespace SlimeVR::Logging {
+void Logger::setTag(const char* tag) {
+	m_Tag = (char*)malloc(strlen(tag) + 1);
+	strcpy(m_Tag, tag);
 }
+
+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, ...) const {
+	va_list args;
+	va_start(args, format);
+	log(DEBUG, format, args);
+	va_end(args);
+}
+
+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, ...) const {
+	va_list args;
+	va_start(args, format);
+	log(WARN, format, args);
+	va_end(args);
+}
+
+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, ...) 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) const {
+	if (level < LOG_LEVEL) {
+		return;
+	}
+
+	char buffer[256];
+	vsnprintf(buffer, 256, format, args);
+
+	char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2];
+	strcpy(buf, m_Prefix);
+	if (m_Tag != nullptr) {
+		strcat(buf, ":");
+		strcat(buf, m_Tag);
+	}
+
+	Serial.printf("[%-5s] [%s] %s\n", levelToString(level), buf, buffer);
+}
+}  // namespace SlimeVR::Logging

src/logging/Logger.h

@@ -1,109 +1,96 @@
 #ifndef LOGGING_LOGGER_H
 #define LOGGING_LOGGER_H
 
-#include "Level.h"
-#include "debug.h"
 #include <Arduino.h>
 
-namespace SlimeVR
-{
-  namespace Logging
-  {
-    class Logger
-    {
-    public:
-      Logger(const char *prefix) : m_Prefix(prefix), m_Tag(nullptr){};
-      Logger(const char *prefix, const char *tag) : m_Prefix(prefix), m_Tag(nullptr)
-      {
-        setTag(tag);
-      };
+#include "Level.h"
+#include "debug.h"
 
-      ~Logger()
-      {
-        if (m_Tag != nullptr)
-        {
-          free(m_Tag);
-        }
-      }
+namespace SlimeVR::Logging {
+class Logger {
+public:
+	Logger(const char* prefix)
+		: m_Prefix(prefix)
+		, m_Tag(nullptr){};
+	Logger(const char* prefix, const char* tag)
+		: m_Prefix(prefix)
+		, m_Tag(nullptr) {
+		setTag(tag);
+	};
 
-      void setTag(const char *tag);
+	~Logger() {
+		if (m_Tag != nullptr) {
+			free(m_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 setTag(const char* tag);
 
-      template <typename T>
-      inline void traceArray(const char *str, const T *array, size_t size)
-      {
-        logArray(TRACE, str, array, size);
-      }
+	void trace(const char* str, ...) const __attribute__((format(printf, 2, 3)));
+	void debug(const char* str, ...) const __attribute__((format(printf, 2, 3)));
+	void info(const char* str, ...) const __attribute__((format(printf, 2, 3)));
+	void warn(const char* str, ...) const __attribute__((format(printf, 2, 3)));
+	void error(const char* str, ...) const __attribute__((format(printf, 2, 3)));
+	void fatal(const char* str, ...) const __attribute__((format(printf, 2, 3)));
 
-      template <typename T>
-      inline void debugArray(const char *str, const T *array, size_t size)
-      {
-        logArray(DEBUG, str, array, size);
-      }
+	template <typename T>
+	inline void traceArray(const char* str, const T* array, size_t size) const {
+		logArray(TRACE, str, array, size);
+	}
 
-      template <typename T>
-      inline void infoArray(const char *str, const T *array, size_t size)
-      {
-        logArray(INFO, str, array, size);
-      }
+	template <typename T>
+	inline void debugArray(const char* str, const T* array, size_t size) const {
+		logArray(DEBUG, str, array, size);
+	}
 
-      template <typename T>
-      inline void warnArray(const char *str, const T *array, size_t size)
-      {
-        logArray(WARN, str, array, size);
-      }
+	template <typename T>
+	inline void infoArray(const char* str, const T* array, size_t size) const {
+		logArray(INFO, str, array, size);
+	}
 
-      template <typename T>
-      inline void errorArray(const char *str, const T *array, size_t size)
-      {
-        logArray(ERROR, str, array, size);
-      }
+	template <typename T>
+	inline void warnArray(const char* str, const T* array, size_t size) const {
+		logArray(WARN, str, array, size);
+	}
 
-      template <typename T>
-      inline void fatalArray(const char *str, const T *array, size_t size)
-      {
-        logArray(FATAL, str, array, size);
-      }
+	template <typename T>
+	inline void errorArray(const char* str, const T* array, size_t size) const {
+		logArray(ERROR, str, array, size);
+	}
 
-    private:
-      void log(Level level, const char *str, va_list args);
+	template <typename T>
+	inline void fatalArray(const char* str, const T* array, size_t size) const {
+		logArray(FATAL, str, array, size);
+	}
 
-      template <typename T>
-      void logArray(Level level, const char *str, const T *array, size_t size)
-      {
-        if (level < LOG_LEVEL)
-        {
-          return;
-        }
+private:
+	void log(Level level, const char* str, va_list args) const;
 
-        char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2];
-        strcpy(buf, m_Prefix);
-        if (m_Tag != nullptr)
-        {
-          strcat(buf, ":");
-          strcat(buf, m_Tag);
-        }
+	template <typename T>
+	void logArray(Level level, const char* str, const T* array, size_t size) const {
+		if (level < LOG_LEVEL) {
+			return;
+		}
 
-        Serial.printf("[%-5s] [%s] %s", levelToString(level), buf, str);
+		char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2];
+		strcpy(buf, m_Prefix);
+		if (m_Tag != nullptr) {
+			strcat(buf, ":");
+			strcat(buf, m_Tag);
+		}
 
-        for (size_t i = 0; i < size; i++)
-        {
-          Serial.print(array[i]);
-        }
+		Serial.printf("[%-5s] [%s] %s", levelToString(level), buf, str);
 
-        Serial.println();
-      }
+		for (size_t i = 0; i < size; i++) {
+			Serial.print(array[i]);
+		}
 
-      const char *const m_Prefix;
-      char *m_Tag;
-    };
-  }
-}
+		Serial.println();
+	}
+
+	const char* const m_Prefix;
+	char* m_Tag;
+};
+}  // namespace SlimeVR::Logging
 
 #endif

src/main.cpp

@@ -1,45 +1,52 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 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:
+	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 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.
+	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 "Wire.h"
-#include "ota.h"
-#include "GlobalVars.h"
-#include "globals.h"
-#include "credentials.h"
 #include <i2cscan.h>
-#include "serial/serialcommands.h"
+
+#include "GlobalVars.h"
+#include "Wire.h"
 #include "batterymonitor.h"
+#include "credentials.h"
+#include "debugging/TimeTaken.h"
+#include "globals.h"
 #include "logging/Logger.h"
+#include "ota.h"
+#include "serial/serialcommands.h"
+#include "status/TPSCounter.h"
 
 Timer<> globalTimer;
 SlimeVR::Logging::Logger logger("SlimeVR");
 SlimeVR::Sensors::SensorManager sensorManager;
-SlimeVR::LEDManager ledManager(LED_PIN);
+SlimeVR::LEDManager ledManager;
 SlimeVR::Status::StatusManager statusManager;
 SlimeVR::Configuration::Configuration configuration;
 SlimeVR::Network::Manager networkManager;
 SlimeVR::Network::Connection networkConnection;
 
+#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
+SlimeVR::Debugging::TimeTakenMeasurer sensorMeasurer{"Sensors"};
+#endif
+
 int sensorToCalibrate = -1;
 bool blinking = false;
 unsigned long blinkStart = 0;
@@ -47,100 +54,109 @@ unsigned long loopTime = 0;
 unsigned long lastStatePrint = 0;
 bool secondImuActive = false;
 BatteryMonitor battery;
+TPSCounter tpsCounter;
 
-void setup()
-{
-    Serial.begin(serialBaudRate);
-    globalTimer = timer_create_default();
+void setup() {
+	Serial.begin(serialBaudRate);
+	globalTimer = timer_create_default();
 
-#ifdef ESP32C3
-    // Wait for the Computer to be able to connect.
-    delay(2000);
+	Serial.println();
+	Serial.println();
+	Serial.println();
+
+	logger.info("SlimeVR v" FIRMWARE_VERSION " starting up...");
+
+	statusManager.setStatus(SlimeVR::Status::LOADING, true);
+
+	ledManager.setup();
+	configuration.setup();
+
+	SerialCommands::setUp();
+	// Make sure the bus isn't stuck when resetting ESP without powering it down
+	// Fixes I2C issues for certain IMUs. Previously this feature was enabled for
+	// selected IMUs, now it's enabled for all. If some IMU turned out to be broken by
+	// this, check needs to be re-added.
+	auto clearResult = I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL);
+	if (clearResult != 0) {
+		logger.warn("Can't clear I2C bus, error %d", clearResult);
+	}
+
+	// join I2C bus
+
+#ifdef ESP32
+	// For some unknown reason the I2C seem to be open on ESP32-C3 by default. Let's
+	// just close it before opening it again. (The ESP32-C3 only has 1 I2C.)
+	Wire.end();
 #endif
 
-    Serial.println();
-    Serial.println();
-    Serial.println();
-
-    logger.info("SlimeVR v" FIRMWARE_VERSION " starting up...");
-
-    statusManager.setStatus(SlimeVR::Status::LOADING, true);
-
-    ledManager.setup();
-    configuration.setup();
-
-    SerialCommands::setUp();
-
-    I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down
-    // Fixes I2C issues for certain IMUs. Previously this feature was enabled for selected IMUs, now it's enabled for all.
-    // If some IMU turned out to be broken by this, check needs to be re-added.
-
-    // join I2C bus
-
-#if ESP32
-    // For some unknown reason the I2C seem to be open on ESP32-C3 by default. Let's just close it before opening it again. (The ESP32-C3 only has 1 I2C.)
-    Wire.end();
-#endif
-
-    // using `static_cast` here seems to be better, because there are 2 similar function signatures
-    Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+	// using `static_cast` here seems to be better, because there are 2 similar function
+	// signatures
+	Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
 
 #ifdef ESP8266
-    Wire.setClockStretchLimit(150000L); // Default stretch limit 150mS
+	Wire.setClockStretchLimit(150000L);  // Default stretch limit 150mS
 #endif
-#ifdef ESP32 // Counterpart on ESP32 to ClockStretchLimit
-    Wire.setTimeOut(150);
+#ifdef ESP32  // Counterpart on ESP32 to ClockStretchLimit
+	Wire.setTimeOut(150);
 #endif
-    Wire.setClock(I2C_SPEED);
+	Wire.setClock(I2C_SPEED);
 
-    // Wait for IMU to boot
-    delay(500);
+	// Wait for IMU to boot
+	delay(500);
 
-    sensorManager.setup();
+	sensorManager.setup();
 
-    networkManager.setup();
-    OTA::otaSetup(otaPassword);
-    battery.Setup();
+	networkManager.setup();
+	OTA::otaSetup(otaPassword);
+	battery.Setup();
 
-    statusManager.setStatus(SlimeVR::Status::LOADING, false);
+	statusManager.setStatus(SlimeVR::Status::LOADING, false);
 
-    sensorManager.postSetup();
+	sensorManager.postSetup();
 
-    loopTime = micros();
+	loopTime = micros();
+	tpsCounter.reset();
 }
 
-void loop()
-{
-    globalTimer.tick();
-    SerialCommands::update();
-    OTA::otaUpdate();
-    networkManager.update();
-    sensorManager.update();
-    battery.Loop();
-    ledManager.update();
+void loop() {
+	tpsCounter.update();
+	globalTimer.tick();
+	SerialCommands::update();
+	OTA::otaUpdate();
+	networkManager.update();
+
+#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
+	sensorMeasurer.before();
+#endif
+	sensorManager.update();
+#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
+	sensorMeasurer.after();
+#endif
+
+	battery.Loop();
+	ledManager.update();
+	I2CSCAN::update();
 #ifdef TARGET_LOOPTIME_MICROS
-    long elapsed = (micros() - loopTime);
-    if (elapsed < TARGET_LOOPTIME_MICROS)
-    {
-        long sleepus = TARGET_LOOPTIME_MICROS - elapsed - 100;//µs to sleep
-        long sleepms = sleepus / 1000;//ms to sleep
-        if(sleepms > 0) // if >= 1 ms
-        {
-            delay(sleepms); // sleep ms = save power
-            sleepus -= sleepms * 1000;
-        }
-        if (sleepus > 100)
-        {
-            delayMicroseconds(sleepus);
-        }
-    }
-    loopTime = micros();
+	long elapsed = (micros() - loopTime);
+	if (elapsed < TARGET_LOOPTIME_MICROS) {
+		long sleepus = TARGET_LOOPTIME_MICROS - elapsed - 100;  // µs to sleep
+		long sleepms = sleepus / 1000;  // ms to sleep
+		if (sleepms > 0)  // if >= 1 ms
+		{
+			delay(sleepms);  // sleep ms = save power
+			sleepus -= sleepms * 1000;
+		}
+		if (sleepus > 100) {
+			delayMicroseconds(sleepus);
+		}
+	}
+	loopTime = micros();
+#endif
+#if defined(PRINT_STATE_EVERY_MS) && PRINT_STATE_EVERY_MS > 0
+	unsigned long now = millis();
+	if (lastStatePrint + PRINT_STATE_EVERY_MS < now) {
+		lastStatePrint = now;
+		SerialCommands::printState();
+	}
 #endif
-    #if defined(PRINT_STATE_EVERY_MS) && PRINT_STATE_EVERY_MS > 0
-        unsigned long now = millis();
-        if(lastStatePrint + PRINT_STATE_EVERY_MS < now) {
-            lastStatePrint = now;
-            SerialCommands::printState();
-        }
-    #endif
 }

src/motionprocessing/GyroTemperatureCalibrator.cpp

@@ -1,211 +1,244 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 SlimeVR Contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 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:
+	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 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.
+	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 "GyroTemperatureCalibrator.h"
+
 #include "GlobalVars.h"
 
 void GyroTemperatureCalibrator::resetCurrentTemperatureState() {
-    if (!state.numSamples) return;
-    state.numSamples = 0;
-    state.tSum = 0;
-    state.xSum = 0;
-    state.ySum = 0;
-    state.zSum = 0;
+	if (!state.numSamples) {
+		return;
+	}
+	state.numSamples = 0;
+	state.tSum = 0;
+	state.xSum = 0;
+	state.ySum = 0;
+	state.zSum = 0;
 }
 
 // must be called for every raw gyro sample
-void GyroTemperatureCalibrator::updateGyroTemperatureCalibration(const float temperature, const bool restDetected, int16_t x, int16_t y, int16_t z) {
-    if (!restDetected) {
-        return resetCurrentTemperatureState();
-    }
+void GyroTemperatureCalibrator::updateGyroTemperatureCalibration(
+	const float temperature,
+	const bool restDetected,
+	int16_t x,
+	int16_t y,
+	int16_t z
+) {
+	if (!restDetected) {
+		return resetCurrentTemperatureState();
+	}
 
-    if (temperature < TEMP_CALIBRATION_MIN && !calibrationRunning) {
-        calibrationRunning = true;
-        configSaved = false;
-        config.reset();
-    }
-    if (temperature > TEMP_CALIBRATION_MAX && calibrationRunning) {
-        auto coeffs = poly.computeCoefficients();
-        for (uint32_t i = 0; i < poly.numCoefficients; i++) {
-            config.cx[i] = coeffs[0][i];
-            config.cy[i] = coeffs[1][i];
-            config.cz[i] = coeffs[2][i];
-        }
-        config.hasCoeffs = true;
-        bst = 0.0f;
-        bsx = 0;
-        bsy = 0;
-        bsz = 0;
-        bn = 0;
-        lastTemp = 0;
-        calibrationRunning = false;
-        if (!configSaveFailed && !configSaved) {
-            saveConfig();
-        }
-    }
-    if (calibrationRunning) {
-        if (fabs(lastTemp - temperature) > 0.03f) {
-            const double avgt = (double)bst / bn;
-            const double avgValues[] = {
-                (double)bsx / bn,
-                (double)bsy / bn,
-                (double)bsz / bn,
-            };
-            if (bn > 0) poly.update(avgt, avgValues);
-            bst = 0.0f;
-            bsx = 0;
-            bsy = 0;
-            bsz = 0;
-            bn = 0;
-            lastTemp = temperature;
-        } else {
-            bst += temperature;
-            bsx += x;
-            bsy += y;
-            bsz += z;
-            bn++;
-        }
-    }
-    
-    const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(temperature);
-    
-    if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) return;
+	if (temperature < TEMP_CALIBRATION_MIN && !calibrationRunning) {
+		calibrationRunning = true;
+		configSaved = false;
+		config.reset();
+	}
+	if (temperature > TEMP_CALIBRATION_MAX && calibrationRunning) {
+		auto coeffs = poly.computeCoefficients();
+		for (uint32_t i = 0; i < poly.numCoefficients; i++) {
+			config.cx[i] = coeffs[0][i];
+			config.cy[i] = coeffs[1][i];
+			config.cz[i] = coeffs[2][i];
+		}
+		config.hasCoeffs = true;
+		bst = 0.0f;
+		bsx = 0;
+		bsy = 0;
+		bsz = 0;
+		bn = 0;
+		lastTemp = 0;
+		calibrationRunning = false;
+		if (!configSaveFailed && !configSaved) {
+			saveConfig();
+		}
+	}
+	if (calibrationRunning) {
+		if (fabs(lastTemp - temperature) > 0.03f) {
+			const double avgt = (double)bst / bn;
+			const double avgValues[] = {
+				(double)bsx / bn,
+				(double)bsy / bn,
+				(double)bsz / bn,
+			};
+			if (bn > 0) {
+				poly.update(avgt, avgValues);
+			}
+			bst = 0.0f;
+			bsx = 0;
+			bsy = 0;
+			bsz = 0;
+			bn = 0;
+			lastTemp = temperature;
+		} else {
+			bst += temperature;
+			bsx += x;
+			bsy += y;
+			bsz += z;
+			bn++;
+		}
+	}
 
-    bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
-    if (currentTempAlreadyCalibrated) return;
+	const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(temperature);
 
-    if (state.temperatureCurrentIdx != idx) {
-        state.temperatureCurrentIdx = idx;
-        resetCurrentTemperatureState();
-    }
+	if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) {
+		return;
+	}
 
-    float temperatureStepBoundsMin = TEMP_CALIBRATION_IDX_TO_TEMP(idx) - TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP;
-    float temperatureStepBoundsMax = TEMP_CALIBRATION_IDX_TO_TEMP(idx) + TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP;
-    bool isTemperatureOutOfDeviationRange = 
-        temperature < temperatureStepBoundsMin || temperature > temperatureStepBoundsMax;
-    if (isTemperatureOutOfDeviationRange) {
-        return resetCurrentTemperatureState();
-    }
+	bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
+	if (currentTempAlreadyCalibrated) {
+		return;
+	}
 
-    state.numSamples++;
-    state.tSum += temperature;
-    state.xSum += x;
-    state.ySum += y;
-    state.zSum += z;
-    if (state.numSamples > samplesPerStep) {
-        bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
-        if (!currentTempAlreadyCalibrated) {
-            config.samplesTotal++;
-        }
-        config.samples[idx].t = state.tSum / state.numSamples;
-        config.samples[idx].x = ((double)state.xSum / state.numSamples);
-        config.samples[idx].y = ((double)state.ySum / state.numSamples);
-        config.samples[idx].z = ((double)state.zSum / state.numSamples);
+	if (state.temperatureCurrentIdx != idx) {
+		state.temperatureCurrentIdx = idx;
+		resetCurrentTemperatureState();
+	}
 
-        config.minTemperatureRange =
-            min(config.samples[idx].t, config.minTemperatureRange);
-        config.maxTemperatureRange =
-            max(config.samples[idx].t, config.maxTemperatureRange);
-        config.minCalibratedIdx =
-            TEMP_CALIBRATION_TEMP_TO_IDX(config.minTemperatureRange);
-        config.maxCalibratedIdx =
-            TEMP_CALIBRATION_TEMP_TO_IDX(config.maxTemperatureRange);
-        resetCurrentTemperatureState();
-    }
+	float temperatureStepBoundsMin
+		= TEMP_CALIBRATION_IDX_TO_TEMP(idx) - TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP;
+	float temperatureStepBoundsMax
+		= TEMP_CALIBRATION_IDX_TO_TEMP(idx) + TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP;
+	bool isTemperatureOutOfDeviationRange = temperature < temperatureStepBoundsMin
+										 || temperature > temperatureStepBoundsMax;
+	if (isTemperatureOutOfDeviationRange) {
+		return resetCurrentTemperatureState();
+	}
+
+	state.numSamples++;
+	state.tSum += temperature;
+	state.xSum += x;
+	state.ySum += y;
+	state.zSum += z;
+	if (state.numSamples > samplesPerStep) {
+		bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
+		if (!currentTempAlreadyCalibrated) {
+			config.samplesTotal++;
+		}
+		config.samples[idx].t = state.tSum / state.numSamples;
+		config.samples[idx].x = ((double)state.xSum / state.numSamples);
+		config.samples[idx].y = ((double)state.ySum / state.numSamples);
+		config.samples[idx].z = ((double)state.zSum / state.numSamples);
+
+		config.minTemperatureRange
+			= min(config.samples[idx].t, config.minTemperatureRange);
+		config.maxTemperatureRange
+			= max(config.samples[idx].t, config.maxTemperatureRange);
+		config.minCalibratedIdx
+			= TEMP_CALIBRATION_TEMP_TO_IDX(config.minTemperatureRange);
+		config.maxCalibratedIdx
+			= TEMP_CALIBRATION_TEMP_TO_IDX(config.maxTemperatureRange);
+		resetCurrentTemperatureState();
+	}
 }
 
-bool GyroTemperatureCalibrator::approximateOffset(const float temperature, float GOxyz[3]) {
-    if (!config.hasData()) return false;
-    if (config.hasCoeffs) {
-        if (lastApproximatedTemperature != 0.0f && temperature == lastApproximatedTemperature) {
-            GOxyz[0] = lastApproximatedOffsets[0];
-            GOxyz[1] = lastApproximatedOffsets[1];
-            GOxyz[2] = lastApproximatedOffsets[2];
-        } else {
-            float offsets[3] = { config.cx[3], config.cy[3], config.cz[3] };
-            for (int32_t i = 2; i >= 0; i--) {
-                offsets[0] = offsets[0] * temperature + config.cx[i];
-                offsets[1] = offsets[1] * temperature + config.cy[i];
-                offsets[2] = offsets[2] * temperature + config.cz[i];
-            }
-            lastApproximatedTemperature = temperature;
-            lastApproximatedOffsets[0] = GOxyz[0] = offsets[0];
-            lastApproximatedOffsets[1] = GOxyz[1] = offsets[1];
-            lastApproximatedOffsets[2] = GOxyz[2] = offsets[2];
-        }
-        return true;
-    }
+bool GyroTemperatureCalibrator::approximateOffset(
+	const float temperature,
+	float GOxyz[3]
+) {
+	if (!config.hasData()) {
+		return false;
+	}
+	if (config.hasCoeffs) {
+		if (lastApproximatedTemperature != 0.0f
+			&& temperature == lastApproximatedTemperature) {
+			GOxyz[0] = lastApproximatedOffsets[0];
+			GOxyz[1] = lastApproximatedOffsets[1];
+			GOxyz[2] = lastApproximatedOffsets[2];
+		} else {
+			float offsets[3] = {config.cx[3], config.cy[3], config.cz[3]};
+			for (int32_t i = 2; i >= 0; i--) {
+				offsets[0] = offsets[0] * temperature + config.cx[i];
+				offsets[1] = offsets[1] * temperature + config.cy[i];
+				offsets[2] = offsets[2] * temperature + config.cz[i];
+			}
+			lastApproximatedTemperature = temperature;
+			lastApproximatedOffsets[0] = GOxyz[0] = offsets[0];
+			lastApproximatedOffsets[1] = GOxyz[1] = offsets[1];
+			lastApproximatedOffsets[2] = GOxyz[2] = offsets[2];
+		}
+		return true;
+	}
 
-    const float constrainedTemperature = constrain(temperature,
-        config.minTemperatureRange,
-        config.maxTemperatureRange
-    );
+	const float constrainedTemperature = constrain(
+		temperature,
+		config.minTemperatureRange,
+		config.maxTemperatureRange
+	);
 
-    const int16_t idx =
-        TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature);
+	const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature);
 
-    if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) return false;
+	if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) {
+		return false;
+	}
 
-    bool isCurrentTempCalibrated = config.samples[idx].t != 0.0f;
-    if (isCurrentTempCalibrated) {
-        GOxyz[0] = config.samples[idx].x;
-        GOxyz[1] = config.samples[idx].y;
-        GOxyz[2] = config.samples[idx].z;
-        return true;
-    }
+	bool isCurrentTempCalibrated = config.samples[idx].t != 0.0f;
+	if (isCurrentTempCalibrated) {
+		GOxyz[0] = config.samples[idx].x;
+		GOxyz[1] = config.samples[idx].y;
+		GOxyz[2] = config.samples[idx].z;
+		return true;
+	}
 
-    return false;
+	return false;
 }
 
 bool GyroTemperatureCalibrator::loadConfig(float newSensitivity) {
-    bool ok = configuration.loadTemperatureCalibration(sensorId, config);
-    if (ok) {
-        config.rescaleSamples(newSensitivity);
-        if (config.fullyCalibrated()) {
-            configSaved = true;
-        }
-    } else {
-        m_Logger.warn("No temperature calibration data found for sensor %d, ignoring...", sensorId);
-        // m_Logger.info("Temperature calibration is advised");
-        m_Logger.info("Temperature calibration is a work-in-progress feature; any changes to its parameters or updates will render the saved temperature curve invalid and unloadable.");
-    }
-    return configSaved;
+	bool ok = configuration.loadTemperatureCalibration(sensorId, config);
+	if (ok) {
+		config.rescaleSamples(newSensitivity);
+		if (config.fullyCalibrated()) {
+			configSaved = true;
+		}
+	} else {
+		m_Logger.warn(
+			"No temperature calibration data found for sensor %d, ignoring...",
+			sensorId
+		);
+		// m_Logger.info("Temperature calibration is advised");
+		m_Logger.info(
+			"Temperature calibration is a work-in-progress feature; any changes to its "
+			"parameters or updates will render the saved temperature curve invalid and "
+			"unloadable."
+		);
+	}
+	return configSaved;
 }
 
 bool GyroTemperatureCalibrator::saveConfig() {
-    if (configuration.saveTemperatureCalibration(sensorId, config)) {
-        m_Logger.info("Saved temperature calibration config (%0.1f%) for sensorId:%i",
-            config.getCalibrationDonePercent(),
-            sensorId
-        );
-        if (config.fullyCalibrated()) {
-            configSaved = true;
-        } else {
-            m_Logger.info("Calibration will resume from this checkpoint after reboot");
-        }
-    } else {
-        configSaveFailed = true;
-        m_Logger.error("Something went wrong");
-    }
-    return configSaved;
+	if (configuration.saveTemperatureCalibration(sensorId, config)) {
+		m_Logger.info(
+			"Saved temperature calibration config (%0.1f%%) for sensorId:%i",
+			config.getCalibrationDonePercent(),
+			sensorId
+		);
+		if (config.fullyCalibrated()) {
+			configSaved = true;
+		} else {
+			m_Logger.info("Calibration will resume from this checkpoint after reboot");
+		}
+	} else {
+		configSaveFailed = true;
+		m_Logger.error("Something went wrong");
+	}
+	return configSaved;
 }

src/motionprocessing/GyroTemperatureCalibrator.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 SlimeVR Contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 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:
+	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 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.
+	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.
 */
 
 #ifndef GYRO_TEMPERATURE_CALIBRATOR_H
@@ -26,11 +26,11 @@
 
 #include <Arduino.h>
 #include <stdint.h>
-#include "debug.h"
-#include "../logging/Logger.h"
-#include "../configuration/SensorConfig.h"
-#include "OnlinePolyfit.h"
 
+#include "../configuration/SensorConfig.h"
+#include "../logging/Logger.h"
+#include "OnlinePolyfit.h"
+#include "debug.h"
 
 // Degrees C
 // default: 15.0f
@@ -54,166 +54,186 @@
 #define TEMP_CALIBRATION_SECONDS_PER_STEP 0.2f
 
 #if IMU == IMU_ICM20948
-    // 16 bit 333 lsb/K, ~0.00508 degrees per bit
-    // already calibrated by DMP?
+// 16 bit 333 lsb/K, ~0.00508 degrees per bit
+// already calibrated by DMP?
 #elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050
-    // 16 bit 340 lsb/K, ~0.00518 degrees per bit
+// 16 bit 340 lsb/K, ~0.00518 degrees per bit
 #elif IMU == IMU_MPU9250
-    // 16 bit 333 lsb/K, ~0.00508 degrees per bit
+// 16 bit 333 lsb/K, ~0.00508 degrees per bit
 #elif IMU == IMU_BMI160
-    // 16 bit 128 lsb/K, ~0.00195 degrees per bit
+// 16 bit 128 lsb/K, ~0.00195 degrees per bit
 #endif
 
+constexpr uint16_t TEMP_CALIBRATION_BUFFER_SIZE
+	= (uint16_t)((TEMP_CALIBRATION_MAX - TEMP_CALIBRATION_MIN)
+				 * (1 / TEMP_CALIBRATION_STEP));
 
-constexpr uint16_t TEMP_CALIBRATION_BUFFER_SIZE =
-    (uint16_t)((TEMP_CALIBRATION_MAX - TEMP_CALIBRATION_MIN) * (1/TEMP_CALIBRATION_STEP));
+#define TEMP_CALIBRATION_TEMP_TO_IDX(temperature)                                  \
+	(uint16_t)(                                                                    \
+		(temperature + TEMP_CALIBRATION_STEP / 2.0f) * (1 / TEMP_CALIBRATION_STEP) \
+		- TEMP_CALIBRATION_MIN * (1 / TEMP_CALIBRATION_STEP)                       \
+	)
 
-#define TEMP_CALIBRATION_TEMP_TO_IDX(temperature) (uint16_t)( \
-    (temperature + TEMP_CALIBRATION_STEP/2.0f) * (1/TEMP_CALIBRATION_STEP) - \
-    TEMP_CALIBRATION_MIN * (1/TEMP_CALIBRATION_STEP) \
-)
-
-#define TEMP_CALIBRATION_IDX_TO_TEMP(idx) (float)( \
-    ((float)idx / (1.0f/TEMP_CALIBRATION_STEP)) + \
-    TEMP_CALIBRATION_MIN \
-)
+#define TEMP_CALIBRATION_IDX_TO_TEMP(idx) \
+	(float)(((float)idx / (1.0f / TEMP_CALIBRATION_STEP)) + TEMP_CALIBRATION_MIN)
 
 struct GyroTemperatureCalibrationState {
-    uint16_t temperatureCurrentIdx;
-    uint32_t numSamples;
-    float tSum;
-    int32_t xSum;
-    int32_t ySum;
-    int32_t zSum;
+	uint16_t temperatureCurrentIdx;
+	uint32_t numSamples;
+	float tSum;
+	int32_t xSum;
+	int32_t ySum;
+	int32_t zSum;
 
-    GyroTemperatureCalibrationState(): temperatureCurrentIdx(-1), numSamples(0), tSum(0.0f), xSum(0), ySum(0), zSum(0)
-    { };
+	GyroTemperatureCalibrationState()
+		: temperatureCurrentIdx(-1)
+		, numSamples(0)
+		, tSum(0.0f)
+		, xSum(0)
+		, ySum(0)
+		, zSum(0){};
 };
 
 struct GyroTemperatureOffsetSample {
-    float t;
-    float x;
-    float y;
-    float z;
+	float t;
+	float x;
+	float y;
+	float z;
 
-    GyroTemperatureOffsetSample(): t(0.0f), x(0), y(0), z(0)
-    { }
+	GyroTemperatureOffsetSample()
+		: t(0.0f)
+		, x(0)
+		, y(0)
+		, z(0) {}
 };
 
 struct GyroTemperatureCalibrationConfig {
-    SlimeVR::Configuration::SensorConfigType type;
+	SlimeVR::Configuration::SensorConfigType type;
 
-    float sensitivityLSB;
-    float minTemperatureRange;
-    float maxTemperatureRange;
-    uint16_t minCalibratedIdx = 0;
-    uint16_t maxCalibratedIdx = 0;
-    GyroTemperatureOffsetSample samples[TEMP_CALIBRATION_BUFFER_SIZE];
-    uint16_t samplesTotal = 0;
-    float cx[4] = {0.0};
-    float cy[4] = {0.0};
-    float cz[4] = {0.0};
-    bool hasCoeffs = false;
+	float sensitivityLSB;
+	float minTemperatureRange;
+	float maxTemperatureRange;
+	uint16_t minCalibratedIdx = 0;
+	uint16_t maxCalibratedIdx = 0;
+	GyroTemperatureOffsetSample samples[TEMP_CALIBRATION_BUFFER_SIZE];
+	uint16_t samplesTotal = 0;
+	float cx[4] = {0.0};
+	float cy[4] = {0.0};
+	float cz[4] = {0.0};
+	bool hasCoeffs = false;
 
-    GyroTemperatureCalibrationConfig(SlimeVR::Configuration::SensorConfigType _type, float _sensitivityLSB) :
-        type(_type),
-        sensitivityLSB(_sensitivityLSB),
-        minTemperatureRange(1000),
-        maxTemperatureRange(-1000)
-    { }
+	GyroTemperatureCalibrationConfig(
+		SlimeVR::Configuration::SensorConfigType _type,
+		float _sensitivityLSB
+	)
+		: type(_type)
+		, sensitivityLSB(_sensitivityLSB)
+		, minTemperatureRange(1000)
+		, maxTemperatureRange(-1000) {}
 
-    bool hasData() {
-        return minTemperatureRange != 1000;
-    }
+	bool hasData() { return minTemperatureRange != 1000; }
 
-    bool fullyCalibrated() {
-        return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs;
-    }
+	bool fullyCalibrated() {
+		return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs;
+	}
 
-    float getCalibrationDonePercent() {
-        return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f;
-    }
+	float getCalibrationDonePercent() {
+		return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f;
+	}
 
-    void rescaleSamples(float newSensitivityLSB) {
-        if (sensitivityLSB == newSensitivityLSB) return;
-        float mul = newSensitivityLSB / sensitivityLSB;
-        for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) {
-            if (samples[i].t == 0) continue;
-            samples[i].x *= mul;
-            samples[i].y *= mul;
-            samples[i].z *= mul;
-        }
-        sensitivityLSB = newSensitivityLSB;
-    }
+	void rescaleSamples(float newSensitivityLSB) {
+		if (sensitivityLSB == newSensitivityLSB) {
+			return;
+		}
+		float mul = newSensitivityLSB / sensitivityLSB;
+		for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) {
+			if (samples[i].t == 0) {
+				continue;
+			}
+			samples[i].x *= mul;
+			samples[i].y *= mul;
+			samples[i].z *= mul;
+		}
+		sensitivityLSB = newSensitivityLSB;
+	}
 
-    void reset() {
-        minTemperatureRange = 1000;
-        maxTemperatureRange = -1000;
-        samplesTotal = 0;
-        for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) {
-            samples[i].t = 0;
-            samples[i].x = 0;
-            samples[i].y = 0;
-            samples[i].z = 0;
-        }
-        hasCoeffs = false;
-    }
+	void reset() {
+		minTemperatureRange = 1000;
+		maxTemperatureRange = -1000;
+		samplesTotal = 0;
+		for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) {
+			samples[i].t = 0;
+			samples[i].x = 0;
+			samples[i].y = 0;
+			samples[i].z = 0;
+		}
+		hasCoeffs = false;
+	}
 };
 
 class GyroTemperatureCalibrator {
 public:
-    uint8_t sensorId;
-    GyroTemperatureCalibrationConfig config;
+	uint8_t sensorId;
+	GyroTemperatureCalibrationConfig config;
 
-    // set when config is fully calibrated is saved OR on startup when loaded config is fully calibrated;
-    // left unset when sending saving command over serial so it can continue calibration and autosave later
-    bool configSaved = false;
-    bool configSaveFailed = false;
+	// set when config is fully calibrated is saved OR on startup when loaded config is
+	// fully calibrated; left unset when sending saving command over serial so it can
+	// continue calibration and autosave later
+	bool configSaved = false;
+	bool configSaveFailed = false;
 
-    GyroTemperatureCalibrator(SlimeVR::Configuration::SensorConfigType _configType, uint8_t _sensorId, float sensitivity, uint32_t _samplesPerStep):
-        sensorId(_sensorId),
-        config(_configType, sensitivity),
-        samplesPerStep(_samplesPerStep),
-        m_Logger(SlimeVR::Logging::Logger("GyroTemperatureCalibrator"))
-    {
-        char buf[4];
-        sprintf(buf, "%u", _sensorId);
-        m_Logger.setTag(buf);
-    }
+	GyroTemperatureCalibrator(
+		SlimeVR::Configuration::SensorConfigType _configType,
+		uint8_t _sensorId,
+		float sensitivity,
+		uint32_t _samplesPerStep
+	)
+		: sensorId(_sensorId)
+		, config(_configType, sensitivity)
+		, samplesPerStep(_samplesPerStep)
+		, m_Logger(SlimeVR::Logging::Logger("GyroTemperatureCalibrator")) {
+		char buf[4];
+		sprintf(buf, "%u", _sensorId);
+		m_Logger.setTag(buf);
+	}
 
-    void updateGyroTemperatureCalibration(const float temperature, const bool restDetected, int16_t x, int16_t y, int16_t z);
-    bool approximateOffset(const float temperature, float GOxyz[3]);
-    bool loadConfig(float newSensitivity);
-    bool saveConfig();
+	void updateGyroTemperatureCalibration(
+		const float temperature,
+		const bool restDetected,
+		int16_t x,
+		int16_t y,
+		int16_t z
+	);
+	bool approximateOffset(const float temperature, float GOxyz[3]);
+	bool loadConfig(float newSensitivity);
+	bool saveConfig();
 
-    void reset() {
-        config.reset();
-        configSaved = false;
-        configSaveFailed = false;
-    }
+	void reset() {
+		config.reset();
+		configSaved = false;
+		configSaveFailed = false;
+	}
 
-    bool isCalibrating() {
-        return calibrationRunning;
-    }
+	bool isCalibrating() { return calibrationRunning; }
 
 private:
-    GyroTemperatureCalibrationState state;
-    uint32_t samplesPerStep;
-    SlimeVR::Logging::Logger m_Logger;
+	GyroTemperatureCalibrationState state;
+	uint32_t samplesPerStep;
+	SlimeVR::Logging::Logger m_Logger;
 
-    float lastApproximatedTemperature = 0.0f;
-    float lastApproximatedOffsets[3];
+	float lastApproximatedTemperature = 0.0f;
+	float lastApproximatedOffsets[3];
 
-    bool calibrationRunning = false;
-    OnlineVectorPolyfit<3, 3, (uint64_t)1e9> poly;
-    float bst = 0.0f;
-    int32_t bsx = 0;
-    int32_t bsy = 0;
-    int32_t bsz = 0;
-    int32_t bn = 0;
-    float lastTemp = 0;
+	bool calibrationRunning = false;
+	OnlineVectorPolyfit<3, 3, (uint64_t)1e9> poly;
+	float bst = 0.0f;
+	int32_t bsx = 0;
+	int32_t bsy = 0;
+	int32_t bsz = 0;
+	int32_t bn = 0;
+	float lastTemp = 0;
 
-    void resetCurrentTemperatureState();
+	void resetCurrentTemperatureState();
 };
 
 #endif

src/motionprocessing/OnlinePolyfit.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 SlimeVR Contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 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:
+	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 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.
+	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 <Arduino.h>
 
@@ -28,98 +28,103 @@
 template <uint32_t degree, uint32_t dimensions, uint64_t forgettingFactorNumSamples>
 class OnlineVectorPolyfit {
 public:
-  constexpr static int32_t numDimensions = dimensions;
-  constexpr static int32_t numCoefficients = degree + 1;
-  constexpr static double forgettingFactor = std::exp(-1.0/forgettingFactorNumSamples);
-  
-  OnlineVectorPolyfit() {
-    reset();
-  }
+	constexpr static int32_t numDimensions = dimensions;
+	constexpr static int32_t numCoefficients = degree + 1;
+	constexpr static double forgettingFactor
+		= std::exp(-1.0 / forgettingFactorNumSamples);
 
-  void reset() {
-    std::fill(Rb[0], Rb[0] + rows * cols, 0.0);
-    std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f);
-  }
+	OnlineVectorPolyfit() { reset(); }
 
-  // Recursive least squares update using QR decomposition by Givens transformations
-  void update(double xValue, const double yValues[numDimensions]) {
-    double xin[cols];
-    xin[0] = 1;
-    for (int32_t i = 1; i < cols - numDimensions; i++) {
-      xin[i] = xin[i - 1] * xValue;
-    }
-    for (int32_t i = 0; i < numDimensions; i++) {
-      xin[cols - numDimensions + i] = yValues[i];
-    }
-    
-    // degree = 3, dimensions = 3, yValues = [x, y, z]
-    // B I I I Ix Iy Iz       R R R R bx by bz
-    // . B I I Ix Iy Iz  ===  . R R R bx by bz
-    // . . B I Ix Iy Iz  ===  . . R R bx by bz
-    // . . . B Ix Iy Iz       . . . R bx by bz
+	void reset() {
+		std::fill(Rb[0], Rb[0] + rows * cols, 0.0);
+		std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f);
+	}
 
-    // https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf
+	// Recursive least squares update using QR decomposition by Givens transformations
+	void update(double xValue, const double yValues[numDimensions]) {
+		double xin[cols];
+		xin[0] = 1;
+		for (int32_t i = 1; i < cols - numDimensions; i++) {
+			xin[i] = xin[i - 1] * xValue;
+		}
+		for (int32_t i = 0; i < numDimensions; i++) {
+			xin[cols - numDimensions + i] = yValues[i];
+		}
 
-    for (int32_t y = 0; y < rows; y++) {
-      double c = 1, s = 0;
-      if (xin[y] != 0.0) {
-        Rb[y][y] *= forgettingFactor;
-        const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]);
-        c = Rb[y][y] * (1.0 / norm);
-        s = xin[y] * (1.0 / norm);
-        Rb[y][y] = norm;
-      }
-      for (int32_t x = y + 1; x < cols; x++) {
-        Rb[y][x] *= forgettingFactor;
-        const double xout = (c * xin[x] - s * Rb[y][x]);
-        Rb[y][x] = (s * xin[x] + c * Rb[y][x]);
-        xin[x] = xout;
-      }
-    }
-  }
+		// degree = 3, dimensions = 3, yValues = [x, y, z]
+		// B I I I Ix Iy Iz       R R R R bx by bz
+		// . B I I Ix Iy Iz  ===  . R R R bx by bz
+		// . . B I Ix Iy Iz  ===  . . R R bx by bz
+		// . . . B Ix Iy Iz       . . . R bx by bz
 
-  // Back solves upper triangular system
-  // Returns float[numDimensions][numCoefficients] coefficients from lowest to highest power
-  const auto computeCoefficients() {
-    // https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution
-    for (int32_t d = 0; d < numDimensions; d++) {
-      for (int32_t y = rows - 1; y >= 0; y--) {
-        const int32_t bColumn = cols - numDimensions + d;
-        coeffs[d][y] = Rb[y][bColumn];
-        if (Rb[y][y] == 0.0) continue;
-        for (int32_t x = y + 1; x < rows; x++) {
-          coeffs[d][y] -= coeffs[d][x] * Rb[y][x];
-        }
-        coeffs[d][y] /= Rb[y][y];
-      }
-    }
-    return coeffs;
-  }
+		// https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf
 
-  float predict(int32_t d, float x) {
-    if (d >= numDimensions) return 0.0;
-    // https://en.wikipedia.org/wiki/Horner%27s_method
-    float y = coeffs[d][numCoefficients - 1];
-    for (int32_t i = numCoefficients - 2; i >= 0; i--) {
-      y = y * x + coeffs[d][i];
-    }
-    return y;
-  }
+		for (int32_t y = 0; y < rows; y++) {
+			double c = 1, s = 0;
+			if (xin[y] != 0.0) {
+				Rb[y][y] *= forgettingFactor;
+				const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]);
+				c = Rb[y][y] * (1.0 / norm);
+				s = xin[y] * (1.0 / norm);
+				Rb[y][y] = norm;
+			}
+			for (int32_t x = y + 1; x < cols; x++) {
+				Rb[y][x] *= forgettingFactor;
+				const double xout = (c * xin[x] - s * Rb[y][x]);
+				Rb[y][x] = (s * xin[x] + c * Rb[y][x]);
+				xin[x] = xout;
+			}
+		}
+	}
+
+	// Back solves upper triangular system
+	// Returns float[numDimensions][numCoefficients] coefficients from lowest to highest
+	// power
+	const auto computeCoefficients() {
+		// https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution
+		for (int32_t d = 0; d < numDimensions; d++) {
+			for (int32_t y = rows - 1; y >= 0; y--) {
+				const int32_t bColumn = cols - numDimensions + d;
+				coeffs[d][y] = Rb[y][bColumn];
+				if (Rb[y][y] == 0.0) {
+					continue;
+				}
+				for (int32_t x = y + 1; x < rows; x++) {
+					coeffs[d][y] -= coeffs[d][x] * Rb[y][x];
+				}
+				coeffs[d][y] /= Rb[y][y];
+			}
+		}
+		return coeffs;
+	}
+
+	float predict(int32_t d, float x) {
+		if (d >= numDimensions) {
+			return 0.0;
+		}
+		// https://en.wikipedia.org/wiki/Horner%27s_method
+		float y = coeffs[d][numCoefficients - 1];
+		for (int32_t i = numCoefficients - 2; i >= 0; i--) {
+			y = y * x + coeffs[d][i];
+		}
+		return y;
+	}
+
+	std::pair<float, float> tangentAt(float x) {
+		float intercept = coeffs[0];
+		float slope = coeffs[1];
+		for (uint32_t i = 2; i < degree + 1; i++) {
+			intercept -= coeffs[i] * (i - 1) * pow(x, i);
+			slope += coeffs[i] * i * pow(x, i - 1);
+		}
+		return std::make_pair(slope, intercept);
+	}
 
-  std::pair<float, float> tangentAt(float x) {
-    float intercept = coeffs[0];
-    float slope = coeffs[1];
-    for (uint32_t i = 2; i < degree + 1; i++) {
-      intercept -= coeffs[i] * (i - 1) * pow(x, i);
-      slope += coeffs[i] * i * pow(x, i - 1);
-    }
-    return std::make_pair(slope, intercept);
-  }
 private:
-  constexpr static int32_t rows = numCoefficients;
-  constexpr static int32_t cols = numCoefficients + 3;
-  double Rb[rows][cols];
-  float coeffs[numDimensions][rows];
+	constexpr static int32_t rows = numCoefficients;
+	constexpr static int32_t cols = numCoefficients + 3;
+	double Rb[rows][cols];
+	float coeffs[numDimensions][rows];
 };
 
 #endif

src/motionprocessing/RestDetection.h

@@ -11,252 +11,281 @@
 // #define REST_DETECTION_DISABLE_LPF
 
 #include <Arduino.h>
-#include <vqf.h>
 #include <basicvqf.h>
+#include <vqf.h>
+
 #include "types.h"
 
 #define NaN std::numeric_limits<sensor_real_t>::quiet_NaN()
 
 struct RestDetectionParams {
-    sensor_real_t biasClip;
-    sensor_real_t biasSigmaRest;
-    sensor_real_t restMinTime;
-    sensor_real_t restFilterTau;
-    sensor_real_t restThGyr;
-    sensor_real_t restThAcc;
-    RestDetectionParams():
-        biasClip(2.0f),
-        biasSigmaRest(0.03f),
-        restMinTime(1.5),
-        restFilterTau(0.5f),
-        restThGyr(2.0f),
-        restThAcc(0.5f)
-    { }
+	sensor_real_t biasClip;
+	sensor_real_t biasSigmaRest;
+	sensor_real_t restMinTime;
+	sensor_real_t restFilterTau;
+	sensor_real_t restThGyr;
+	sensor_real_t restThAcc;
+	RestDetectionParams()
+		: biasClip(2.0f)
+		, biasSigmaRest(0.03f)
+		, restMinTime(1.5)
+		, restFilterTau(0.5f)
+		, restThGyr(2.0f)
+		, restThAcc(0.5f) {}
 };
 
 inline sensor_real_t square(sensor_real_t x) { return x * x; }
 
 class RestDetection {
 public:
-    RestDetection(sensor_real_t gyrTs, sensor_real_t accTs) {
-        this->gyrTs = gyrTs;
-        this->accTs = accTs;
-        setup();
-    }
-    RestDetection(const RestDetectionParams &params, sensor_real_t gyrTs, sensor_real_t accTs) {
-        this->params = params;
-        this->gyrTs = gyrTs;
-        this->accTs = accTs;
-        setup();
-    }
+	RestDetection(sensor_real_t gyrTs, sensor_real_t accTs) {
+		this->gyrTs = gyrTs;
+		this->accTs = accTs;
+		setup();
+	}
+	RestDetection(
+		const RestDetectionParams& params,
+		sensor_real_t gyrTs,
+		sensor_real_t accTs
+	) {
+		this->params = params;
+		this->gyrTs = gyrTs;
+		this->accTs = accTs;
+		setup();
+	}
 
 #ifndef REST_DETECTION_DISABLE_LPF
-    void filterInitialState(sensor_real_t x0, const double b[3], const double a[2], double out[])
-    {
-        // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter
-        // update equation)
-        out[0] = x0*(1 - b[0]);
-        out[1] = x0*(b[2] - a[1]);
-    }
+	void filterInitialState(
+		sensor_real_t x0,
+		const double b[3],
+		const double a[2],
+		double out[]
+	) {
+		// initial state for steady state (equivalent to scipy.signal.lfilter_zi,
+		// obtained by setting y=x=x0 in the filter update equation)
+		out[0] = x0 * (1 - b[0]);
+		out[1] = x0 * (b[2] - a[1]);
+	}
 
-    sensor_real_t filterStep(sensor_real_t x, const double b[3], const double a[2], double state[2])
-    {
-        // difference equations based on scipy.signal.lfilter documentation
-        // assumes that a0 == 1.0
-        double y = b[0]*x + state[0];
-        state[0] = b[1]*x - a[0]*y + state[1];
-        state[1] = b[2]*x - a[1]*y;
-        return y;
-    }
+	sensor_real_t
+	filterStep(sensor_real_t x, const double b[3], const double a[2], double state[2]) {
+		// difference equations based on scipy.signal.lfilter documentation
+		// assumes that a0 == 1.0
+		double y = b[0] * x + state[0];
+		state[0] = b[1] * x - a[0] * y + state[1];
+		state[1] = b[2] * x - a[1] * y;
+		return y;
+	}
 
+	void filterVec(
+		const sensor_real_t x[],
+		size_t N,
+		sensor_real_t tau,
+		sensor_real_t Ts,
+		const double b[3],
+		const double a[2],
+		double state[],
+		sensor_real_t out[]
+	) {
+		assert(N >= 2);
 
-    void filterVec(const sensor_real_t x[], size_t N, sensor_real_t tau, sensor_real_t Ts, const double b[3],
-                        const double a[2], double state[], sensor_real_t out[])
-    {
-        assert(N>=2);
+		// to avoid depending on a single sample, average the first samples (for
+		// duration tau) and then use this average to calculate the filter initial state
+		if (isnan(state[0])) {  // initialization phase
+			if (isnan(state[1])) {  // first sample
+				state[1] = 0;  // state[1] is used to store the sample count
+				for (size_t i = 0; i < N; i++) {
+					state[2 + i] = 0;  // state[2+i] is used to store the sum
+				}
+			}
+			state[1]++;
+			for (size_t i = 0; i < N; i++) {
+				state[2 + i] += x[i];
+				out[i] = state[2 + i] / state[1];
+			}
+			if (state[1] * Ts >= tau) {
+				for (size_t i = 0; i < N; i++) {
+					filterInitialState(out[i], b, a, state + 2 * i);
+				}
+			}
+			return;
+		}
 
-        // to avoid depending on a single sample, average the first samples (for duration tau)
-        // and then use this average to calculate the filter initial state
-        if (isnan(state[0])) { // initialization phase
-            if (isnan(state[1])) { // first sample
-                state[1] = 0; // state[1] is used to store the sample count
-                for(size_t i = 0; i < N; i++) {
-                    state[2+i] = 0; // state[2+i] is used to store the sum
-                }
-            }
-            state[1]++;
-            for (size_t i = 0; i < N; i++) {
-                state[2+i] += x[i];
-                out[i] = state[2+i]/state[1];
-            }
-            if (state[1]*Ts >= tau) {
-                for(size_t i = 0; i < N; i++) {
-                    filterInitialState(out[i], b, a, state+2*i);
-                }
-            }
-            return;
-        }
-
-        for (size_t i = 0; i < N; i++) {
-            out[i] = filterStep(x[i], b, a, state+2*i);
-        }
-    }
+		for (size_t i = 0; i < N; i++) {
+			out[i] = filterStep(x[i], b, a, state + 2 * i);
+		}
+	}
 #endif
 
-    void updateGyr(const sensor_real_t gyr[3]) {
+	void updateGyr(const sensor_real_t gyr[3]) {
 #ifdef REST_DETECTION_DISABLE_LPF
-        gyrLastSquaredDeviation =
-            square(gyr[0] - lastSample.gyr[0]) +
-            square(gyr[1] - lastSample.gyr[1]) +
-            square(gyr[2] - lastSample.gyr[2]);
+		gyrLastSquaredDeviation = square(gyr[0] - lastSample.gyr[0])
+								+ square(gyr[1] - lastSample.gyr[1])
+								+ square(gyr[2] - lastSample.gyr[2]);
 
-        sensor_real_t biasClip = params.biasClip*sensor_real_t(M_PI/180.0);
-        if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0))
-                || fabs(lastSample.gyr[0]) > biasClip || fabs(lastSample.gyr[1]) > biasClip
-                || fabs(lastSample.gyr[2]) > biasClip) {
-            restTime = 0;
-            restDetected = false;
-        }
+		sensor_real_t biasClip = params.biasClip * sensor_real_t(M_PI / 180.0);
+		if (gyrLastSquaredDeviation
+				>= square(params.restThGyr * sensor_real_t(M_PI / 180.0))
+			|| fabs(lastSample.gyr[0]) > biasClip || fabs(lastSample.gyr[1]) > biasClip
+			|| fabs(lastSample.gyr[2]) > biasClip) {
+			restTime = 0;
+			restDetected = false;
+		}
 
-        lastSample.gyr[0] = gyr[0];
-        lastSample.gyr[1] = gyr[1];
-        lastSample.gyr[2] = gyr[2];
+		lastSample.gyr[0] = gyr[0];
+		lastSample.gyr[1] = gyr[1];
+		lastSample.gyr[2] = gyr[2];
 #else
-        filterVec(gyr, 3, params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA,
-                restGyrLpState, restLastGyrLp);
+		filterVec(
+			gyr,
+			3,
+			params.restFilterTau,
+			gyrTs,
+			restGyrLpB,
+			restGyrLpA,
+			restGyrLpState,
+			restLastGyrLp
+		);
 
-        gyrLastSquaredDeviation =
-            square(gyr[0] - restLastGyrLp[0]) +
-            square(gyr[1] - restLastGyrLp[1]) +
-            square(gyr[2] - restLastGyrLp[2]);
+		gyrLastSquaredDeviation = square(gyr[0] - restLastGyrLp[0])
+								+ square(gyr[1] - restLastGyrLp[1])
+								+ square(gyr[2] - restLastGyrLp[2]);
 
-        sensor_real_t biasClip = params.biasClip*sensor_real_t(M_PI/180.0);
-        if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0))
-                || fabs(restLastGyrLp[0]) > biasClip || fabs(restLastGyrLp[1]) > biasClip
-                || fabs(restLastGyrLp[2]) > biasClip) {
-            restTime = 0;
-            restDetected = false;
-        }
+		sensor_real_t biasClip = params.biasClip * sensor_real_t(M_PI / 180.0);
+		if (gyrLastSquaredDeviation
+				>= square(params.restThGyr * sensor_real_t(M_PI / 180.0))
+			|| fabs(restLastGyrLp[0]) > biasClip || fabs(restLastGyrLp[1]) > biasClip
+			|| fabs(restLastGyrLp[2]) > biasClip) {
+			restTime = 0;
+			restDetected = false;
+		}
 #endif
-    }
+	}
 
-    void updateAcc(sensor_real_t dt, const sensor_real_t acc[3]) {
-        if (acc[0] == sensor_real_t(0.0) && acc[1] == sensor_real_t(0.0) && acc[2] == sensor_real_t(0.0)) {
-            return;
-        }
+	void updateAcc(sensor_real_t dt, const sensor_real_t acc[3]) {
+		if (acc[0] == sensor_real_t(0.0) && acc[1] == sensor_real_t(0.0)
+			&& acc[2] == sensor_real_t(0.0)) {
+			return;
+		}
 
 #ifdef REST_DETECTION_DISABLE_LPF
-        accLastSquaredDeviation =
-            square(acc[0] - lastSample.acc[0]) +
-            square(acc[1] - lastSample.acc[1]) +
-            square(acc[2] - lastSample.acc[2]);
+		accLastSquaredDeviation = square(acc[0] - lastSample.acc[0])
+								+ square(acc[1] - lastSample.acc[1])
+								+ square(acc[2] - lastSample.acc[2]);
 
-        if (accLastSquaredDeviation >= square(params.restThAcc)) {
-            restTime = 0;
-            restDetected = false;
-        } else {
-            restTime += dt;
-            if (restTime >= params.restMinTime) {
-                restDetected = true;
-            }
-        }
+		if (accLastSquaredDeviation >= square(params.restThAcc)) {
+			restTime = 0;
+			restDetected = false;
+		} else {
+			restTime += dt;
+			if (restTime >= params.restMinTime) {
+				restDetected = true;
+			}
+		}
 
-        lastSample.acc[0] = acc[0];
-        lastSample.acc[1] = acc[1];
-        lastSample.acc[2] = acc[2];
+		lastSample.acc[0] = acc[0];
+		lastSample.acc[1] = acc[1];
+		lastSample.acc[2] = acc[2];
 #else
-        filterVec(acc, 3, params.restFilterTau, accTs, restAccLpB, restAccLpA,
-                  restAccLpState, restLastAccLp);
+		filterVec(
+			acc,
+			3,
+			params.restFilterTau,
+			accTs,
+			restAccLpB,
+			restAccLpA,
+			restAccLpState,
+			restLastAccLp
+		);
 
-        accLastSquaredDeviation =
-            square(acc[0] - restLastAccLp[0]) +
-            square(acc[1] - restLastAccLp[1]) +
-            square(acc[2] - restLastAccLp[2]);
+		accLastSquaredDeviation = square(acc[0] - restLastAccLp[0])
+								+ square(acc[1] - restLastAccLp[1])
+								+ square(acc[2] - restLastAccLp[2]);
 
-        if (accLastSquaredDeviation >= square(params.restThAcc)) {
-            restTime = 0;
-            restDetected = false;
-        } else {
-            restTime += dt;
-            if (restTime >= params.restMinTime) {
-                restDetected = true;
-            }
-        }
+		if (accLastSquaredDeviation >= square(params.restThAcc)) {
+			restTime = 0;
+			restDetected = false;
+		} else {
+			restTime += dt;
+			if (restTime >= params.restMinTime) {
+				restDetected = true;
+			}
+		}
 #endif
-    }
+	}
 
-    bool getRestDetected() {
-        return restDetected;
-    }
+	bool getRestDetected() { return restDetected; }
 
 #ifndef REST_DETECTION_DISABLE_LPF
-    void resetState() {
-        restDetected = false;
+	void resetState() {
+		restDetected = false;
 
-        gyrLastSquaredDeviation = 0.0;
-        accLastSquaredDeviation = 0.0;
-        restTime = 0.0;
-        std::fill(restLastGyrLp, restLastGyrLp + 3, 0.0);
-        std::fill(restGyrLpState, restGyrLpState + 3*2, NaN);
-        std::fill(restLastAccLp, restLastAccLp + 3, 0.0);
-        std::fill(restAccLpState, restAccLpState + 3*2, NaN);
-    }
+		gyrLastSquaredDeviation = 0.0;
+		accLastSquaredDeviation = 0.0;
+		restTime = 0.0;
+		std::fill(restLastGyrLp, restLastGyrLp + 3, 0.0);
+		std::fill(restGyrLpState, restGyrLpState + 3 * 2, NaN);
+		std::fill(restLastAccLp, restLastAccLp + 3, 0.0);
+		std::fill(restAccLpState, restAccLpState + 3 * 2, NaN);
+	}
 
-    void filterCoeffs(sensor_real_t tau, sensor_real_t Ts, double outB[], double outA[]) {
-        assert(tau > 0);
-        assert(Ts > 0);
-        // second order Butterworth filter based on https://stackoverflow.com/a/52764064
-        double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response
-        double C = tan(M_PI*fc*double(Ts));
-        double D = C*C + sqrt(2)*C + 1;
-        double b0 = C*C/D;
-        outB[0] = b0;
-        outB[1] = 2*b0;
-        outB[2] = b0;
-        // a0 = 1.0
-        outA[0] = 2*(C*C-1)/D; // a1
-        outA[1] = (1-sqrt(2)*C+C*C)/D; // a2
-    }
+	void
+	filterCoeffs(sensor_real_t tau, sensor_real_t Ts, double outB[], double outA[]) {
+		assert(tau > 0);
+		assert(Ts > 0);
+		// second order Butterworth filter based on https://stackoverflow.com/a/52764064
+		double fc
+			= (M_SQRT2 / (2.0 * M_PI))
+			/ double(tau
+			);  // time constant of dampened, non-oscillating part of step response
+		double C = tan(M_PI * fc * double(Ts));
+		double D = C * C + sqrt(2) * C + 1;
+		double b0 = C * C / D;
+		outB[0] = b0;
+		outB[1] = 2 * b0;
+		outB[2] = b0;
+		// a0 = 1.0
+		outA[0] = 2 * (C * C - 1) / D;  // a1
+		outA[1] = (1 - sqrt(2) * C + C * C) / D;  // a2
+	}
 #endif
 
-    void setup() {
+	void setup() {
 #ifndef REST_DETECTION_DISABLE_LPF
-        assert(gyrTs > 0);
-        assert(accTs > 0);
+		assert(gyrTs > 0);
+		assert(accTs > 0);
 
-        filterCoeffs(params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA);
-        filterCoeffs(params.restFilterTau, accTs, restAccLpB, restAccLpA);
+		filterCoeffs(params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA);
+		filterCoeffs(params.restFilterTau, accTs, restAccLpB, restAccLpA);
 
-        resetState();
+		resetState();
 #endif
-    }
-
+	}
 
 private:
-    RestDetectionParams params;
-    bool restDetected;
-    sensor_real_t restTime;
-    sensor_real_t gyrLastSquaredDeviation = 0;
-    sensor_real_t accLastSquaredDeviation = 0;
+	RestDetectionParams params;
+	bool restDetected;
+	sensor_real_t restTime;
+	sensor_real_t gyrLastSquaredDeviation = 0;
+	sensor_real_t accLastSquaredDeviation = 0;
 
-    sensor_real_t gyrTs;
-    sensor_real_t accTs;
+	sensor_real_t gyrTs;
+	sensor_real_t accTs;
 #ifndef REST_DETECTION_DISABLE_LPF
-    sensor_real_t restLastGyrLp[3];
-    double restGyrLpState[3*2];
-    double restGyrLpB[3];
-    double restGyrLpA[2];
-    sensor_real_t restLastAccLp[3];
-    double restAccLpState[3*2];
-    double restAccLpB[3];
-    double restAccLpA[2];
+	sensor_real_t restLastGyrLp[3];
+	double restGyrLpState[3 * 2];
+	double restGyrLpB[3];
+	double restGyrLpA[2];
+	sensor_real_t restLastAccLp[3];
+	double restAccLpState[3 * 2];
+	double restAccLpB[3];
+	double restAccLpA[2];
 #else
-    struct {
-        float gyr[3];
-        float acc[3];
-    } lastSample;
+	struct {
+		float gyr[3];
+		float acc[3];
+	} lastSample;
 #endif
 };
 
-
 #endif

src/motionprocessing/types.h

@@ -2,10 +2,10 @@
 #define MOTIONPROCESSING_TYPES_H
 
 #if SENSORS_DOUBLE_PRECISION
-    typedef double sensor_real_t;
+typedef double sensor_real_t;
 #else
-    typedef float sensor_real_t;
-    #define VQF_SINGLE_PRECISION
+typedef float sensor_real_t;
+#define VQF_SINGLE_PRECISION
 #endif
 
 #endif

src/network/connection.cpp

@@ -23,6 +23,8 @@
 
 #include "connection.h"
 
+#include <string_view>
+
 #include "GlobalVars.h"
 #include "logging/Logger.h"
 #include "packets.h"
@@ -30,40 +32,14 @@
 #define TIMEOUT 3000UL
 
 template <typename T>
-unsigned char* convert_to_chars(T src, unsigned char* target) {
-	union uwunion {
-		unsigned char c[sizeof(T)];
-		T v;
-	} un;
-	un.v = src;
-	for (size_t i = 0; i < sizeof(T); i++) {
-		target[i] = un.c[sizeof(T) - i - 1];
-	}
+uint8_t* convert_to_chars(T src, uint8_t* target) {
+	auto* rawBytes = reinterpret_cast<uint8_t*>(&src);
+	std::memcpy(target, rawBytes, sizeof(T));
+	std::reverse(target, target + sizeof(T));
 	return target;
 }
 
-template <typename T>
-T convert_chars(unsigned char* const src) {
-	union uwunion {
-		unsigned char c[sizeof(T)];
-		T v;
-	} un;
-	for (size_t i = 0; i < sizeof(T); i++) {
-		un.c[i] = src[sizeof(T) - i - 1];
-	}
-	return un.v;
-}
-
-namespace SlimeVR {
-namespace Network {
-
-#define MUST_TRANSFER_BOOL(b) \
-	if (!b)                   \
-		return false;
-
-#define MUST(b) \
-	if (!b)     \
-		return;
+namespace SlimeVR::Network {
 
 bool Connection::beginPacket() {
 	if (m_IsBundle) {
@@ -91,7 +67,7 @@ bool Connection::endPacket() {
 		m_IsBundle = false;
 
 		if (m_BundlePacketInnerCount == 0) {
-			sendPacketType(PACKET_BUNDLE);
+			sendPacketType(SendPacketType::Bundle);
 			sendPacketNumber();
 		}
 		sendShort(innerPacketSize);
@@ -134,7 +110,7 @@ bool Connection::endBundle() {
 	return endPacket();
 }
 
-size_t Connection::write(const uint8_t *buffer, size_t size) {
+size_t Connection::write(const uint8_t* buffer, size_t size) {
 	if (m_IsBundle) {
 		if (m_BundlePacketPosition + size > sizeof(m_Packet)) {
 			return 0;
@@ -146,9 +122,7 @@ size_t Connection::write(const uint8_t *buffer, size_t size) {
 	return m_UDP.write(buffer, size);
 }
 
-size_t Connection::write(uint8_t byte) {
-	return write(&byte, 1);
-}
+size_t Connection::write(uint8_t byte) { return write(&byte, 1); }
 
 bool Connection::sendFloat(float f) {
 	convert_to_chars(f, m_Buf);
@@ -193,18 +167,22 @@ bool Connection::sendPacketNumber() {
 bool Connection::sendShortString(const char* str) {
 	uint8_t size = strlen(str);
 
+	assert(size <= 255);
+
 	MUST_TRANSFER_BOOL(sendByte(size));
-	MUST_TRANSFER_BOOL(sendBytes((const uint8_t*)str, size));
+	if (size > 0) {
+		MUST_TRANSFER_BOOL(sendBytes((const uint8_t*)str, size));
+	}
 
 	return true;
 }
 
-bool Connection::sendPacketType(uint8_t type) {
+bool Connection::sendPacketType(SendPacketType type) {
 	MUST_TRANSFER_BOOL(sendByte(0));
 	MUST_TRANSFER_BOOL(sendByte(0));
 	MUST_TRANSFER_BOOL(sendByte(0));
 
-	return sendByte(type);
+	return sendByte(static_cast<uint8_t>(type));
 }
 
 bool Connection::sendLongString(const char* str) {
@@ -220,87 +198,77 @@ int Connection::getWriteError() { return m_UDP.getWriteError(); }
 // PACKET_HEARTBEAT 0
 void Connection::sendHeartbeat() {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_HEARTBEAT));
-	MUST(sendPacketNumber());
-
-	MUST(endPacket());
+	MUST(sendPacketCallback(SendPacketType::HeartBeat, []() { return true; }));
 }
 
 // PACKET_ACCEL 4
 void Connection::sendSensorAcceleration(uint8_t sensorId, Vector3 vector) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ACCEL));
-	MUST(sendPacketNumber());
-	MUST(sendFloat(vector.x));
-	MUST(sendFloat(vector.y));
-	MUST(sendFloat(vector.z));
-	MUST(sendByte(sensorId));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::Accel,
+		AccelPacket{
+			.x = vector.x,
+			.y = vector.y,
+			.z = vector.z,
+			.sensorId = sensorId,
+		}
+	));
 }
 
 // PACKET_BATTERY_LEVEL 12
 void Connection::sendBatteryLevel(float batteryVoltage, float batteryPercentage) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_BATTERY_LEVEL));
-	MUST(sendPacketNumber());
-	MUST(sendFloat(batteryVoltage));
-	MUST(sendFloat(batteryPercentage));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::BatteryLevel,
+		BatteryLevelPacket{
+			.batteryVoltage = batteryVoltage,
+			.batteryPercentage = batteryPercentage,
+		}
+	));
 }
 
 // PACKET_TAP 13
 void Connection::sendSensorTap(uint8_t sensorId, uint8_t value) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_TAP));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendByte(value));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::Tap,
+		TapPacket{
+			.sensorId = sensorId,
+			.value = value,
+		}
+	));
 }
 
 // PACKET_ERROR 14
 void Connection::sendSensorError(uint8_t sensorId, uint8_t error) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ERROR));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendByte(error));
-
-	MUST(endPacket());
+	sendPacket(
+		SendPacketType::Error,
+		ErrorPacket{
+			.sensorId = sensorId,
+			.error = error,
+		}
+	);
 }
 
 // PACKET_SENSOR_INFO 15
 void Connection::sendSensorInfo(Sensor& sensor) {
 	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::SensorInfo,
+		SensorInfoPacket{
+			.sensorId = sensor.getSensorId(),
+			.sensorState = sensor.getSensorState(),
+			.sensorType = sensor.getSensorType(),
+			.sensorConfigData = sensor.getSensorConfigData(),
+			.hasCompletedRestCalibration = sensor.hasCompletedRestCalibration(),
+			.sensorPosition = sensor.getSensorPosition(),
+			.sensorDataType = sensor.getDataType(),
 
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_SENSOR_INFO));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensor.getSensorId()));
-	MUST(sendByte(static_cast<uint8_t>(sensor.getSensorState())));
-	MUST(sendByte(static_cast<uint8_t>(sensor.getSensorType())));
-	MUST(sendShort(sensor.getSensorConfigData()));
-
-	MUST(endPacket());
+			.tpsCounterAveragedTps = sensor.m_tpsCounter.getAveragedTPS(),
+			.dataCounterAveragedTps = sensor.m_dataCounter.getAveragedTPS(),
+		}
+	));
 }
 
 // PACKET_ROTATION_DATA 17
@@ -311,118 +279,132 @@ void Connection::sendRotationData(
 	uint8_t accuracyInfo
 ) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ROTATION_DATA));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendByte(dataType));
-	MUST(sendFloat(quaternion->x));
-	MUST(sendFloat(quaternion->y));
-	MUST(sendFloat(quaternion->z));
-	MUST(sendFloat(quaternion->w));
-	MUST(sendByte(accuracyInfo));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::RotationData,
+		RotationDataPacket{
+			.sensorId = sensorId,
+			.dataType = dataType,
+			.x = quaternion->x,
+			.y = quaternion->y,
+			.z = quaternion->z,
+			.w = quaternion->w,
+			.accuracyInfo = accuracyInfo,
+		}
+	));
 }
 
 // PACKET_MAGNETOMETER_ACCURACY 18
 void Connection::sendMagnetometerAccuracy(uint8_t sensorId, float accuracyInfo) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_MAGNETOMETER_ACCURACY));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendFloat(accuracyInfo));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::MagnetometerAccuracy,
+		MagnetometerAccuracyPacket{
+			.sensorId = sensorId,
+			.accuracyInfo = accuracyInfo,
+		}
+	));
 }
 
 // PACKET_SIGNAL_STRENGTH 19
 void Connection::sendSignalStrength(uint8_t signalStrength) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_SIGNAL_STRENGTH));
-	MUST(sendPacketNumber());
-	MUST(sendByte(255));
-	MUST(sendByte(signalStrength));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::SignalStrength,
+		SignalStrengthPacket{
+			.sensorId = 255,
+			.signalStrength = signalStrength,
+		}
+	));
 }
 
 // PACKET_TEMPERATURE 20
 void Connection::sendTemperature(uint8_t sensorId, float temperature) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_TEMPERATURE));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendFloat(temperature));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::Temperature,
+		TemperaturePacket{
+			.sensorId = sensorId,
+			.temperature = temperature,
+		}
+	));
 }
 
 // PACKET_FEATURE_FLAGS 22
 void Connection::sendFeatureFlags() {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_FEATURE_FLAGS));
-	MUST(sendPacketNumber());
-	MUST(write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size()));
-
-	MUST(endPacket());
+	sendPacketCallback(SendPacketType::FeatureFlags, [&]() {
+		return write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size());
+	});
 }
 
 // PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24
 
-void Connection::sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType) {
+void Connection::sendAcknowledgeConfigChange(
+	uint8_t sensorId,
+	SensorToggles configType
+) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ACKNOWLEDGE_CONFIG_CHANGE));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendShort(configType));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::AcknowledgeConfigChange,
+		AcknowledgeConfigChangePacket{
+			.sensorId = sensorId,
+			.configType = configType,
+		}
+	));
 }
 
 void Connection::sendTrackerDiscovery() {
 	MUST(!m_Connected);
+	MUST(sendPacketCallback(
+		SendPacketType::Handshake,
+		[&]() {
+			uint8_t mac[6];
+			WiFi.macAddress(mac);
 
-	uint8_t mac[6];
-	WiFi.macAddress(mac);
+			MUST_TRANSFER_BOOL(sendInt(BOARD));
+			// This is kept for backwards compatibility,
+			// but the latest SlimeVR server will not initialize trackers
+			// with firmware build > 8 until it recieves a sensor info packet
+			MUST_TRANSFER_BOOL(sendInt(static_cast<int>(sensorManager.getSensorType(0)))
+			);
+			MUST_TRANSFER_BOOL(sendInt(HARDWARE_MCU));
+			// Backwards compatibility, unused IMU data
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(PROTOCOL_VERSION));
+			MUST_TRANSFER_BOOL(sendShortString(FIRMWARE_VERSION));
+			// MAC address string
+			MUST_TRANSFER_BOOL(sendBytes(mac, 6));
+			// Tracker type to hint the server if it's a glove or normal tracker or
+			// something else
+			MUST_TRANSFER_BOOL(sendByte(static_cast<uint8_t>(TRACKER_TYPE)));
+			static_assert(std::string_view{VENDOR_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(VENDOR_NAME));
+			static_assert(std::string_view{VENDOR_URL}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(VENDOR_URL));
+			static_assert(std::string_view{PRODUCT_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(PRODUCT_NAME));
+			static_assert(std::string_view{UPDATE_ADDRESS}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(UPDATE_ADDRESS));
+			static_assert(std::string_view{UPDATE_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(UPDATE_NAME));
+			return true;
+		},
+		0
+	));
+}
 
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_HANDSHAKE));
-	// Packet number is always 0 for handshake
-	MUST(sendLong(0));
-	MUST(sendInt(BOARD));
-	// This is kept for backwards compatibility,
-	// but the latest SlimeVR server will not initialize trackers
-	// with firmware build > 8 until it recieves a sensor info packet
-	MUST(sendInt(static_cast<int>(sensorManager.getSensorType(0))));
-	MUST(sendInt(HARDWARE_MCU));
-	MUST(sendInt(0));
-	MUST(sendInt(0));
-	MUST(sendInt(0));
-	MUST(sendInt(PROTOCOL_VERSION));
-	MUST(sendShortString(FIRMWARE_VERSION));
-	// MAC address string
-	MUST(sendBytes(mac, 6));
-
-	MUST(endPacket());
+// PACKET_FLEX_DATA 24
+void Connection::sendFlexData(uint8_t sensorId, float flexLevel) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::FlexData,
+		FlexDataPacket{
+			.sensorId = sensorId,
+			.flexLevel = flexLevel,
+		}
+	));
 }
 
 #if ENABLE_INSPECTION
@@ -442,33 +424,29 @@ void Connection::sendInspectionRawIMUData(
 	uint8_t mA
 ) {
 	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::Inspection,
+		IntRawImuDataInspectionPacket{
+			.inspectionPacketType = InspectionPacketType::RawImuData,
+			.sensorId = sensorId,
+			.inspectionDataType = InspectionDataType::Int,
 
-	MUST(beginPacket());
+			.rX = static_cast<uint32_t>(rX),
+			.rY = static_cast<uint32_t>(rY),
+			.rZ = static_cast<uint32_t>(rZ),
+			.rA = rA,
 
-	MUST(sendPacketType(PACKET_INSPECTION));
-	MUST(sendPacketNumber());
+			.aX = static_cast<uint32_t>(aX),
+			.aY = static_cast<uint32_t>(aY),
+			.aZ = static_cast<uint32_t>(aZ),
+			.aA = aA,
 
-	MUST(sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA));
-
-	MUST(sendByte(sensorId));
-	MUST(sendByte(PACKET_INSPECTION_DATATYPE_INT));
-
-	MUST(sendInt(rX));
-	MUST(sendInt(rY));
-	MUST(sendInt(rZ));
-	MUST(sendByte(rA));
-
-	MUST(sendInt(aX));
-	MUST(sendInt(aY));
-	MUST(sendInt(aZ));
-	MUST(sendByte(aA));
-
-	MUST(sendInt(mX));
-	MUST(sendInt(mY));
-	MUST(sendInt(mZ));
-	MUST(sendByte(mA));
-
-	MUST(endPacket());
+			.mX = static_cast<uint32_t>(mX),
+			.mY = static_cast<uint32_t>(mY),
+			.mZ = static_cast<uint32_t>(mZ),
+			.mA = mA,
+		}
+	))
 }
 
 void Connection::sendInspectionRawIMUData(
@@ -487,33 +465,29 @@ void Connection::sendInspectionRawIMUData(
 	uint8_t mA
 ) {
 	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::Inspection,
+		FloatRawImuDataInspectionPacket{
+			.inspectionPacketType = InspectionPacketType::RawImuData,
+			.sensorId = sensorId,
+			.inspectionDataType = InspectionDataType::Float,
 
-	MUST(beginPacket());
+			.rX = rX,
+			.rY = rY,
+			.rZ = rZ,
+			.rA = rA,
 
-	MUST(sendPacketType(PACKET_INSPECTION));
-	MUST(sendPacketNumber());
+			.aX = aX,
+			.aY = aY,
+			.aZ = aZ,
+			.aA = aA,
 
-	MUST(sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA));
-
-	MUST(sendByte(sensorId));
-	MUST(sendByte(PACKET_INSPECTION_DATATYPE_FLOAT));
-
-	MUST(sendFloat(rX));
-	MUST(sendFloat(rY));
-	MUST(sendFloat(rZ));
-	MUST(sendByte(rA));
-
-	MUST(sendFloat(aX));
-	MUST(sendFloat(aY));
-	MUST(sendFloat(aZ));
-	MUST(sendByte(aA));
-
-	MUST(sendFloat(mX));
-	MUST(sendFloat(mY));
-	MUST(sendFloat(mZ));
-	MUST(sendByte(mA));
-
-	MUST(endPacket());
+			.mX = mX,
+			.mY = mY,
+			.mZ = mZ,
+			.mA = mA,
+		}
+	));
 }
 #endif
 
@@ -527,7 +501,7 @@ void Connection::returnLastPacket(int len) {
 	MUST(endPacket());
 }
 
-void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>> & sensors) {
+void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors) {
 	if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
 		return;
 	}
@@ -535,7 +509,7 @@ void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>> & sensor
 	m_LastSensorInfoPacketTimestamp = millis();
 
 	for (int i = 0; i < (int)sensors.size(); i++) {
-		if (m_AckedSensorState[i] != sensors[i]->getSensorState()) {
+		if (isSensorStateUpdated(i, sensors[i])) {
 			sendSensorInfo(*sensors[i]);
 		}
 	}
@@ -555,6 +529,14 @@ void Connection::maybeRequestFeatureFlags() {
 	m_FeatureFlagsRequestAttempts++;
 }
 
+bool Connection::isSensorStateUpdated(int i, std::unique_ptr<Sensor>& sensor) {
+	return (m_AckedSensorState[i] != sensor->getSensorState()
+			|| m_AckedSensorCalibration[i] != sensor->hasCompletedRestCalibration()
+			|| m_AckedSensorConfigData[i] != sensor->getSensorConfigData())
+		&& sensor->getSensorType() != SensorTypeID::Unknown
+		&& sensor->getSensorType() != SensorTypeID::Empty;
+}
+
 void Connection::searchForServer() {
 	while (true) {
 		int packetSize = m_UDP.parsePacket();
@@ -573,13 +555,12 @@ void Connection::searchForServer() {
 			m_UDP.remotePort()
 		);
 		m_Logger.traceArray("UDP packet contents: ", m_Packet, len);
-#else
-		(void)len;
 #endif
 
 		// Handshake is different, it has 3 in the first byte, not the 4th, and data
 		// starts right after
-		if (m_Packet[0] == PACKET_HANDSHAKE) {
+		if (static_cast<ReceivePacketType>(m_Packet[0])
+			== ReceivePacketType::Handshake) {
 			if (strncmp((char*)m_Packet + 1, "Hey OVR =D 5", 12) != 0) {
 				m_Logger.error("Received invalid handshake packet");
 				continue;
@@ -591,7 +572,7 @@ void Connection::searchForServer() {
 			m_Connected = true;
 
 			m_FeatureFlagsRequestAttempts = 0;
-			m_ServerFeatures = ServerFeatures { };
+			m_ServerFeatures = ServerFeatures{};
 
 			statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
 			ledManager.off();
@@ -621,7 +602,21 @@ void Connection::searchForServer() {
 
 void Connection::reset() {
 	m_Connected = false;
-	std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
+	std::fill(
+		m_AckedSensorState,
+		m_AckedSensorState + MAX_SENSORS_COUNT,
+		SensorStatus::SENSOR_OFFLINE
+	);
+	std::fill(
+		m_AckedSensorCalibration,
+		m_AckedSensorCalibration + MAX_SENSORS_COUNT,
+		false
+	);
+	std::fill(
+		m_AckedSensorConfigData,
+		m_AckedSensorConfigData + MAX_SENSORS_COUNT,
+		SlimeVR::Configuration::SensorConfigBits{}
+	);
 
 	m_UDP.begin(m_ServerPort);
 
@@ -629,21 +624,30 @@ void Connection::reset() {
 }
 
 void Connection::update() {
-	auto & sensors = sensorManager.getSensors();
-
-	updateSensorState(sensors);
-	maybeRequestFeatureFlags();
-
 	if (!m_Connected) {
 		searchForServer();
 		return;
 	}
 
+	auto& sensors = sensorManager.getSensors();
+
+	updateSensorState(sensors);
+	maybeRequestFeatureFlags();
+
 	if (m_LastPacketTimestamp + TIMEOUT < millis()) {
 		statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
 
 		m_Connected = false;
-		std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
+		std::fill(
+			m_AckedSensorState,
+			m_AckedSensorState + MAX_SENSORS_COUNT,
+			SensorStatus::SENSOR_OFFLINE
+		);
+		std::fill(
+			m_AckedSensorCalibration,
+			m_AckedSensorCalibration + MAX_SENSORS_COUNT,
+			false
+		);
 		m_Logger.warn("Connection to server timed out");
 
 		return;
@@ -669,45 +673,56 @@ void Connection::update() {
 	(void)packetSize;
 #endif
 
-	switch (convert_chars<int>(m_Packet)) {
-		case PACKET_RECEIVE_HEARTBEAT:
+	switch (static_cast<ReceivePacketType>(m_Packet[3])) {
+		case ReceivePacketType::HeartBeat:
 			sendHeartbeat();
 			break;
 
-		case PACKET_RECEIVE_VIBRATE:
+		case ReceivePacketType::Vibrate:
 			break;
 
-		case PACKET_RECEIVE_HANDSHAKE:
+		case ReceivePacketType::Handshake:
 			// Assume handshake successful
 			m_Logger.warn("Handshake received again, ignoring");
 			break;
 
-		case PACKET_RECEIVE_COMMAND:
+		case ReceivePacketType::Command:
 			break;
 
-		case PACKET_CONFIG:
+		case ReceivePacketType::Config:
 			break;
 
-		case PACKET_PING_PONG:
+		case ReceivePacketType::PingPong:
 			returnLastPacket(len);
 			break;
 
-		case PACKET_SENSOR_INFO:
+		case ReceivePacketType::SensorInfo: {
 			if (len < 6) {
 				m_Logger.warn("Wrong sensor info packet");
 				break;
 			}
 
+			SensorInfoPacket sensorInfoPacket;
+			memcpy(&sensorInfoPacket, m_Packet + 4, sizeof(sensorInfoPacket));
+
 			for (int i = 0; i < (int)sensors.size(); i++) {
-				if (m_Packet[4] == sensors[i]->getSensorId()) {
-					m_AckedSensorState[i] = (SensorStatus)m_Packet[5];
+				if (sensorInfoPacket.sensorId == sensors[i]->getSensorId()) {
+					m_AckedSensorState[i] = sensorInfoPacket.sensorState;
+					if (len < 12) {
+						m_AckedSensorCalibration[i]
+							= sensors[i]->hasCompletedRestCalibration();
+						m_AckedSensorConfigData[i] = sensors[i]->getSensorConfigData();
+						break;
+					}
+					m_AckedSensorCalibration[i]
+						= sensorInfoPacket.hasCompletedRestCalibration;
 					break;
 				}
 			}
 
 			break;
-
-		case PACKET_FEATURE_FLAGS: {
+		}
+		case ReceivePacketType::FeatureFlags: {
 			// Packet type (4) + Packet number (8) + flags (len - 12)
 			if (len < 13) {
 				m_Logger.warn("Invalid feature flags packet: too short");
@@ -720,45 +735,52 @@ void Connection::update() {
 			m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength);
 
 			if (!hadFlags) {
-				#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
-					if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) {
-						m_Logger.debug("Server supports packet bundling");
-					}
-				#endif
+#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
+				if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) {
+					m_Logger.debug("Server supports packet bundling");
+				}
+#endif
 			}
 
 			break;
 		}
 
-		case PACKET_SET_CONFIG_FLAG: {
-			// Packet type (4) + Packet number (8) + sensor_id(1) + flag_id (2) + state (1)
+		case ReceivePacketType::SetConfigFlag: {
+			// Packet type (4) + Packet number (8) + sensor_id(1) + flag_id (2) + state
+			// (1)
 			if (len < 16) {
 				m_Logger.warn("Invalid sensor config flag packet: too short");
 				break;
 			}
-			uint8_t sensorId = m_Packet[12];
-			uint16_t flagId = m_Packet[13] << 8 | m_Packet[14];
-			bool newState = m_Packet[15] > 0;
-			if(sensorId == UINT8_MAX) {
+
+			SetConfigFlagPacket setConfigFlagPacket;
+			memcpy(&setConfigFlagPacket, m_Packet + 12, sizeof(SetConfigFlagPacket));
+
+			uint8_t sensorId = setConfigFlagPacket.sensorId;
+			SensorToggles flag = setConfigFlagPacket.flag;
+			bool newState = setConfigFlagPacket.newState;
+			if (sensorId == UINT8_MAX) {
 				for (auto& sensor : sensors) {
-					sensor->setFlag(flagId, newState);
+					sensor->setFlag(flag, newState);
 				}
 			} else {
-				auto & sensors = sensorManager.getSensors();
-				if(sensorId < sensors.size()) {
-					auto& sensor = sensors[sensorId];
-					sensor->setFlag(flagId, newState);
-				} else {
-					m_Logger.warn("Invalid sensor config flag packet: invalid sensor id");
+				auto& sensors = sensorManager.getSensors();
+
+				if (sensorId >= sensors.size()) {
+					m_Logger.warn(
+						"Invalid sensor config flag packet: invalid sensor id"
+					);
 					break;
 				}
+
+				auto& sensor = sensors[sensorId];
+				sensor->setFlag(flag, newState);
 			}
-			sendAcknowledgeConfigChange(sensorId, flagId);
+			sendAcknowledgeConfigChange(sensorId, flag);
 			configuration.save();
 			break;
 		}
 	}
 }
 
-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network

src/network/connection.h

@@ -26,14 +26,25 @@
 #include <Arduino.h>
 #include <WiFiUdp.h>
 
+#include <optional>
+
+#include "../configuration/SensorConfig.h"
+#include "featureflags.h"
 #include "globals.h"
+#include "packets.h"
 #include "quat.h"
 #include "sensors/sensor.h"
 #include "wifihandler.h"
-#include "featureflags.h"
 
-namespace SlimeVR {
-namespace Network {
+namespace SlimeVR::Network {
+
+#define MUST_TRANSFER_BOOL(b) \
+	if (!(b))                 \
+		return false;
+
+#define MUST(b) \
+	if (!(b))   \
+		return;
 
 class Connection {
 public:
@@ -84,6 +95,9 @@ public:
 	// PACKET_FEATURE_FLAGS 22
 	void sendFeatureFlags();
 
+	// PACKET_FLEX_DATA 26
+	void sendFlexData(uint8_t sensorId, float flexLevel);
+
 #if ENABLE_INSPECTION
 	void sendInspectionRawIMUData(
 		uint8_t sensorId,
@@ -117,24 +131,23 @@ public:
 	);
 #endif
 
-	const ServerFeatures& getServerFeatureFlags() {
-		return m_ServerFeatures;
-	}
+	const ServerFeatures& getServerFeatureFlags() { return m_ServerFeatures; }
 
 	bool beginBundle();
 	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 beginPacket();
 	bool endPacket();
 
-	size_t write(const uint8_t *buffer, size_t size);
+	size_t write(const uint8_t* buffer, size_t size);
 	size_t write(uint8_t byte);
 
-	bool sendPacketType(uint8_t type);
+	bool sendPacketType(SendPacketType type);
 	bool sendPacketNumber();
 	bool sendFloat(float f);
 	bool sendByte(uint8_t c);
@@ -145,6 +158,46 @@ private:
 	bool sendShortString(const char* str);
 	bool sendLongString(const char* str);
 
+	template <typename Packet>
+	bool sendPacket(
+		SendPacketType type,
+		Packet packet,
+		std::optional<uint64_t> packetNumberOverride = std::nullopt
+	) {
+		MUST_TRANSFER_BOOL(beginPacket());
+		MUST_TRANSFER_BOOL(sendPacketType(type));
+		if (packetNumberOverride) {
+			MUST_TRANSFER_BOOL(sendLong(*packetNumberOverride));
+		} else {
+			MUST_TRANSFER_BOOL(sendPacketNumber());
+		}
+
+		MUST_TRANSFER_BOOL(
+			sendBytes(reinterpret_cast<uint8_t*>(&packet), sizeof(Packet))
+		);
+
+		return endPacket();
+	}
+
+	template <typename Callback>
+	bool sendPacketCallback(
+		SendPacketType type,
+		Callback bodyCallback,
+		std::optional<uint64_t> packetNumberOverride = std::nullopt
+	) {
+		MUST_TRANSFER_BOOL(beginPacket());
+		MUST_TRANSFER_BOOL(sendPacketType(type));
+		if (packetNumberOverride) {
+			MUST_TRANSFER_BOOL(sendLong(*packetNumberOverride));
+		} else {
+			MUST_TRANSFER_BOOL(sendPacketNumber());
+		}
+
+		MUST_TRANSFER_BOOL(bodyCallback());
+
+		return endPacket();
+	}
+
 	int getWriteError();
 
 	void returnLastPacket(int len);
@@ -156,9 +209,9 @@ private:
 	void sendTrackerDiscovery();
 
 	// PACKET_SENSOR_INFO 15
-	void sendSensorInfo(Sensor& sensor);
+	void sendSensorInfo(::Sensor& sensor);
 
-	void sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType);
+	void sendAcknowledgeConfigChange(uint8_t sensorId, SensorToggles configType);
 
 	bool m_Connected = false;
 	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection");
@@ -172,7 +225,10 @@ private:
 	unsigned long m_LastConnectionAttemptTimestamp;
 	unsigned long m_LastPacketTimestamp;
 
-	SensorStatus m_AckedSensorState[MAX_IMU_COUNT] = {SensorStatus::SENSOR_OFFLINE};
+	SensorStatus m_AckedSensorState[MAX_SENSORS_COUNT] = {SensorStatus::SENSOR_OFFLINE};
+	SlimeVR::Configuration::SensorConfigBits m_AckedSensorConfigData[MAX_SENSORS_COUNT]
+		= {};
+	bool m_AckedSensorCalibration[MAX_SENSORS_COUNT] = {false};
 	unsigned long m_LastSensorInfoPacketTimestamp = 0;
 
 	uint8_t m_FeatureFlagsRequestAttempts = 0;
@@ -186,7 +242,6 @@ private:
 	unsigned char m_Buf[8];
 };
 
-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network
 
 #endif  // SLIMEVR_NETWORK_CONNECTION_H_

src/network/featureflags.h

@@ -24,17 +24,17 @@
 #ifndef SLIMEVR_FEATURE_FLAGS_H_
 #define SLIMEVR_FEATURE_FLAGS_H_
 
-#include <cstring>
 #include <algorithm>
+#include <cstring>
 
 /**
  * Bit packed flags, enum values start with 0 and indicate which bit it is.
  *
  * Change the enums and `flagsEnabled` inside to extend.
-*/
+ */
 struct ServerFeatures {
 public:
-	enum EServerFeatureFlags: uint32_t {
+	enum EServerFeatureFlags : uint32_t {
 		// Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64).
 		PROTOCOL_BUNDLE_SUPPORT,
 
@@ -51,15 +51,17 @@ public:
 	/**
 	 * Whether the server supports the "feature flags" feature,
 	 * set to true when we've received flags packet from the server.
-	*/
-	bool isAvailable() {
-		return m_Available;
-	}
+	 */
+	bool isAvailable() { return m_Available; }
 
 	static ServerFeatures from(uint8_t* received, uint32_t length) {
 		ServerFeatures res;
 		res.m_Available = true;
-		memcpy(res.m_Flags, received, std::min(static_cast<uint32_t>(sizeof(res.m_Flags)), length));
+		memcpy(
+			res.m_Flags,
+			received,
+			std::min(static_cast<uint32_t>(sizeof(res.m_Flags)), length)
+		);
 		return res;
 	}
 
@@ -71,7 +73,7 @@ private:
 
 class FirmwareFeatures {
 public:
-	enum EFirmwareFeatureFlags: uint32_t {
+	enum EFirmwareFeatureFlags : uint32_t {
 		// EXAMPLE_FEATURE,
 		B64_WIFI_SCANNING = 1,
 		SENSOR_CONFIG = 2,
@@ -88,7 +90,7 @@ public:
 		// Add enabled flags here
 	};
 
-	static constexpr auto flags = []{
+	static constexpr auto flags = [] {
 		constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1;
 		std::array<uint8_t, flagsLength> packed{};
 

src/network/manager.cpp

@@ -24,8 +24,7 @@
 
 #include "GlobalVars.h"
 
-namespace SlimeVR {
-namespace Network {
+namespace SlimeVR::Network {
 
 void Manager::setup() { ::WiFiNetwork::setUp(); }
 
@@ -48,5 +47,4 @@ void Manager::update() {
 	networkConnection.update();
 }
 
-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network

src/network/manager.h

@@ -28,8 +28,7 @@
 #include "wifihandler.h"
 #include "wifiprovisioning.h"
 
-namespace SlimeVR {
-namespace Network {
+namespace SlimeVR::Network {
 
 class Manager {
 public:
@@ -40,7 +39,6 @@ private:
 	bool m_IsConnected = false;
 };
 
-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network
 
 #endif  // SLIMEVR_NETWORK_MANAGER_H_

src/network/packets.h

@@ -24,46 +24,212 @@
 #ifndef SLIMEVR_PACKETS_H_
 #define SLIMEVR_PACKETS_H_
 
-#define PACKET_HEARTBEAT 0
-// #define PACKET_ROTATION 1 // Deprecated
-// #define PACKET_GYRO 2 // Deprecated
-#define PACKET_HANDSHAKE 3
-#define PACKET_ACCEL 4
-// #define PACKET_MAG 5 // Deprecated
-// #define PACKET_RAW_CALIBRATION_DATA 6 // Deprecated
-// #define PACKET_CALIBRATION_FINISHED 7 // Deprecated
-#define PACKET_CONFIG 8
-// #define PACKET_RAW_MAGNETOMETER 9 // Deprecated
-#define PACKET_PING_PONG 10
-#define PACKET_SERIAL 11
-#define PACKET_BATTERY_LEVEL 12
-#define PACKET_TAP 13
-#define PACKET_ERROR 14
-#define PACKET_SENSOR_INFO 15
-// #define PACKET_ROTATION_2 16 // Deprecated
-#define PACKET_ROTATION_DATA 17
-#define PACKET_MAGNETOMETER_ACCURACY 18
-#define PACKET_SIGNAL_STRENGTH 19
-#define PACKET_TEMPERATURE 20
-// #define PACKET_USER_ACTION 21 // Joycon buttons only currently
-#define PACKET_FEATURE_FLAGS 22
-// #define PACKET_ROTATION_ACCELERATION 23 // Unification of rot and accel data in one packet
-#define PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24
-#define PACKET_SET_CONFIG_FLAG 25
+#include <cstdint>
 
-#define PACKET_BUNDLE 100
+#include "../consts.h"
+#include "../sensors/sensor.h"
 
-#define PACKET_INSPECTION 105  // 0x69
+enum class SendPacketType : uint8_t {
+	HeartBeat = 0,
+	//  Rotation = 1,
+	//  Gyro = 2,
+	Handshake = 3,
+	Accel = 4,
+	// Mag = 5,
+	// RawCalibrationData = 6,
+	// CalibrationFinished = 7,
+	// RawMagnetometer = 9,
+	Serial = 11,
+	BatteryLevel = 12,
+	Tap = 13,
+	Error = 14,
+	SensorInfo = 15,
+	// Rotation2 = 16,
+	RotationData = 17,
+	MagnetometerAccuracy = 18,
+	SignalStrength = 19,
+	Temperature = 20,
+	// UserAction = 21,
+	FeatureFlags = 22,
+	// RotationAcceleration = 23,
+	AcknowledgeConfigChange = 24,
+	FlexData = 26,
+	Bundle = 100,
+	Inspection = 105,
+};
 
-#define PACKET_RECEIVE_HEARTBEAT 1
-#define PACKET_RECEIVE_VIBRATE 2
-#define PACKET_RECEIVE_HANDSHAKE 3
-#define PACKET_RECEIVE_COMMAND 4
+enum class ReceivePacketType : uint8_t {
+	HeartBeat = 1,
+	Vibrate = 2,
+	Handshake = 3,
+	Command = 4,
+	Config = 8,
+	PingPong = 10,
+	SensorInfo = 15,
+	FeatureFlags = 22,
+	SetConfigFlag = 25,
+};
 
-#define PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA 1
-#define PACKET_INSPECTION_PACKETTYPE_FUSED_IMU_DATA 2
-#define PACKET_INSPECTION_PACKETTYPE_CORRECTION_DATA 3
-#define PACKET_INSPECTION_DATATYPE_INT 1
-#define PACKET_INSPECTION_DATATYPE_FLOAT 2
+enum class InspectionPacketType : uint8_t {
+	RawImuData = 1,
+	FusedImuData = 2,
+	CorrectionData = 3,
+};
+
+enum class InspectionDataType : uint8_t {
+	Int = 1,
+	Float = 2,
+};
+
+// From the SH-2 interface that BNO08x use.
+enum class PacketErrorCode : uint8_t {
+	NOT_APPLICABLE = 0,
+	POWER_ON_RESET = 1,
+	INTERNAL_SYSTEM_RESET = 2,
+	WATCHDOG_TIMEOUT = 3,
+	EXTERNAL_RESET = 4,
+	OTHER = 5,
+};
+
+#pragma pack(push, 1)
+
+template <typename T>
+T swapEndianness(T value) {
+	auto* bytes = reinterpret_cast<uint8_t*>(&value);
+	std::reverse(bytes, bytes + sizeof(T));
+	return value;
+}
+
+template <typename T>
+struct BigEndian {
+	BigEndian() = default;
+	explicit(false) BigEndian(T val) { value = swapEndianness(val); }
+	explicit(false) operator T() const { return swapEndianness(value); }
+
+	T value{};
+};
+
+struct AccelPacket {
+	BigEndian<float> x;
+	BigEndian<float> y;
+	BigEndian<float> z;
+	uint8_t sensorId{};
+};
+
+struct BatteryLevelPacket {
+	BigEndian<float> batteryVoltage;
+	BigEndian<float> batteryPercentage;
+};
+
+struct TapPacket {
+	uint8_t sensorId;
+	uint8_t value;
+};
+
+struct ErrorPacket {
+	uint8_t sensorId;
+	uint8_t error;
+};
+
+struct SensorInfoPacket {
+	uint8_t sensorId{};
+	SensorStatus sensorState{};
+	SensorTypeID sensorType{};
+	BigEndian<SlimeVR::Configuration::SensorConfigBits> sensorConfigData{};
+	bool hasCompletedRestCalibration{};
+	SensorPosition sensorPosition{};
+	SensorDataType sensorDataType{};
+	// ADD NEW FIELDS ABOVE THIS COMMENT ^^^^^^^^
+	// WARNING! Only for debug purposes and SHOULD ALWAYS BE LAST IN THE PACKET.
+	// It WILL BE REMOVED IN THE FUTURE
+	// Send TPS
+	BigEndian<float> tpsCounterAveragedTps;
+	BigEndian<float> dataCounterAveragedTps;
+};
+
+struct RotationDataPacket {
+	uint8_t sensorId{};
+	uint8_t dataType{};
+	BigEndian<float> x;
+	BigEndian<float> y;
+	BigEndian<float> z;
+	BigEndian<float> w;
+	uint8_t accuracyInfo{};
+};
+
+struct MagnetometerAccuracyPacket {
+	uint8_t sensorId{};
+	BigEndian<float> accuracyInfo;
+};
+
+struct SignalStrengthPacket {
+	uint8_t sensorId;
+	uint8_t signalStrength;
+};
+
+struct TemperaturePacket {
+	uint8_t sensorId{};
+	BigEndian<float> temperature;
+};
+
+struct AcknowledgeConfigChangePacket {
+	uint8_t sensorId{};
+	BigEndian<SensorToggles> configType;
+};
+
+struct FlexDataPacket {
+	uint8_t sensorId{};
+	BigEndian<float> flexLevel;
+};
+
+struct IntRawImuDataInspectionPacket {
+	InspectionPacketType inspectionPacketType{};
+	uint8_t sensorId{};
+	InspectionDataType inspectionDataType{};
+
+	BigEndian<uint32_t> rX;
+	BigEndian<uint32_t> rY;
+	BigEndian<uint32_t> rZ;
+	uint8_t rA{};
+
+	BigEndian<uint32_t> aX;
+	BigEndian<uint32_t> aY;
+	BigEndian<uint32_t> aZ;
+	uint8_t aA{};
+
+	BigEndian<uint32_t> mX;
+	BigEndian<uint32_t> mY;
+	BigEndian<uint32_t> mZ;
+	uint8_t mA{};
+};
+
+struct FloatRawImuDataInspectionPacket {
+	InspectionPacketType inspectionPacketType{};
+	uint8_t sensorId{};
+	InspectionDataType inspectionDataType{};
+
+	BigEndian<float> rX;
+	BigEndian<float> rY;
+	BigEndian<float> rZ;
+	uint8_t rA{};
+
+	BigEndian<float> aX;
+	BigEndian<float> aY;
+	BigEndian<float> aZ;
+	uint8_t aA{};
+
+	BigEndian<float> mX;
+	BigEndian<float> mY;
+	BigEndian<float> mZ;
+	uint8_t mA{};
+};
+
+struct SetConfigFlagPacket {
+	uint8_t sensorId{};
+	BigEndian<SensorToggles> flag;
+	bool newState{};
+};
+
+#pragma pack(pop)
 
 #endif  // SLIMEVR_PACKETS_H_

src/network/wifihandler.cpp

@@ -1,28 +1,28 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 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:
+	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 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.
+	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 "GlobalVars.h"
 #include "globals.h"
 #include "logging/Logger.h"
-#include "GlobalVars.h"
 #if !ESP8266
 #include "esp_wifi.h"
 #endif
@@ -38,211 +38,239 @@ unsigned long last_rssi_sample = 0;
 SlimeVR::Logging::Logger wifiHandlerLogger("WiFiHandler");
 
 void reportWifiError() {
-    if(lastWifiReportTime + 1000 < millis()) {
-        lastWifiReportTime = millis();
-        Serial.print(".");
-    }
+	if (lastWifiReportTime + 1000 < millis()) {
+		lastWifiReportTime = millis();
+		Serial.print(".");
+	}
 }
 
 void setStaticIPIfDefined() {
-    #ifdef WIFI_USE_STATICIP
-    const IPAddress ip(WIFI_STATIC_IP);
-    const IPAddress gateway(WIFI_STATIC_GATEWAY);
-    const IPAddress subnet(WIFI_STATIC_SUBNET);
-    WiFi.config(ip, gateway, subnet);
-    #endif
+#ifdef WIFI_USE_STATICIP
+	const IPAddress ip(WIFI_STATIC_IP);
+	const IPAddress gateway(WIFI_STATIC_GATEWAY);
+	const IPAddress subnet(WIFI_STATIC_SUBNET);
+	WiFi.config(ip, gateway, subnet);
+#endif
 }
 
-bool WiFiNetwork::isConnected() {
-    return isWifiConnected;
+bool WiFiNetwork::isConnected() { return isWifiConnected; }
+
+void WiFiNetwork::setWiFiCredentials(const char* SSID, const char* pass) {
+	stopProvisioning();
+	setStaticIPIfDefined();
+	WiFi.begin(SSID, pass);
+	// Reset state, will get back into provisioning if can't connect
+	hadWifi = false;
+	wifiState = SLIME_WIFI_SERVER_CRED_ATTEMPT;
+	wifiConnectionTimeout = millis();
 }
 
-void WiFiNetwork::setWiFiCredentials(const char * SSID, const char * pass) {
-    stopProvisioning();
-    setStaticIPIfDefined();
-    WiFi.begin(SSID, pass);
-    // Reset state, will get back into provisioning if can't connect
-    hadWifi = false;
-    wifiState = SLIME_WIFI_SERVER_CRED_ATTEMPT;
-    wifiConnectionTimeout = millis();
-}
-
-IPAddress WiFiNetwork::getAddress() {
-    return WiFi.localIP();
-}
+IPAddress WiFiNetwork::getAddress() { return WiFi.localIP(); }
 
 void WiFiNetwork::setUp() {
-    wifiHandlerLogger.info("Setting up WiFi");
-    WiFi.persistent(true);
-    WiFi.mode(WIFI_STA);
-    #if ESP8266
-        #if USE_ATTENUATION
-            WiFi.setOutputPower(20.0 -  ATTENUATION_N);
-        #endif
-    WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-    #endif
-    WiFi.hostname("SlimeVR FBT Tracker");
-    wifiHandlerLogger.info("Loaded credentials for SSID %s and pass length %d", WiFi.SSID().c_str(), WiFi.psk().length());
-    setStaticIPIfDefined();
-    wl_status_t status = WiFi.begin(); // Should connect to last used access point, see https://arduino-esp8266.readthedocs.io/en/latest/esp8266wifi/station-class.html#begin
-    wifiHandlerLogger.debug("Status: %d", status);
-    wifiState = SLIME_WIFI_SAVED_ATTEMPT;
-    wifiConnectionTimeout = millis();
+	wifiHandlerLogger.info("Setting up WiFi");
+	WiFi.persistent(true);
+	WiFi.mode(WIFI_STA);
+#if ESP8266
+#if USE_ATTENUATION
+	WiFi.setOutputPower(20.0 - ATTENUATION_N);
+#endif
+	WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+#endif
+	WiFi.hostname("SlimeVR FBT Tracker");
+	wifiHandlerLogger.info(
+		"Loaded credentials for SSID '%s' and pass length %d",
+		WiFi.SSID().c_str(),
+		WiFi.psk().length()
+	);
+	setStaticIPIfDefined();
+	wl_status_t status = WiFi.begin(
+	);  // Should connect to last used access point, see
+		// https://arduino-esp8266.readthedocs.io/en/latest/esp8266wifi/station-class.html#begin
+	wifiHandlerLogger.debug("Status: %d", status);
+	wifiState = SLIME_WIFI_SAVED_ATTEMPT;
+	wifiConnectionTimeout = millis();
 
 #if ESP8266
 #if POWERSAVING_MODE == POWER_SAVING_NONE
-    WiFi.setSleepMode(WIFI_NONE_SLEEP);
+	WiFi.setSleepMode(WIFI_NONE_SLEEP);
 #elif POWERSAVING_MODE == POWER_SAVING_MINIMUM
-    WiFi.setSleepMode(WIFI_MODEM_SLEEP);
+	WiFi.setSleepMode(WIFI_MODEM_SLEEP);
 #elif POWERSAVING_MODE == POWER_SAVING_MODERATE
-    WiFi.setSleepMode(WIFI_MODEM_SLEEP, 10);
+	WiFi.setSleepMode(WIFI_MODEM_SLEEP, 10);
 #elif POWERSAVING_MODE == POWER_SAVING_MAXIMUM
-    WiFi.setSleepMode(WIFI_LIGHT_SLEEP, 10);
+	WiFi.setSleepMode(WIFI_LIGHT_SLEEP, 10);
 #error "MAX POWER SAVING NOT WORKING YET, please disable!"
 #endif
 #else
 #if POWERSAVING_MODE == POWER_SAVING_NONE
-    WiFi.setSleep(WIFI_PS_NONE);
+	WiFi.setSleep(WIFI_PS_NONE);
 #elif POWERSAVING_MODE == POWER_SAVING_MINIMUM
-    WiFi.setSleep(WIFI_PS_MIN_MODEM);
-#elif POWERSAVING_MODE == POWER_SAVING_MODERATE || POWERSAVING_MODE == POWER_SAVING_MAXIMUM
-    wifi_config_t conf;
-    if (esp_wifi_get_config(WIFI_IF_STA, &conf) == ESP_OK)
-    {
-        conf.sta.listen_interval = 10;
-        esp_wifi_set_config(WIFI_IF_STA, &conf);
-        WiFi.setSleep(WIFI_PS_MAX_MODEM);
-    }
-    else
-    {
-        wifiHandlerLogger.error("Unable to get WiFi config, power saving not enabled!");
-    }
+	WiFi.setSleep(WIFI_PS_MIN_MODEM);
+#elif POWERSAVING_MODE == POWER_SAVING_MODERATE \
+	|| POWERSAVING_MODE == POWER_SAVING_MAXIMUM
+	wifi_config_t conf;
+	if (esp_wifi_get_config(WIFI_IF_STA, &conf) == ESP_OK) {
+		conf.sta.listen_interval = 10;
+		esp_wifi_set_config(WIFI_IF_STA, &conf);
+		WiFi.setSleep(WIFI_PS_MAX_MODEM);
+	} else {
+		wifiHandlerLogger.error("Unable to get WiFi config, power saving not enabled!");
+	}
 #endif
 #endif
 }
 
 void onConnected() {
-    WiFiNetwork::stopProvisioning();
-    statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false);
-    isWifiConnected = true;
-    hadWifi = true;
-    wifiHandlerLogger.info("Connected successfully to SSID '%s', ip address %s", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
+	WiFiNetwork::stopProvisioning();
+	statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false);
+	isWifiConnected = true;
+	hadWifi = true;
+	wifiHandlerLogger.info(
+		"Connected successfully to SSID '%s', IP address %s",
+		WiFi.SSID().c_str(),
+		WiFi.localIP().toString().c_str()
+	);
 }
 
-uint8_t WiFiNetwork::getWiFiState() {
-    return wifiState;
-}
+uint8_t WiFiNetwork::getWiFiState() { return wifiState; }
 
 void WiFiNetwork::upkeep() {
-    upkeepProvisioning();
-    if(WiFi.status() != WL_CONNECTED) {
-        if(isWifiConnected) {
-            wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting...");
-            isWifiConnected = false;
-        }
-        statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true);
-        reportWifiError();
-        if(wifiConnectionTimeout + 11000 < millis()) {
-            switch(wifiState) {
-                case SLIME_WIFI_NOT_SETUP: // Wasn't set up
-                return;
-                case SLIME_WIFI_SAVED_ATTEMPT: // Couldn't connect with first set of credentials
-                    #if ESP8266
-                        // Try again but with 11G
-                        // But only if there are credentials, otherwise we just waste time before
-                        // switching to hardcoded credentials.
-                        if (WiFi.SSID().length() > 0) {
-                            #if USE_ATTENUATION
-                                WiFi.setOutputPower(20.0 -  ATTENUATION_G);
-                            #endif
-                            WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-                            setStaticIPIfDefined();
-                            WiFi.begin();
-                            wifiConnectionTimeout = millis();
-                            wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status());
-                            wifiHandlerLogger.debug("Trying saved credentials with PHY Mode G...");
-                        } else {
-                            wifiHandlerLogger.debug("Skipping PHY Mode G attempt on 0-length SSID...");
-                        }
-                    #endif
-                    wifiState = SLIME_WIFI_SAVED_G_ATTEMPT;
-                return;
-                case SLIME_WIFI_SAVED_G_ATTEMPT: // Couldn't connect with first set of credentials with PHY Mode G
-                    #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
-                        // Try hardcoded credentials now
-                        #if ESP8266
-                            #if USE_ATTENUATION
-                                WiFi.setOutputPower(20.0 -  ATTENUATION_N);
-                            #endif
-                            WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-                        #endif
-                        setStaticIPIfDefined();
-                        WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
-                        wifiConnectionTimeout = millis();
-                        wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status());
-                        wifiHandlerLogger.debug("Trying hardcoded credentials...");
-                    #endif
-                    wifiState = SLIME_WIFI_HARDCODE_ATTEMPT;
-                return;
-                case SLIME_WIFI_HARDCODE_ATTEMPT: // Couldn't connect with second set of credentials
-                    #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266
-                        // Try hardcoded credentials again, but with PHY Mode G
-                        #if USE_ATTENUATION
-                            WiFi.setOutputPower(20.0 -  ATTENUATION_G);
-                        #endif
-                        WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-                        setStaticIPIfDefined();
-                        WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
-                        wifiConnectionTimeout = millis();
-                        wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status());
-                        wifiHandlerLogger.debug("Trying hardcoded credentials with WiFi PHY Mode G...");
-                    #endif
-                    wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT;
-                return;
-                case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with server-sent credentials.
-                    #if ESP8266
-                        // Try again silently but with 11G
-                        #if USE_ATTENUATION
-                            WiFi.setOutputPower(20.0 -  ATTENUATION_G);
-                        #endif
-                        WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-                        setStaticIPIfDefined();
-                        WiFi.begin();
-                        wifiConnectionTimeout = millis();
-                        wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT;
-                    #endif
-                return;
-                case SLIME_WIFI_HARDCODE_G_ATTEMPT: // Couldn't connect with second set of credentials with PHY Mode G.
-                case SLIME_WIFI_SERVER_CRED_G_ATTEMPT: // Or if couldn't connect with server-sent credentials
-                    // Return to the default PHY Mode N.
-                    #if ESP8266
-                        #if USE_ATTENUATION
-                            WiFi.setOutputPower(20.0 -  ATTENUATION_N);
-                        #endif
-                        WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-                    #endif
-                    // Start smart config
-                    if(!hadWifi && !WiFi.smartConfigDone() && wifiConnectionTimeout + 11000 < millis()) {
-                        if(WiFi.status() != WL_IDLE_STATUS) {
-                            wifiHandlerLogger.error("Can't connect from any credentials, status: %d.", WiFi.status());
-                            wifiConnectionTimeout = millis();
-                        }
-                        startProvisioning();
-                    }
-                return;
-            }
-        }
-        return;
-    }
-    if(!isWifiConnected) {
-        onConnected();
-        return;
-    } else {
-        if(millis() - last_rssi_sample >= 2000) {
-            last_rssi_sample = millis();
-            uint8_t signalStrength = WiFi.RSSI();
-            networkConnection.sendSignalStrength(signalStrength);
-        }
-    }
-    return;
+	upkeepProvisioning();
+	if (WiFi.status() != WL_CONNECTED) {
+		if (isWifiConnected) {
+			wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting...");
+			isWifiConnected = false;
+		}
+		statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true);
+		reportWifiError();
+		if (wifiConnectionTimeout + 11000 < millis()) {
+			switch (wifiState) {
+				case SLIME_WIFI_NOT_SETUP:  // Wasn't set up
+					return;
+				case SLIME_WIFI_SAVED_ATTEMPT:  // Couldn't connect with first set of
+												// credentials
+#if ESP8266
+					// Try again but with 11G but only if there are credentials,
+					// otherwise we just waste time before switching to hardcoded
+					// credentials.
+					if (WiFi.SSID().length() > 0) {
+#if USE_ATTENUATION
+						WiFi.setOutputPower(20.0 - ATTENUATION_G);
+#endif
+						WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+						setStaticIPIfDefined();
+						WiFi.begin();
+						wifiConnectionTimeout = millis();
+						wifiHandlerLogger.error(
+							"Can't connect from saved credentials, status: %d.",
+							WiFi.status()
+						);
+						wifiHandlerLogger.debug(
+							"Trying saved credentials with PHY Mode G..."
+						);
+					} else {
+						wifiHandlerLogger.debug(
+							"Skipping PHY Mode G attempt on 0-length SSID..."
+						);
+					}
+#endif
+					wifiState = SLIME_WIFI_SAVED_G_ATTEMPT;
+					return;
+				case SLIME_WIFI_SAVED_G_ATTEMPT:  // Couldn't connect with first set of
+												  // credentials with PHY Mode G
+#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
+												  // Try hardcoded credentials now
+#if ESP8266
+#if USE_ATTENUATION
+					WiFi.setOutputPower(20.0 - ATTENUATION_N);
+#endif
+					WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+#endif
+					setStaticIPIfDefined();
+					WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
+					wifiConnectionTimeout = millis();
+					wifiHandlerLogger.error(
+						"Can't connect from saved credentials, status: %d.",
+						WiFi.status()
+					);
+					wifiHandlerLogger.debug("Trying hardcoded credentials...");
+#endif
+					wifiState = SLIME_WIFI_HARDCODE_ATTEMPT;
+					return;
+				case SLIME_WIFI_HARDCODE_ATTEMPT:  // Couldn't connect with second set
+												   // of credentials
+#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266
+												   // Try hardcoded credentials again,
+												   // but with PHY Mode G
+#if USE_ATTENUATION
+					WiFi.setOutputPower(20.0 - ATTENUATION_G);
+#endif
+					WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+					setStaticIPIfDefined();
+					WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
+					wifiConnectionTimeout = millis();
+					wifiHandlerLogger.error(
+						"Can't connect from saved credentials, status: %d.",
+						WiFi.status()
+					);
+					wifiHandlerLogger.debug(
+						"Trying hardcoded credentials with WiFi PHY Mode G..."
+					);
+#endif
+					wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT;
+					return;
+				case SLIME_WIFI_SERVER_CRED_ATTEMPT:  // Couldn't connect with
+													  // server-sent credentials.
+#if ESP8266
+													  // Try again silently but with 11G
+#if USE_ATTENUATION
+					WiFi.setOutputPower(20.0 - ATTENUATION_G);
+#endif
+					WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+					setStaticIPIfDefined();
+					WiFi.begin();
+					wifiConnectionTimeout = millis();
+					wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT;
+#endif
+					return;
+				case SLIME_WIFI_HARDCODE_G_ATTEMPT:  // Couldn't connect with second set
+													 // of credentials with PHY Mode G.
+				case SLIME_WIFI_SERVER_CRED_G_ATTEMPT:  // Or if couldn't connect with
+														// server-sent credentials
+// Return to the default PHY Mode N.
+#if ESP8266
+#if USE_ATTENUATION
+					WiFi.setOutputPower(20.0 - ATTENUATION_N);
+#endif
+					WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+#endif
+					// Start smart config
+					if (!hadWifi && !WiFi.smartConfigDone()
+						&& wifiConnectionTimeout + 11000 < millis()) {
+						if (WiFi.status() != WL_IDLE_STATUS) {
+							wifiHandlerLogger.error(
+								"Can't connect from any credentials, status: %d.",
+								WiFi.status()
+							);
+							wifiConnectionTimeout = millis();
+						}
+						startProvisioning();
+					}
+					return;
+			}
+		}
+		return;
+	}
+	if (!isWifiConnected) {
+		onConnected();
+		return;
+	} else {
+		if (millis() - last_rssi_sample >= 2000) {
+			last_rssi_sample = millis();
+			uint8_t signalStrength = WiFi.RSSI();
+			networkConnection.sendSignalStrength(signalStrength);
+		}
+	}
+	return;
 }

src/network/wifihandler.h

@@ -1,52 +1,52 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 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:
+	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 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.
+	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.
 */
 #ifndef SLIMEVR_WIFI_H_
 #define SLIMEVR_WIFI_H_
 
 #ifdef ESP8266
-    #include <ESP8266WiFi.h>
+#include <ESP8266WiFi.h>
 #else
-    #include <WiFi.h>
+#include <WiFi.h>
 #endif
 
 namespace WiFiNetwork {
-    bool isConnected();
-    void setUp();
-    void upkeep();
-    void setWiFiCredentials(const char * SSID, const char * pass);
-    IPAddress getAddress();
-    uint8_t getWiFiState();
-}
+bool isConnected();
+void setUp();
+void upkeep();
+void setWiFiCredentials(const char* SSID, const char* pass);
+IPAddress getAddress();
+uint8_t getWiFiState();
+}  // namespace WiFiNetwork
 
 /** Wifi Reconnection Statuses **/
 typedef enum {
-    SLIME_WIFI_NOT_SETUP = 0,
-    SLIME_WIFI_SAVED_ATTEMPT,
-    SLIME_WIFI_SAVED_G_ATTEMPT,
-    SLIME_WIFI_HARDCODE_ATTEMPT,
-    SLIME_WIFI_HARDCODE_G_ATTEMPT,
-    SLIME_WIFI_SERVER_CRED_ATTEMPT,
-    SLIME_WIFI_SERVER_CRED_G_ATTEMPT
+	SLIME_WIFI_NOT_SETUP = 0,
+	SLIME_WIFI_SAVED_ATTEMPT,
+	SLIME_WIFI_SAVED_G_ATTEMPT,
+	SLIME_WIFI_HARDCODE_ATTEMPT,
+	SLIME_WIFI_HARDCODE_G_ATTEMPT,
+	SLIME_WIFI_SERVER_CRED_ATTEMPT,
+	SLIME_WIFI_SERVER_CRED_G_ATTEMPT
 } wifi_reconnection_statuses;
 
-#endif // SLIMEVR_WIFI_H_
+#endif  // SLIMEVR_WIFI_H_

src/network/wifiprovisioning.cpp

@@ -1,28 +1,29 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    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:
+	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 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.
+	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 "wifiprovisioning.h"
-#include "wifihandler.h"
+
 #include "logging/Logger.h"
+#include "wifihandler.h"
 
 // TODO Currently provisioning implemented via SmartConfig
 // it sucks.
@@ -33,25 +34,24 @@ SlimeVR::Logging::Logger wifiProvisioningLogger("WiFiProvisioning");
 bool provisioning = false;
 
 void WiFiNetwork::upkeepProvisioning() {
-    // Called even when not provisioning to do things like provide neighbours or other upkeep
+	// Called even when not provisioning to do things like provide neighbours or other
+	// upkeep
 }
 
 void WiFiNetwork::startProvisioning() {
-    if(WiFi.beginSmartConfig()) {
-        provisioning = true;
-        wifiProvisioningLogger.info("SmartConfig started");
-    }
+	if (WiFi.beginSmartConfig()) {
+		provisioning = true;
+		wifiProvisioningLogger.info("SmartConfig started");
+	}
 }
 
 void WiFiNetwork::stopProvisioning() {
-    WiFi.stopSmartConfig();
-    provisioning = false;
+	WiFi.stopSmartConfig();
+	provisioning = false;
 }
 
 void WiFiNetwork::provideNeighbours() {
-    // TODO: SmartConfig can't do this, created for future
+	// TODO: SmartConfig can't do this, created for future
 }
 
-bool WiFiNetwork::isProvisioning() {
-    return provisioning && !WiFi.smartConfigDone();
-}
+bool WiFiNetwork::isProvisioning() { return provisioning && !WiFi.smartConfigDone(); }

src/network/wifiprovisioning.h

@@ -1,34 +1,34 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    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:
+	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 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.
+	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.
 */
 #ifndef SLIMEVR_WIFIPROVISIONING_H_
 #define SLIMEVR_WIFIPROVISIONING_H_
 
 namespace WiFiNetwork {
-    void upkeepProvisioning();
-    void startProvisioning();
-    void stopProvisioning();
-    bool isProvisioning();
-    void provideNeighbours();
-}
+void upkeepProvisioning();
+void startProvisioning();
+void stopProvisioning();
+bool isProvisioning();
+void provideNeighbours();
+}  // namespace WiFiNetwork
 
-#endif // SLIMEVR_WIFIPROVISIONING_H_
+#endif  // SLIMEVR_WIFIPROVISIONING_H_

src/sensorinterface/DirectPinInterface.cpp

@@ -0,0 +1,29 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+#include "DirectPinInterface.h"
+
+int DirectPinInterface::digitalRead() { return ::digitalRead(_pinNum); }
+
+void DirectPinInterface::pinMode(uint8_t mode) { ::pinMode(_pinNum, mode); }
+
+void DirectPinInterface::digitalWrite(uint8_t val) { ::digitalWrite(_pinNum, val); }

src/sensorinterface/DirectPinInterface.h

@@ -0,0 +1,51 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+#ifndef _H_DIRECT_PIN_INTERFACE_
+#define _H_DIRECT_PIN_INTERFACE_
+
+#include <Arduino.h>
+#include <PinInterface.h>
+
+/**
+ * Pin interface using direct pins
+ *
+ */
+class DirectPinInterface : public PinInterface {
+public:
+	DirectPinInterface(uint8_t pin)
+		: _pinNum(pin){};
+
+	int digitalRead() override final;
+	void pinMode(uint8_t mode) override final;
+	void digitalWrite(uint8_t val) override final;
+
+	[[nodiscard]] std::string toString() const final {
+		using namespace std::string_literals;
+		return "Pin("s + std::to_string(_pinNum) + ")";
+	}
+
+private:
+	uint8_t _pinNum;
+};
+
+#endif  // _H_DIRECT_PIN_INTERFACE_

src/sensorinterface/DirectSPIInterface.cpp

@@ -0,0 +1,54 @@
+/*
+	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 "DirectSPIInterface.h"
+
+#include <Arduino.h>
+#include <PinInterface.h>
+
+namespace SlimeVR {
+
+DirectSPIInterface::DirectSPIInterface(SPIClass& spiClass, SPISettings spiSettings)
+	: m_spiClass{spiClass}
+	, m_spiSettings{spiSettings} {}
+
+bool DirectSPIInterface::init() {
+	m_spiClass.begin();
+	return true;
+}
+
+void DirectSPIInterface::swapIn() {}
+
+void DirectSPIInterface::beginTransaction(PinInterface* csPin) {
+	m_spiClass.beginTransaction(m_spiSettings);
+	csPin->digitalWrite(LOW);
+}
+
+void DirectSPIInterface::endTransaction(PinInterface* csPin) {
+	csPin->digitalWrite(HIGH);
+	m_spiClass.endTransaction();
+}
+
+const SPISettings& DirectSPIInterface::getSpiSettings() { return m_spiSettings; }
+
+}  // namespace SlimeVR

src/sensorinterface/DirectSPIInterface.h

@@ -0,0 +1,56 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <PinInterface.h>
+#include <SPI.h>
+
+#include "SensorInterface.h"
+
+namespace SlimeVR {
+
+class DirectSPIInterface : public SensorInterface {
+public:
+	DirectSPIInterface(SPIClass& spiClass, SPISettings spiSettings);
+	bool init() final;
+	void swapIn() final;
+
+	void beginTransaction(PinInterface* csPin);
+	void endTransaction(PinInterface* csPin);
+
+	[[nodiscard]] std::string toString() const final { return std::string{"SPI"}; }
+
+	template <typename... Args>
+	auto transfer(Args... args) {
+		return m_spiClass.transfer(args...);
+	}
+
+	const SPISettings& getSpiSettings();
+
+private:
+	SPIClass& m_spiClass;
+	SPISettings m_spiSettings;
+};
+
+}  // namespace SlimeVR

src/sensorinterface/I2CPCAInterface.cpp

@@ -0,0 +1,40 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+#include "I2CPCAInterface.h"
+
+bool SlimeVR::I2CPCASensorInterface::init() {
+	m_Wire.init();
+	return true;
+}
+
+void SlimeVR::I2CPCASensorInterface::swapIn() {
+	m_Wire.swapIn();
+	Wire.beginTransmission(m_Address);
+	Wire.write(1 << m_Channel);
+	Wire.endTransmission();
+#ifdef ESP32
+	// On ESP32 we need to reconnect to I2C bus for some reason
+	m_Wire.disconnect();
+	m_Wire.swapIn();
+#endif
+}

src/sensorinterface/I2CPCAInterface.h

@@ -0,0 +1,62 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+#ifndef I2C_PCA_INTERFACE_H
+#define I2C_PCA_INTERFACE_H
+
+#include "I2CWireSensorInterface.h"
+
+namespace SlimeVR {
+/**
+ * I2C Sensor interface for use with PCA9547 (8-channel I2C-buss multiplexer)
+ * or PCA9546A (4-channel I2C-bus multiplexer) or analogs
+ */
+class I2CPCASensorInterface : public SensorInterface {
+public:
+	I2CPCASensorInterface(
+		uint8_t sclpin,
+		uint8_t sdapin,
+		uint8_t address,
+		uint8_t channel
+	)
+		: m_Wire(sclpin, sdapin)
+		, m_Address(address)
+		, m_Channel(channel){};
+	~I2CPCASensorInterface(){};
+
+	bool init() override final;
+	void swapIn() override final;
+
+	[[nodiscard]] std::string toString() const final {
+		using namespace std::string_literals;
+		return "PCAWire("s + std::to_string(m_Channel) + ")";
+	}
+
+protected:
+	I2CWireSensorInterface m_Wire;
+	uint8_t m_Address;
+	uint8_t m_Channel;
+};
+
+}  // namespace SlimeVR
+
+#endif

src/sensorinterface/I2CWireSensorInterface.cpp

@@ -0,0 +1,75 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+
+#include "I2CWireSensorInterface.h"
+
+#include <optional>
+
+#ifdef ESP32
+#include "driver/i2c.h"
+#endif
+
+std::optional<uint8_t> activeSCLPin;
+std::optional<uint8_t> activeSDAPin;
+bool isI2CActive = false;
+
+namespace SlimeVR {
+void swapI2C(uint8_t sclPin, uint8_t sdaPin) {
+	if (sclPin != activeSCLPin || sdaPin != activeSDAPin || !isI2CActive) {
+		Wire.flush();
+#ifdef ESP32
+		if (!isI2CActive) {
+			// Reset HWI2C to avoid being affected by I2CBUS reset
+			Wire.end();
+		}
+
+		if (activeSCLPin && activeSCLPin) {
+			// Disconnect pins from HWI2C
+			gpio_set_direction((gpio_num_t)*activeSCLPin, GPIO_MODE_INPUT);
+			gpio_set_direction((gpio_num_t)*activeSDAPin, GPIO_MODE_INPUT);
+		}
+
+		if (isI2CActive) {
+			i2c_set_pin(I2C_NUM_0, sdaPin, sclPin, false, false, I2C_MODE_MASTER);
+		} else {
+			Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin), I2C_SPEED);
+			Wire.setTimeOut(150);
+		}
+#else
+		Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin));
+#endif
+
+		activeSCLPin = sclPin;
+		activeSDAPin = sdaPin;
+		isI2CActive = true;
+	}
+}
+
+void disconnectI2C() {
+	Wire.flush();
+	isI2CActive = false;
+#ifdef ESP32
+	Wire.end();
+#endif
+}
+}  // namespace SlimeVR

src/sensorinterface/I2CWireSensorInterface.h

@@ -0,0 +1,65 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+
+#ifndef SENSORINTERFACE_I2CWIRE_H
+#define SENSORINTERFACE_I2CWIRE_H
+
+#include <Arduino.h>
+#include <i2cscan.h>
+
+#include "SensorInterface.h"
+#include "globals.h"
+
+namespace SlimeVR {
+void swapI2C(uint8_t sclPin, uint8_t sdaPin);
+void disconnectI2C();
+
+/**
+ * I2C Sensor interface using direct arduino Wire on provided pins
+ *
+ */
+class I2CWireSensorInterface : public SensorInterface {
+public:
+	I2CWireSensorInterface(uint8_t sclpin, uint8_t sdapin)
+		: _sdaPin(sdapin)
+		, _sclPin(sclpin){};
+	~I2CWireSensorInterface(){};
+
+	bool init() override final { return true; }
+	void swapIn() override final { swapI2C(_sclPin, _sdaPin); }
+	void disconnect() { disconnectI2C(); }
+
+	[[nodiscard]] std::string toString() const final {
+		using namespace std::string_literals;
+		return "Wire("s + std::to_string(_sclPin) + ": " + std::to_string(_sdaPin)
+			 + ")"s;
+	}
+
+protected:
+	uint8_t _sdaPin;
+	uint8_t _sclPin;
+};
+
+}  // namespace SlimeVR
+
+#endif  // SENSORINTERFACE_I2CWIRE_H

src/sensorinterface/MCP23X17PinInterface.cpp

@@ -0,0 +1,31 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+#include "MCP23X17PinInterface.h"
+
+int MCP23X17PinInterface::digitalRead() { return _mcp23x17->digitalRead(_pinNum); }
+
+void MCP23X17PinInterface::pinMode(uint8_t mode) { _mcp23x17->pinMode(_pinNum, mode); }
+
+void MCP23X17PinInterface::digitalWrite(uint8_t val) {
+	_mcp23x17->digitalWrite(_pinNum, val);
+}

src/sensorinterface/MCP23X17PinInterface.h

@@ -0,0 +1,69 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+#ifndef _H_MCP23X17PinInterface_
+#define _H_MCP23X17PinInterface_
+
+#include <Adafruit_MCP23X17.h>
+#include <PinInterface.h>
+
+#define MCP_GPA0 0
+#define MCP_GPA1 1
+#define MCP_GPA2 2
+#define MCP_GPA3 3
+#define MCP_GPA4 4
+#define MCP_GPA5 5
+#define MCP_GPA6 6
+#define MCP_GPA7 7
+#define MCP_GPB0 8
+#define MCP_GPB1 9
+#define MCP_GPB2 10
+#define MCP_GPB3 11
+#define MCP_GPB4 12
+#define MCP_GPB5 13
+#define MCP_GPB6 14
+#define MCP_GPB7 15
+
+/**
+ * Pin interface to use MCP23008/17 I2C GPIO port extenders
+ */
+class MCP23X17PinInterface : public PinInterface {
+public:
+	MCP23X17PinInterface(Adafruit_MCP23X17* mcp, uint8_t pin)
+		: _mcp23x17(mcp)
+		, _pinNum(pin){};
+
+	int digitalRead() override final;
+	void pinMode(uint8_t mode) override final;
+	void digitalWrite(uint8_t val) override final;
+
+	[[nodiscard]] std::string toString() const final {
+		using namespace std::string_literals;
+		return "MCPPin("s + std::to_string(_pinNum) + ")";
+	}
+
+private:
+	Adafruit_MCP23X17* _mcp23x17;
+	uint8_t _pinNum;
+};
+
+#endif  // _H_MCP23X17PinInterface_

src/sensorinterface/RegisterInterface.cpp

@@ -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

src/sensorinterface/RegisterInterface.h

@@ -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

src/sensorinterface/SPIImpl.h

@@ -0,0 +1,130 @@
+/*
+	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 "DirectSPIInterface.h"
+#include "RegisterInterface.h"
+
+#define ICM_READ_FLAG 0x80
+
+namespace SlimeVR::Sensors {
+
+struct SPIImpl : public RegisterInterface {
+	SPIImpl(DirectSPIInterface* spi, PinInterface* csPin)
+		: m_spi(spi)
+		, m_csPin(csPin) {
+		auto& spiSettings = spi->getSpiSettings();
+		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);
+	}
+
+	uint8_t readReg(uint8_t regAddr) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr | ICM_READ_FLAG);
+		uint8_t buffer = m_spi->transfer(0);
+
+		m_spi->endTransaction(m_csPin);
+
+		return buffer;
+	}
+
+	uint16_t readReg16(uint8_t regAddr) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr | ICM_READ_FLAG);
+		uint8_t b1 = m_spi->transfer(0);
+		uint8_t b2 = m_spi->transfer(0);
+
+		m_spi->endTransaction(m_csPin);
+		return b2 << 8 | b1;
+	}
+
+	void writeReg(uint8_t regAddr, uint8_t value) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr);
+		m_spi->transfer(value);
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	void writeReg16(uint8_t regAddr, uint16_t value) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr);
+		m_spi->transfer(value & 0xFF);
+		m_spi->transfer(value >> 8);
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr | ICM_READ_FLAG);
+		for (uint8_t i = 0; i < size; ++i) {
+			buffer[i] = m_spi->transfer(0);
+		}
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr);
+		for (uint8_t i = 0; i < size; ++i) {
+			m_spi->transfer(buffer[i]);
+		}
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	bool hasSensorOnBus() override {
+		return true;  // TODO
+	}
+
+	uint8_t getAddress() const override { return 0; }
+
+	std::string toString() const override { return std::string("SPI"); }
+
+private:
+	DirectSPIInterface* m_spi;
+	PinInterface* m_csPin;
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("SPI");
+};
+
+}  // namespace SlimeVR::Sensors

src/sensorinterface/SensorInterface.cpp

@@ -0,0 +1,30 @@
+/*
+	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 "SensorInterface.h"
+
+namespace SlimeVR {
+
+EmptySensorInterface EmptySensorInterface::instance;
+
+}

src/sensorinterface/SensorInterface.h

@@ -0,0 +1,48 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+
+#ifndef SENSORINTERFACE_H
+#define SENSORINTERFACE_H
+
+#include <string>
+
+namespace SlimeVR {
+class SensorInterface {
+public:
+	virtual bool init() = 0;
+	virtual void swapIn() = 0;
+	[[nodiscard]] virtual std::string toString() const = 0;
+};
+
+class EmptySensorInterface : public SensorInterface {
+public:
+	EmptySensorInterface(){};
+	bool init() override final { return true; };
+	void swapIn() override final{};
+	[[nodiscard]] std::string toString() const final { return "None"; }
+
+	static EmptySensorInterface instance;
+};
+}  // namespace SlimeVR
+
+#endif  // SENSORINTERFACE_H

src/sensorinterface/SensorInterfaceManager.cpp

@@ -0,0 +1,46 @@
+/*
+	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 "SensorInterfaceManager.h"
+
+template <typename T>
+bool byteCompare(const T& lhs, const T& rhs) {
+	const auto* lhsBytes = reinterpret_cast<const uint8_t*>(&lhs);
+	const auto* rhsBytes = reinterpret_cast<const uint8_t*>(&rhs);
+
+	for (size_t i = 0; i < sizeof(T); i++) {
+		if (lhsBytes[i] < rhsBytes[i]) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool operator<(const SPISettings& lhs, const SPISettings& rhs) {
+	return byteCompare(lhs, rhs);
+}
+
+bool operator<(const SPIClass& lhs, const SPIClass& rhs) {
+	return byteCompare(lhs, rhs);
+}

src/sensorinterface/SensorInterfaceManager.h

@@ -0,0 +1,113 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <PinInterface.h>
+#include <SPI.h>
+
+#include <functional>
+#include <map>
+#include <optional>
+
+#include "DirectPinInterface.h"
+#include "I2CPCAInterface.h"
+#include "I2CWireSensorInterface.h"
+#include "MCP23X17PinInterface.h"
+#include "SPIImpl.h"
+#include "SensorInterface.h"
+#include "i2cimpl.h"
+#include "sensorinterface/DirectSPIInterface.h"
+#include "sensorinterface/SPIImpl.h"
+
+bool operator<(const SPISettings& lhs, const SPISettings& rhs);
+bool operator<(const SPIClass& lhs, const SPIClass& rhs);
+
+bool operator<(const SPISettings& lhs, const SPISettings& rhs);
+bool operator<(const SPIClass& lhs, const SPIClass& rhs);
+
+namespace SlimeVR {
+
+class SensorInterfaceManager {
+private:
+	template <typename InterfaceClass, typename... Args>
+	struct SensorInterface {
+		explicit SensorInterface(
+			std::function<bool(Args...)> validate = [](Args...) { return true; }
+		)
+			: validate{validate} {}
+
+		InterfaceClass* get(Args... args) {
+			if (validate && !validate(args...)) {
+				return static_cast<InterfaceClass*>(nullptr);
+			}
+
+			auto key = std::make_tuple(args...);
+
+			if (!cache.contains(key)) {
+				auto ptr = new InterfaceClass(args...);
+
+				bool success;
+				if constexpr (requires { ptr->init(); }) {
+					success = ptr->init();
+				} else {
+					success = true;
+				}
+
+				if (!success) {
+					cache[key] = nullptr;
+					return nullptr;
+				}
+
+				cache[key] = ptr;
+			}
+			return cache[key];
+		}
+
+	private:
+		std::map<std::tuple<Args...>, InterfaceClass*> cache;
+		std::function<bool(Args...)> validate;
+	};
+
+public:
+	inline auto& directPinInterface() { return directPinInterfaces; }
+	inline auto& mcpPinInterface() { return mcpPinInterfaces; }
+	inline auto& i2cWireInterface() { return i2cWireInterfaces; }
+	inline auto& pcaWireInterface() { return pcaWireInterfaces; }
+	inline auto& i2cImpl() { return i2cImpls; }
+	inline auto& directSPIInterface() { return directSPIInterfaces; }
+	inline auto& spiImpl() { return spiImpls; }
+
+private:
+	SensorInterface<DirectPinInterface, int> directPinInterfaces{[](int pin) {
+		return pin != 255 && pin != -1;
+	}};
+	SensorInterface<MCP23X17PinInterface, Adafruit_MCP23X17*, int> mcpPinInterfaces;
+	SensorInterface<I2CWireSensorInterface, int, int> i2cWireInterfaces;
+	SensorInterface<I2CPCASensorInterface, int, int, int, int> pcaWireInterfaces;
+	SensorInterface<Sensors::I2CImpl, uint8_t> i2cImpls;
+	SensorInterface<DirectSPIInterface, SPIClass, SPISettings> directSPIInterfaces;
+	SensorInterface<Sensors::SPIImpl, DirectSPIInterface*, PinInterface*> spiImpls;
+};
+
+}  // namespace SlimeVR

src/sensorinterface/i2cimpl.h

@@ -0,0 +1,94 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Tailsy13 & 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 <i2cscan.h>
+
+#include <cstdint>
+
+#include "I2Cdev.h"
+#include "RegisterInterface.h"
+
+namespace SlimeVR::Sensors {
+
+struct I2CImpl : public RegisterInterface {
+	I2CImpl(uint8_t devAddr)
+		: m_devAddr(devAddr) {}
+
+	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 override {
+		uint16_t buffer = 0;
+		I2Cdev::readBytes(
+			m_devAddr,
+			regAddr,
+			sizeof(buffer),
+			reinterpret_cast<uint8_t*>(&buffer)
+		);
+		return buffer;
+	}
+
+	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 override {
+		I2Cdev::writeBytes(
+			m_devAddr,
+			regAddr,
+			sizeof(value),
+			reinterpret_cast<uint8_t*>(&value)
+		);
+	}
+
+	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 override {
+		I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer);
+	}
+
+	bool hasSensorOnBus() {
+		// Ask twice, because we're nice like this
+		return I2CSCAN::hasDevOnBus(m_devAddr) || I2CSCAN::hasDevOnBus(m_devAddr);
+	}
+
+	uint8_t getAddress() const override { return m_devAddr; }
+
+	std::string toString() const {
+		char buf[16];
+		std::snprintf(buf, sizeof(buf), "I2C(0x%02x)", m_devAddr);
+		return std::string(buf);
+	}
+
+private:
+	uint8_t m_devAddr;
+};
+
+}  // namespace SlimeVR::Sensors

src/sensors/ADCResistanceSensor.cpp

@@ -0,0 +1,44 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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.
+*/
+#include "ADCResistanceSensor.h"
+
+#include "GlobalVars.h"
+
+namespace SlimeVR::Sensors {
+void ADCResistanceSensor::motionLoop() {
+#if ESP8266
+	float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution;
+	m_Data = m_ResistanceDivider
+		   * (ADCVoltageMax / voltage - 1.0f);  // Convert voltage to resistance
+#elif defined(ESP32)
+	float voltage = ((float)analogReadMilliVolts(m_Pin)) / 1000;
+	m_Data = m_ResistanceDivider
+		   * (m_VCC / voltage - 1.0f);  // Convert voltage to resistance
+#endif
+}
+
+void ADCResistanceSensor::sendData() {
+	networkConnection.sendFlexData(sensorId, m_Data);
+}
+
+}  // namespace SlimeVR::Sensors

src/sensors/ADCResistanceSensor.h

@@ -0,0 +1,71 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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 "../sensorinterface/RegisterInterface.h"
+#include "sensor.h"
+
+namespace SlimeVR::Sensors {
+class ADCResistanceSensor : Sensor {
+public:
+	static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE;
+
+	ADCResistanceSensor(
+		uint8_t id,
+		uint8_t pin,
+		float VCC,
+		float resistanceDivider,
+		float smoothFactor = 0.1f
+	)
+		: Sensor(
+			"ADCResistanceSensor",
+			SensorTypeID::ADC_RESISTANCE,
+			id,
+			EmptyRegisterInterface::instance,
+			0.0f,
+			new SlimeVR::EmptySensorInterface
+		)
+		, m_Pin(pin)
+		, m_VCC(VCC)
+		, m_ResistanceDivider(resistanceDivider)
+		, m_SmoothFactor(smoothFactor){};
+	~ADCResistanceSensor();
+
+	void motionLoop() override final;
+	void sendData() override final;
+
+	SensorStatus getSensorState() override final { return SensorStatus::SENSOR_OK; }
+
+	SensorDataType getDataType() override final {
+		return SensorDataType::SENSOR_DATATYPE_FLEX_RESISTANCE;
+	};
+
+private:
+	uint8_t m_Pin;
+	float m_VCC;
+	float m_ResistanceDivider;
+	float m_SmoothFactor;
+
+	float m_Data = 0.0f;
+};
+}  // namespace SlimeVR::Sensors

src/sensors/EmptySensor.h

@@ -1,51 +1,53 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    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:
+	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 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.
+	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.
 */
 
 #ifndef SENSORS_EMPTYSENSOR_H
 #define SENSORS_EMPTYSENSOR_H
 
+#include "../sensorinterface/RegisterInterface.h"
 #include "sensor.h"
 
-namespace SlimeVR
-{
-    namespace Sensors
-    {
-        class EmptySensor : public Sensor
-        {
-        public:
-            EmptySensor(uint8_t id) : Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){};
-            ~EmptySensor(){};
+namespace SlimeVR::Sensors {
+class EmptySensor : public Sensor {
+public:
+	EmptySensor(uint8_t id)
+		: Sensor(
+			"EmptySensor",
+			SensorTypeID::Empty,
+			id,
+			EmptyRegisterInterface::instance,
+			0.0
+		){};
+	~EmptySensor(){};
 
-            void motionSetup() override final{};
-            void motionLoop() override final{};
-            void sendData() override final{};
-            void startCalibration(int calibrationType) override final{};
-            SensorStatus getSensorState() override final
-            {
-                return SensorStatus::SENSOR_OFFLINE;
-            };
-        };
-    }
-}
+	void motionSetup() override final{};
+	void motionLoop() override final{};
+	void sendData() override final{};
+	void startCalibration(int calibrationType) override final{};
+	SensorStatus getSensorState() override final {
+		return SensorStatus::SENSOR_OFFLINE;
+	};
+};
+}  // namespace SlimeVR::Sensors
 
 #endif

src/sensors/ErroneousSensor.cpp

@@ -1,41 +1,39 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    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:
+	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 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.
+	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 "ErroneousSensor.h"
+
 #include "GlobalVars.h"
 
-namespace SlimeVR
-{
-    namespace Sensors
-    {
-        void ErroneousSensor::motionSetup()
-        {
-            m_Logger.error("IMU of type %s failed to initialize", getIMUNameByType(m_ExpectedType));
-        }
-
-        SensorStatus ErroneousSensor::getSensorState()
-        {
-            return SensorStatus::SENSOR_ERROR;
-        };
-    }
+namespace SlimeVR::Sensors {
+void ErroneousSensor::motionSetup() {
+	m_Logger.error(
+		"IMU of type %s failed to initialize",
+		getIMUNameByType(m_ExpectedType)
+	);
+	m_tpsCounter.reset();
+	m_dataCounter.reset();
 }
+
+SensorStatus ErroneousSensor::getSensorState() { return SensorStatus::SENSOR_ERROR; };
+}  // namespace SlimeVR::Sensors

src/sensors/ErroneousSensor.h

@@ -1,51 +1,49 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    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:
+	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 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.
+	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.
 */
 
 #ifndef SENSORS_ERRONEOUSSENSOR_H
 #define SENSORS_ERRONEOUSSENSOR_H
 
+#include "../sensorinterface/RegisterInterface.h"
 #include "sensor.h"
 
-namespace SlimeVR
-{
-    namespace Sensors
-    {
-        class ErroneousSensor : public Sensor
-        {
-        public:
-            ErroneousSensor(uint8_t id, ImuID type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){};
-            ~ErroneousSensor(){};
+namespace SlimeVR::Sensors {
+class ErroneousSensor : public Sensor {
+public:
+	ErroneousSensor(uint8_t id, SensorTypeID type)
+		: Sensor("ErroneousSensor", type, id, EmptyRegisterInterface::instance, 0.0)
+		, m_ExpectedType(type){};
+	~ErroneousSensor(){};
 
-            void motionSetup() override;
-            void motionLoop() override final{};
-            void sendData() override{};
-            void startCalibration(int calibrationType) override final{};
-            SensorStatus getSensorState() override final;
+	void motionSetup() override;
+	void motionLoop() override final{};
+	void sendData() override{};
+	void startCalibration(int calibrationType) override final{};
+	SensorStatus getSensorState() override final;
 
-        private:
-            ImuID m_ExpectedType;
-        };
-    }
-}
+private:
+	SensorTypeID m_ExpectedType;
+};
+}  // namespace SlimeVR::Sensors
 
 #endif

src/sensors/RestCalibrationDetector.cpp

@@ -0,0 +1,55 @@
+/*
+	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 "RestCalibrationDetector.h"
+
+#include "sensors/SensorFusion.h"
+
+namespace SlimeVR::Sensors {
+
+bool RestCalibrationDetector::update(SensorFusion& fusion) {
+	if (state == CalibrationState::Done) {
+		return false;
+	}
+
+	if (!fusion.getRestDetected()) {
+		state = CalibrationState::NoRest;
+		return false;
+	}
+
+	if (state == CalibrationState::NoRest) {
+		state = CalibrationState::Calibrating;
+		lastRestStartedMillis = millis();
+		return false;
+	}
+
+	uint32_t elapsed = millis() - lastRestStartedMillis;
+	if (elapsed < static_cast<uint32_t>(restCalibrationSeconds * 1e3)) {
+		return false;
+	}
+
+	state = CalibrationState::Done;
+	return true;
+}
+
+}  // namespace SlimeVR::Sensors

src/sensors/RestCalibrationDetector.h

@@ -0,0 +1,49 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+
+#include "sensors/SensorFusion.h"
+
+namespace SlimeVR::Sensors {
+
+class RestCalibrationDetector {
+public:
+	bool update(SensorFusion& fusion);
+
+private:
+	static constexpr float restCalibrationSeconds = 3.0f;
+
+	enum class CalibrationState {
+		NoRest,
+		Calibrating,
+		Done,
+	};
+
+	CalibrationState state = CalibrationState::NoRest;
+	uint32_t lastRestStartedMillis = 0;
+};
+
+}  // namespace SlimeVR::Sensors

src/sensors/SensorBuilder.cpp

@@ -0,0 +1,126 @@
+/*
+	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) {}
+
+#define SENSOR_DESC_ENTRY(ImuType, ...)                                           \
+	activeSensorCount += sensorDescEntry<ImuType>(sensorID, __VA_ARGS__) ? 1 : 0; \
+	sensorID++;
+
+#define SENSOR_INFO_ENTRY(ImuID, SensorPosition) \
+	m_Manager->m_Sensors[ImuID]->setSensorInfo(SensorPosition);
+
+uint8_t SensorBuilder::buildAllSensors() {
+	uint8_t sensorID = 0;
+	uint8_t activeSensorCount = 0;
+
+	[[maybe_unused]] const auto NO_PIN = nullptr;
+	[[maybe_unused]] const auto NO_WIRE = &EmptySensorInterface::instance;
+	[[maybe_unused]] const auto DIRECT_PIN
+		= [&](uint8_t pin) { return interfaceManager.directPinInterface().get(pin); };
+	[[maybe_unused]] const auto DIRECT_WIRE = [&](uint8_t scl, uint8_t sda) {
+		return interfaceManager.i2cWireInterface().get(scl, sda);
+	};
+	[[maybe_unused]] const auto MCP_PIN = [&](uint8_t pin) {
+		return interfaceManager.mcpPinInterface().get(&m_Manager->m_MCP, pin);
+	};
+	[[maybe_unused]] const auto PCA_WIRE
+		= [&](uint8_t scl, uint8_t sda, uint8_t addr, uint8_t ch) {
+			  return interfaceManager.pcaWireInterface().get(scl, sda, addr, ch);
+		  };
+	[[maybe_unused]] const auto DIRECT_SPI
+		= [&](uint32_t clockFreq, uint8_t bitOrder, uint8_t dataMode) {
+			  return interfaceManager.directSPIInterface().get(
+				  SPI,
+				  SPISettings(clockFreq, bitOrder, dataMode)
+			  );
+		  };
+
+	// Apply descriptor list and expand to entries
+	SENSOR_DESC_LIST
+
+	// Apply sensor info list and expand to entries
+	SENSOR_INFO_LIST
+
+	return activeSensorCount;
+}
+
+std::unique_ptr<::Sensor>
+SensorBuilder::buildSensorDynamically(SensorTypeID type, SensorDefinition sensorDef) {
+	switch (type) {
+		// case SensorTypeID::MPU9250:
+		//	return buildSensor<MPU9250Sensor>(sensorDef);
+		// case SensorTypeID::BNO080:
+		//	return buildSensor<BNO080Sensor>(sensorDef);
+		case SensorTypeID::BNO085:
+			return buildSensor<BNO085Sensor>(sensorDef);
+		// case SensorTypeID::BNO055:
+		//	return buildSensor<BNO055Sensor>(sensorDef);
+		// case SensorTypeID::MPU6050:
+		//	return buildSensor<SoftFusionMPU6050>(
+		//		sensorDef
+		//	);
+		// case SensorTypeID::BNO086:
+		//	return buildSensor<BNO086Sensor>(sensorDef);
+		// case SensorTypeID::BMI160:
+		// 	return buildSensor<BMI160Sensor>(sensorDef);
+		// case SensorTypeID::ICM20948:
+		//	return buildSensor<ICM20948Sensor>(sensorDef);
+		// case SensorTypeID::ICM42688:
+		//	return buildSensor<SoftFusionICM42688>(
+		//		sensorDef
+		//	);
+		case SensorTypeID::BMI270:
+			return buildSensor<SoftFusionBMI270>(sensorDef);
+		// case SensorTypeID::LSM6DS3TRC:
+		//	return buildSensor<SoftFusionLSM6DS3TRC>(
+		//		sensorDef
+		//	);
+		case SensorTypeID::LSM6DSV:
+			return buildSensor<SoftFusionLSM6DSV>(sensorDef);
+		case SensorTypeID::LSM6DSO:
+			return buildSensor<SoftFusionLSM6DSO>(sensorDef);
+		case SensorTypeID::LSM6DSR:
+			return buildSensor<SoftFusionLSM6DSR>(sensorDef);
+		case SensorTypeID::ICM45686:
+			return buildSensor<SoftFusionICM45686>(sensorDef);
+		// case SensorTypeID::ICM45605:
+		//	return buildSensor<SoftFusionICM45605>(
+		//		sensorDef
+		//	);
+		default:
+			m_Manager->m_Logger.error(
+				"Unable to create sensor with type %s (%d)",
+				getIMUNameByType(type),
+				static_cast<int>(type)
+			);
+	}
+	return std::make_unique<EmptySensor>(sensorDef.sensorID);
+}
+
+}  // namespace SlimeVR::Sensors

src/sensors/SensorBuilder.h

@@ -0,0 +1,366 @@
+/*
+	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 "PinInterface.h"
+#include "SensorManager.h"
+#include "bno055sensor.h"
+#include "bno080sensor.h"
+#include "consts.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/DirectSPIInterface.h"
+#include "sensorinterface/I2CPCAInterface.h"
+#include "sensorinterface/I2CWireSensorInterface.h"
+#include "sensorinterface/MCP23X17PinInterface.h"
+#include "sensorinterface/RegisterInterface.h"
+#include "sensorinterface/SPIImpl.h"
+#include "sensorinterface/SensorInterface.h"
+#include "sensorinterface/SensorInterfaceManager.h"
+#include "sensorinterface/i2cimpl.h"
+#include "softfusion/drivers/bmi160.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 false
+#endif
+
+#ifndef SECONDARY_IMU_ADDRESS_TWO
+#define SECONDARY_IMU_ADDRESS_TWO true
+#endif
+
+#if USE_RUNTIME_CALIBRATION
+#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
+#define SFCALIBRATOR RuntimeCalibration::RuntimeCalibrator
+#else
+#include "sensors/softfusion/SoftfusionCalibration.h"
+#define SFCALIBRATOR SoftfusionCalibrator
+#endif
+
+#ifdef 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>;
+using SoftFusionBMI160 = SoftFusionSensor<SoftFusion::Drivers::BMI160, SFCALIBRATOR>;
+class SensorAuto {};
+
+struct SensorBuilder {
+private:
+	struct SensorDefinition {
+		uint8_t sensorID;
+		RegisterInterface& imuInterface;
+		float rotation;
+		SensorInterface* sensorInterface;
+		bool optional;
+		PinInterface* intPin;
+		int extraParam;
+	};
+
+public:
+	SensorManager* m_Manager;
+	explicit SensorBuilder(SensorManager* sensorManager);
+
+	uint8_t buildAllSensors();
+
+	std::unique_ptr<::Sensor>
+	buildSensorDynamically(SensorTypeID type, SensorDefinition sensorDef);
+
+	template <typename Sensor, typename AccessInterface>
+	std::optional<std::pair<SensorTypeID, RegisterInterface*>> checkSensorPresent(
+		uint8_t sensorId,
+		SensorInterface* sensorInterface,
+		AccessInterface accessInterface
+	) {
+		auto* registerInterface
+			= getRegisterInterface<Sensor>(sensorId, sensorInterface, accessInterface);
+
+		if (!registerInterface->hasSensorOnBus()) {
+			return {};
+		}
+
+		if constexpr (requires {
+						  {
+							  Sensor::checkPresent(*registerInterface)
+						  } -> std::same_as<SensorTypeID>;
+					  }) {
+			auto type = Sensor::checkPresent(*registerInterface);
+
+			if (type == SensorTypeID::Unknown) {
+				return {};
+			}
+
+			return std::make_pair(type, registerInterface);
+		} else {
+			if (!Sensor::checkPresent(*registerInterface)) {
+				return {};
+			}
+		}
+
+		return std::make_pair(Sensor::TypeID, registerInterface);
+	}
+
+	template <typename AccessInterface>
+	inline std::optional<std::pair<SensorTypeID, RegisterInterface*>>
+	checkSensorsPresent(uint8_t, SensorInterface*, AccessInterface) {
+		return std::nullopt;
+	}
+
+	template <typename AccessInterface, typename Sensor, typename... Rest>
+	inline std::optional<std::pair<SensorTypeID, RegisterInterface*>>
+	checkSensorsPresent(
+		uint8_t sensorId,
+		SensorInterface* sensorInterface,
+		AccessInterface accessInterface
+	) {
+		auto result
+			= checkSensorPresent<Sensor>(sensorId, sensorInterface, accessInterface);
+		if (result) {
+			return result;
+		}
+
+		return checkSensorsPresent<AccessInterface, Rest...>(
+			sensorId,
+			sensorInterface,
+			accessInterface
+		);
+	}
+
+	template <typename Sensor, typename AccessInterface>
+	RegisterInterface* getRegisterInterface(
+		uint8_t sensorId,
+		SensorInterface* interface,
+		AccessInterface access
+	) {
+		if constexpr (std::is_base_of_v<
+						  PinInterface,
+						  std::remove_pointer_t<AccessInterface>>) {
+			return interfaceManager.spiImpl().get(
+				static_cast<DirectSPIInterface*>(interface),
+				access
+			);
+		} else if constexpr (std::is_same_v<AccessInterface, bool>) {
+			uint8_t addressIncrement = access ? 1 : 0;
+			return interfaceManager.i2cImpl().get(Sensor::Address + addressIncrement);
+		} else if constexpr (std::is_integral_v<AccessInterface>) {
+			return interfaceManager.i2cImpl().get(access);
+		}
+
+		return &EmptyRegisterInterface::instance;
+	}
+
+	template <typename AccessInterface>
+	std::optional<std::pair<SensorTypeID, RegisterInterface*>> findSensorType(
+		uint8_t sensorID,
+		SensorInterface* sensorInterface,
+		AccessInterface accessInterface
+	) {
+		sensorInterface->init();
+		sensorInterface->swapIn();
+
+		return checkSensorsPresent<
+			AccessInterface,
+			// SoftFusionLSM6DS3TRC,
+			// SoftFusionICM42688,
+			SoftFusionBMI270,
+			SoftFusionLSM6DSV,
+			SoftFusionLSM6DSO,
+			SoftFusionLSM6DSR,
+			// SoftFusionMPU6050,
+			SoftFusionICM45686,
+			// SoftFusionICM45605
+			BNO085Sensor>(sensorID, sensorInterface, accessInterface);
+	}
+
+	template <typename SensorType, typename AccessInterface>
+	bool sensorDescEntry(
+		uint8_t sensorID,
+		AccessInterface accessInterface,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional = false,
+		PinInterface* intPin = nullptr,
+		int extraParam = 0
+	) {
+		std::unique_ptr<::Sensor> sensor;
+		if constexpr (std::is_same<SensorType, SensorAuto>::value) {
+			auto result = findSensorType(sensorID, sensorInterface, accessInterface);
+
+			if (!result) {
+				m_Manager->m_Logger.error(
+					"Can't find sensor type for sensor %d",
+					sensorID
+				);
+				return false;
+			}
+
+			auto sensorType = result->first;
+			auto& regInterface = *(result->second);
+
+			m_Manager->m_Logger.info(
+				"Sensor %d automatically detected with %s",
+				sensorID,
+				getIMUNameByType(sensorType)
+			);
+			sensor = buildSensorDynamically(
+				sensorType,
+				{
+					sensorID,
+					regInterface,
+					rotation,
+					sensorInterface,
+					optional,
+					intPin,
+					extraParam,
+				}
+			);
+		} else {
+			auto& regInterface = *getRegisterInterface<SensorType>(
+				sensorID,
+				sensorInterface,
+				accessInterface
+			);
+
+			sensor = buildSensor<SensorType>({
+				sensorID,
+				regInterface,
+				rotation,
+				sensorInterface,
+				optional,
+				intPin,
+				extraParam,
+			});
+		}
+
+		bool working = sensor->isWorking();
+		m_Manager->m_Sensors.push_back(std::move(sensor));
+
+		if (!working) {
+			return false;
+		}
+
+		m_Manager->m_Logger.info("Sensor %d configured", sensorID);
+		return true;
+	}
+
+	template <typename ImuType>
+	std::unique_ptr<::Sensor> buildSensor(SensorDefinition sensorDef) {
+		m_Manager->m_Logger.trace(
+			"Building IMU with: id=%d,\n\
+						address=%s, rotation=%f,\n\
+						interface=%s, int=%s, extraParam=%d, optional=%d",
+			sensorDef.sensorID,
+			sensorDef.imuInterface.toString().c_str(),
+			sensorDef.rotation,
+			sensorDef.sensorInterface->toString().c_str(),
+			sensorDef.intPin ? sensorDef.intPin->toString().c_str() : "None",
+			sensorDef.extraParam,
+			sensorDef.optional
+		);
+
+		// Now start detecting and building the IMU
+		std::unique_ptr<::Sensor> sensor;
+
+		// Init I2C bus for each sensor upon startup
+		sensorDef.sensorInterface->init();
+		sensorDef.sensorInterface->swapIn();
+
+		if (!sensorDef.imuInterface.hasSensorOnBus()) {
+			if (!sensorDef.optional) {
+				m_Manager->m_Logger.error(
+					"Mandatory sensor %d not found at address %s",
+					sensorDef.sensorID + 1,
+					sensorDef.imuInterface.toString().c_str()
+				);
+				return std::make_unique<ErroneousSensor>(
+					sensorDef.sensorID,
+					ImuType::TypeID
+				);
+			} else {
+				m_Manager->m_Logger.debug(
+					"Optional sensor %d not found at address %s",
+					sensorDef.sensorID + 1,
+					sensorDef.imuInterface.toString().c_str()
+				);
+				return std::make_unique<EmptySensor>(sensorDef.sensorID);
+			}
+		}
+
+		m_Manager->m_Logger.trace(
+			"Sensor %d found at address %s",
+			sensorDef.sensorID + 1,
+			sensorDef.imuInterface.toString().c_str()
+		);
+
+		sensor = std::make_unique<ImuType>(
+			sensorDef.sensorID,
+			sensorDef.imuInterface,
+			sensorDef.rotation,
+			sensorDef.sensorInterface,
+			sensorDef.intPin,
+			sensorDef.extraParam
+		);
+
+		sensor->motionSetup();
+		return sensor;
+	}
+
+private:
+	SensorInterfaceManager interfaceManager;
+};
+
+}  // namespace SlimeVR::Sensors

src/sensors/SensorFusion.cpp

@@ -1,181 +1,134 @@
 #include "SensorFusion.h"
 
-namespace SlimeVR
-{
-    namespace Sensors
-    {
-        
-        void SensorFusion::update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat)
-        {
-            updateAcc(Axyz, deltat);
-            updateGyro(Gxyz, deltat);
-        }
+namespace SlimeVR::Sensors {
 
-        void SensorFusion::update9D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t Mxyz[3], sensor_real_t deltat)
-        {
-            updateMag(Mxyz, deltat);
-            updateAcc(Axyz, deltat);
-            updateGyro(Gxyz, deltat);
-        }
-
-        void SensorFusion::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat)
-        {
-            if (deltat < 0) deltat = accTs;
-
-            std::copy(Axyz, Axyz+3, bAxyz);
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                accelUpdated = true;
-            #elif SENSOR_USE_BASICVQF
-                basicvqf.updateAcc(Axyz);
-            #elif SENSOR_USE_VQF
-                vqf.updateAcc(Axyz);
-            #endif
-        }
-
-        void SensorFusion::updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat)
-        {
-            if (deltat < 0) deltat = magTs;
-
-            if (!magExist) {
-                if (Mxyz[0] != 0.0f || Mxyz[1] != 0.0f || Mxyz[2] != 0.0f) {
-                    magExist = true;
-                } else {
-                    return;
-                }
-            }
-
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                std::copy(Mxyz, Mxyz+3, bMxyz);
-            #elif SENSOR_USE_BASICVQF
-                basicvqf.updateMag(Mxyz);
-            #elif SENSOR_USE_VQF
-                vqf.updateMag(Mxyz);
-            #endif
-        }
-
-        void SensorFusion::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat)
-        {
-            if (deltat < 0) deltat = gyrTs;
-
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                sensor_real_t Axyz[3] {0.0f, 0.0f, 0.0f};
-                if (accelUpdated) {
-                    std::copy(bAxyz, bAxyz+3, Axyz);
-                    accelUpdated = false;
-                }
-            #endif
-
-            #if SENSOR_USE_MAHONY
-                if (!magExist) {
-                    mahony.update(qwxyz, Axyz[0], Axyz[1], Axyz[2],
-                                         Gxyz[0], Gxyz[1], Gxyz[2],
-                                         deltat);
-                } else {
-                    mahony.update(qwxyz,  Axyz[0],  Axyz[1],  Axyz[2],
-                                          Gxyz[0],  Gxyz[1],  Gxyz[2],
-                                         bMxyz[0], bMxyz[1], bMxyz[2],
-                                         deltat);
-                }
-            #elif SENSOR_USE_MADGWICK
-                if (!magExist) {
-                    madgwick.update(qwxyz, Axyz[0], Axyz[1], Axyz[2],
-                                           Gxyz[0], Gxyz[1], Gxyz[2],
-                                           deltat);
-                } else {
-                    madgwick.update(qwxyz,  Axyz[0],  Axyz[1],  Axyz[2],
-                                            Gxyz[0],  Gxyz[1],  Gxyz[2],
-                                           bMxyz[0], bMxyz[1], bMxyz[2],
-                                           deltat);
-                }
-            #elif SENSOR_USE_BASICVQF
-                basicvqf.updateGyr(Gxyz, deltat);
-            #elif SENSOR_USE_VQF
-                vqf.updateGyr(Gxyz, deltat);
-            #endif
-
-            updated = true;
-            gravityReady = false;
-            linaccelReady = false;
-        }
-
-        bool SensorFusion::isUpdated()
-        {
-            return updated;
-        }
-
-        void SensorFusion::clearUpdated()
-        {
-            updated = false;
-        }
-        
-        sensor_real_t const * SensorFusion::getQuaternion()
-        {
-            #if SENSOR_USE_BASICVQF
-                if(magExist) {
-                    basicvqf.getQuat9D(qwxyz);
-                } else {
-                    basicvqf.getQuat6D(qwxyz);
-                }
-            #elif SENSOR_USE_VQF
-                if(magExist) {
-                    vqf.getQuat9D(qwxyz);
-                } else {
-                    vqf.getQuat6D(qwxyz);
-                }
-            #endif
-
-            return qwxyz;
-        }
-
-        Quat SensorFusion::getQuaternionQuat()
-        {
-            getQuaternion();
-            return Quat(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
-        }
-
-        sensor_real_t const * SensorFusion::getGravityVec()
-        {
-            if (!gravityReady) {
-                calcGravityVec(qwxyz, vecGravity);
-                gravityReady = true;
-            }
-            return vecGravity;
-        }
-
-        sensor_real_t const * SensorFusion::getLinearAcc()
-        {
-            if (!linaccelReady) {
-                getGravityVec();
-                calcLinearAcc(bAxyz, vecGravity, linAccel);
-                linaccelReady = true;
-            }
-            return linAccel;
-        }
-
-        void SensorFusion::getLinearAcc(sensor_real_t outLinAccel[3])
-        {
-            getLinearAcc();
-            std::copy(linAccel, linAccel+3, outLinAccel);
-        }
-
-        Vector3 SensorFusion::getLinearAccVec()
-        {
-            getLinearAcc();
-            return Vector3(linAccel[0], linAccel[1], linAccel[2]);
-        }
-
-        void SensorFusion::calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3])
-        {
-            gravVec[0] = 2 * (qwxyz[1] * qwxyz[3] - qwxyz[0] * qwxyz[2]);
-            gravVec[1] = 2 * (qwxyz[0] * qwxyz[1] + qwxyz[2] * qwxyz[3]);
-            gravVec[2] = qwxyz[0]*qwxyz[0] - qwxyz[1]*qwxyz[1] - qwxyz[2]*qwxyz[2] + qwxyz[3]*qwxyz[3];
-        }
-        
-        void SensorFusion::calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3])
-        {
-            accout[0] = accin[0] - gravVec[0] * CONST_EARTH_GRAVITY;
-            accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY;
-            accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
-        }
-    }
+void SensorFusion::update6D(
+	sensor_real_t Axyz[3],
+	sensor_real_t Gxyz[3],
+	sensor_real_t deltat
+) {
+	updateAcc(Axyz, deltat);
+	updateGyro(Gxyz, deltat);
 }
+
+void SensorFusion::update9D(
+	sensor_real_t Axyz[3],
+	sensor_real_t Gxyz[3],
+	sensor_real_t Mxyz[3],
+	sensor_real_t deltat
+) {
+	updateMag(Mxyz, deltat);
+	updateAcc(Axyz, deltat);
+	updateGyro(Gxyz, deltat);
+}
+
+void SensorFusion::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat) {
+	if (deltat < 0) {
+		deltat = accTs;
+	}
+
+	std::copy(Axyz, Axyz + 3, bAxyz);
+	vqf.updateAcc(Axyz);
+}
+
+void SensorFusion::updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat) {
+	if (deltat < 0) {
+		deltat = magTs;
+	}
+
+	if (!magExist) {
+		if (Mxyz[0] != 0.0f || Mxyz[1] != 0.0f || Mxyz[2] != 0.0f) {
+			magExist = true;
+		} else {
+			return;
+		}
+	}
+
+	vqf.updateMag(Mxyz);
+}
+
+void SensorFusion::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat) {
+	if (deltat < 0) {
+		deltat = gyrTs;
+	}
+
+	vqf.updateGyr(Gxyz, deltat);
+
+	updated = true;
+	gravityReady = false;
+	linaccelReady = false;
+}
+
+bool SensorFusion::isUpdated() { return updated; }
+
+void SensorFusion::clearUpdated() { updated = false; }
+
+sensor_real_t const* SensorFusion::getQuaternion() {
+	if (magExist) {
+		vqf.getQuat9D(qwxyz);
+	} else {
+		vqf.getQuat6D(qwxyz);
+	}
+
+	return qwxyz;
+}
+
+Quat SensorFusion::getQuaternionQuat() {
+	getQuaternion();
+	return Quat(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
+}
+
+sensor_real_t const* SensorFusion::getGravityVec() {
+	if (!gravityReady) {
+		calcGravityVec(qwxyz, vecGravity);
+		gravityReady = true;
+	}
+	return vecGravity;
+}
+
+sensor_real_t const* SensorFusion::getLinearAcc() {
+	if (!linaccelReady) {
+		getGravityVec();
+		calcLinearAcc(bAxyz, vecGravity, linAccel);
+		linaccelReady = true;
+	}
+	return linAccel;
+}
+
+void SensorFusion::getLinearAcc(sensor_real_t outLinAccel[3]) {
+	getLinearAcc();
+	std::copy(linAccel, linAccel + 3, outLinAccel);
+}
+
+Vector3 SensorFusion::getLinearAccVec() {
+	getLinearAcc();
+	return Vector3(linAccel[0], linAccel[1], linAccel[2]);
+}
+
+void SensorFusion::calcGravityVec(
+	const sensor_real_t qwxyz[4],
+	sensor_real_t gravVec[3]
+) {
+	gravVec[0] = 2 * (qwxyz[1] * qwxyz[3] - qwxyz[0] * qwxyz[2]);
+	gravVec[1] = 2 * (qwxyz[0] * qwxyz[1] + qwxyz[2] * qwxyz[3]);
+	gravVec[2] = qwxyz[0] * qwxyz[0] - qwxyz[1] * qwxyz[1] - qwxyz[2] * qwxyz[2]
+			   + qwxyz[3] * qwxyz[3];
+}
+
+void SensorFusion::calcLinearAcc(
+	const sensor_real_t accin[3],
+	const sensor_real_t gravVec[3],
+	sensor_real_t accout[3]
+) {
+	accout[0] = accin[0] - gravVec[0] * CONST_EARTH_GRAVITY;
+	accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY;
+	accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
+}
+
+void SensorFusion::updateBiasForgettingTime(float biasForgettingTime) {
+	vqf.updateBiasForgettingTime(biasForgettingTime);
+}
+
+bool SensorFusion::getRestDetected() const { return vqf.getRestDetected(); }
+
+}  // namespace SlimeVR::Sensors

src/sensors/SensorFusion.h

@@ -6,128 +6,102 @@
 
 #define SENSOR_DOUBLE_PRECISION 0
 
-#define SENSOR_FUSION_TYPE SENSOR_FUSION_VQF
-
-#define SENSOR_FUSION_MAHONY 1
-#define SENSOR_FUSION_MADGWICK 2
-#define SENSOR_FUSION_BASICVQF 3
-#define SENSOR_FUSION_VQF 4
-
-#if SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY
-    #define SENSOR_FUSION_TYPE_STRING "mahony"
-#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK
-    #define SENSOR_FUSION_TYPE_STRING "madgwick"
-#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF
-    #define SENSOR_FUSION_TYPE_STRING "bvqf"
-#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF
-    #define SENSOR_FUSION_TYPE_STRING "vqf"
-#endif
-
-#define SENSOR_USE_MAHONY (SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY)
-#define SENSOR_USE_MADGWICK (SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK)
-#define SENSOR_USE_BASICVQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF)
-#define SENSOR_USE_VQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF)
+#define SENSOR_FUSION_TYPE_STRING "vqf"
 
+#include <vqf.h>
 
 #include "../motionprocessing/types.h"
 
-#include "mahony.h"
-#include "madgwick.h"
-#include <vqf.h>
-#include <basicvqf.h>
+namespace SlimeVR::Sensors {
+constexpr VQFParams DefaultVQFParams = VQFParams{
+	.tauAcc = 2.0f,
+	.restMinT = 2.0f,
+	.restThGyr = 0.6f,
+	.restThAcc = 0.06f,
+};
 
-namespace SlimeVR
-{
-    namespace Sensors
-    {
-        #if SENSOR_USE_VQF
-        struct SensorVQFParams: VQFParams {
-            SensorVQFParams() : VQFParams() {
-                #ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-                motionBiasEstEnabled = true;
-                #endif
-                tauAcc = 2.0f;
-                restMinT = 2.0f;
-                restThGyr = 0.6f; // 400 norm
-                restThAcc = 0.06f; // 100 norm
-            }
-        };
-        #endif
+class SensorFusion {
+public:
+	SensorFusion(
+		VQFParams vqfParams,
+		sensor_real_t gyrTs,
+		sensor_real_t accTs = -1.0,
+		sensor_real_t magTs = -1.0
+	)
+		: gyrTs(gyrTs)
+		, accTs((accTs < 0) ? gyrTs : accTs)
+		, magTs((magTs < 0) ? gyrTs : magTs)
+		, vqfParams(vqfParams)
+		, vqf(this->vqfParams,
+			  gyrTs,
+			  ((accTs < 0) ? gyrTs : accTs),
+			  ((magTs < 0) ? gyrTs : magTs)) {}
 
-        class SensorFusion
-        {
-        public:
-            SensorFusion(sensor_real_t gyrTs, sensor_real_t accTs=-1.0, sensor_real_t magTs=-1.0)
-                : gyrTs(gyrTs), 
-                  accTs( (accTs<0) ? gyrTs : accTs ), 
-                  magTs( (magTs<0) ? gyrTs : magTs )
-            #if SENSOR_USE_MAHONY
-            #elif SENSOR_USE_MADGWICK
-            #elif SENSOR_USE_BASICVQF
-                , basicvqf(gyrTs, ((accTs<0) ? gyrTs : accTs),
-                                    ((magTs<0) ? gyrTs : magTs))
-            #elif SENSOR_USE_VQF
-                , vqf(vqfParams, gyrTs, ((accTs<0) ? gyrTs : accTs), 
-                                        ((magTs<0) ? gyrTs : magTs))
-            #endif
-            {}
+	explicit SensorFusion(
+		sensor_real_t gyrTs,
+		sensor_real_t accTs = -1.0,
+		sensor_real_t magTs = -1.0
+	)
+		: SensorFusion(DefaultVQFParams, gyrTs, accTs, magTs) {}
 
-            void update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f);
-            void update9D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f);
-            void updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat=-1.0f);
-            void updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f);
-            void updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f);
+	void update6D(
+		sensor_real_t Axyz[3],
+		sensor_real_t Gxyz[3],
+		sensor_real_t deltat = -1.0f
+	);
+	void update9D(
+		sensor_real_t Axyz[3],
+		sensor_real_t Gxyz[3],
+		sensor_real_t Mxyz[3],
+		sensor_real_t deltat = -1.0f
+	);
+	void updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat = -1.0f);
+	void updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat = -1.0f);
+	void updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat = -1.0f);
 
-            bool isUpdated();
-            void clearUpdated();
-            sensor_real_t const * getQuaternion();
-            Quat getQuaternionQuat();
-            sensor_real_t const * getGravityVec();
-            sensor_real_t const * getLinearAcc();
-            void getLinearAcc(sensor_real_t outLinAccel[3]);
-            Vector3 getLinearAccVec();
+	bool isUpdated();
+	void clearUpdated();
+	sensor_real_t const* getQuaternion();
+	Quat getQuaternionQuat();
+	sensor_real_t const* getGravityVec();
+	sensor_real_t const* getLinearAcc();
+	void getLinearAcc(sensor_real_t outLinAccel[3]);
+	Vector3 getLinearAccVec();
 
-            static void calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3]);
-            static void calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3]);
+	static void calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3]);
+	static void calcLinearAcc(
+		const sensor_real_t accin[3],
+		const sensor_real_t gravVec[3],
+		sensor_real_t accout[3]
+	);
 
-        protected:
-            sensor_real_t gyrTs;
-            sensor_real_t accTs;
-            sensor_real_t magTs;
+	void updateBiasForgettingTime(float biasForgettingTime);
 
-            #if SENSOR_USE_MAHONY
-                Mahony<sensor_real_t> mahony;
-            #elif SENSOR_USE_MADGWICK
-                Madgwick<sensor_real_t> madgwick;
-            #elif SENSOR_USE_BASICVQF
-                BasicVQF basicvqf;
-            #elif SENSOR_USE_VQF
-                SensorVQFParams vqfParams {};
-                VQF vqf;
-            #endif
+	[[nodiscard]] bool getRestDetected() const;
 
-            // A also used for linear acceleration extraction
-            sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
+protected:
+	sensor_real_t gyrTs;
+	sensor_real_t accTs;
+	sensor_real_t magTs;
 
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                // Buffer M here to keep the behavior of BMI160
-                sensor_real_t bMxyz[3]{0.0f, 0.0f, 0.0f};
-                bool accelUpdated = false;
-            #endif
+	VQFParams vqfParams;
+	VQF vqf;
 
-            bool magExist = false;
-            sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
-            bool updated = false;
+	// A also used for linear acceleration extraction
+	sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
 
-            bool gravityReady = false;
-            sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
-            bool linaccelReady = false;
-            sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
-            #if ESP32
-            sensor_real_t linAccel_guard;   // Temporary patch for some weird ESP32 bug
-            #endif
-        };
-    }
-}
+	bool magExist = false;
+	sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
+	bool updated = false;
 
-#endif // SLIMEVR_SENSORFUSION_H
+	bool gravityReady = false;
+	sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
+	bool linaccelReady = false;
+	sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
+#ifdef ESP32
+	sensor_real_t linAccel_guard;  // Temporary patch for some weird ESP32 bug
+#endif
+};
+}  // namespace SlimeVR::Sensors
+
+#endif  // SLIMEVR_SENSORFUSION_H