Add nrf const to consts.h (#505)

Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 6 to 7.
- [Release notes](https://github.com/actions/upload-artifact/releases)
- [Commits](https://github.com/actions/upload-artifact/compare/v6...v7)

---
updated-dependencies:
- dependency-name: actions/upload-artifact
  dependency-version: '7'
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

.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/dependabot.yml

@@ -0,0 +1,8 @@
+version: 2
+updates:
+
+  - package-ecosystem: "github-actions"
+    directory: "/"
+    schedule:
+      # Check for updates to GitHub Actions every week
+      interval: "weekly"

.github/workflows/actions.yml

@@ -2,28 +2,73 @@ name: Build
 
 on:
   push:
+    branches:
+      - main
+    tags:
+      - "*"
   pull_request:
+  workflow_dispatch:
+  create:
 
 jobs:
+  format:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v6
+      - 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@v2
+      - uses: actions/checkout@v6
+      - uses: actions/cache@v5
+        with:
+          path: |
+            ~/.cache/pip
+            ~/.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@v1
+        uses: actions/setup-python@v6
+        with:
+          python-version: "3.12"
 
-      - name: Install pio and its dependencies
+      - name: Install PlatformIO and its dependencies
         run: |
           python -m pip install --upgrade pip
-          pip install platformio
+          pip install --upgrade platformio
 
       - name: Run builds
         run: python ./ci/build.py
 
       - name: Upload binaries
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v7
         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@v2
-    - uses: ncipollo/release-action@v1
-      with:
-        artifacts: "./build/*.bin"
-        draft: true
-        token: ${{ secrets.GITHUB_TOKEN }}

.gitignore

@@ -2,3 +2,9 @@
 .vscode/*
 build/
 venv/
+cache/
+.idea/
+compile_commands.json
+node_modules/
+dist/
+.nix-platformio

README.md

@@ -35,6 +35,12 @@ The following IMUs and their corresponding `IMU` values are supported by the fir
   * Using fusion in internal DMP for 6Dof or 9DoF, 9DoF mode requires good magnetic environment.
   * Comment out `USE_6DOF` in `debug.h` for 9DoF mode.
   * Experimental support!
+* BMI270 (IMU_BMI270), ICM-42688 (IMU_ICM42688), LSM6DS3TR-C (IMU_LSM6DS3TRC), LSM6DSV (IMU_LSM6DSV), LSM6DSO (IMU_LSM6DSO), LSM6DSR (IMU_LSM6DSR), MPU-6050 (IMU_MPU6050_SF)
+  * Using common code: SoftFusionSensor for sensor fusion of Gyroscope and Accelerometer.
+  * Gyro&Accel sample rate, gyroscope offset and 6-side accelerometer calibration supported.
+  * In case of BMI270, gyroscope sensitivity auto-calibration (CRT) is additionally performed.
+  * Support for magnetometers is currently not implemented.
+  * VERY experimental support!
 
 Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set your pinout properly according to how you connected the IMU.
 
@@ -95,13 +101,6 @@ Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set y
 
   - Calibration data is written to the flash of your MCU and is unique for each BMI160, keep that in mind if you have detachable aux trackers.
 
-## Infos about ESP32-C3 with direct connection to USB
-
-The ESP32-C3 has two ways to connect the serial port. One is directly via the onboard USB CDC or via the onboard UART.
-When the chip is connected to the USB CDC, the serial port shows as `USB Serial Port` in Device Manager. The SlimeVR server will currently not connect to this port.
-If you want to set your WiFi credentials, you can use the PlatformIO serial console.
-There you have to enter the following: `SET WIFI "SSID" "PASSWORD"`
-
 ## Uploading On Linux
 
 Follow the instructions in this link [PlatformIO](https://docs.platformio.org/en/latest//faq.html#platformio-udev-rules), this should solve any permission denied errors

board-defaults.json

@@ -0,0 +1,590 @@
+{
+	"$schema": "board-defaults.schema.json",
+	"toolchain": "platformio",
+	"defaults": {
+		"BOARD_SLIMEVR": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_AUTO",
+						"int": "16",
+						"rotation": "DEG_270",
+						"scl": "12",
+						"sda": "14"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_AUTO",
+						"int": "13",
+						"rotation": "DEG_270",
+						"scl": "12",
+						"sda": "14"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 10,
+					"r2": 40.2,
+					"shieldR": 0,
+					"pin": "17"
+				},
+				"LED": {
+					"LED_PIN": "2",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 0,
+				"needBootPress": true,
+				"needManualReboot": true,
+				"shouldOnlyUseDefaults": true
+			}
+		},
+		"BOARD_SLIMEVR_V1_2": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "SPI",
+						"imu": "IMU_AUTO",
+						"int": "2",
+						"cs": "15",
+						"rotation": "DEG_270"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_AUTO",
+						"int": "16",
+						"rotation": "DEG_270",
+						"scl": "5",
+						"sda": "4"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 10,
+					"r2": 40.2,
+					"shieldR": 0,
+					"pin": "17"
+				},
+				"LED": {
+					"LED_PIN": "2",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 0,
+				"needBootPress": true,
+				"needManualReboot": true,
+				"shouldOnlyUseDefaults": true
+			}
+		},
+		"BOARD_SLIMEVR_DEV": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_AUTO",
+						"int": "10",
+						"rotation": "DEG_270",
+						"scl": "5",
+						"sda": "4"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_AUTO",
+						"int": "13",
+						"rotation": "DEG_270",
+						"scl": "5",
+						"sda": "4"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 10,
+					"r2": 40.2,
+					"shieldR": 0,
+					"pin": "17"
+				},
+				"LED": {
+					"LED_PIN": "2",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 0,
+				"needBootPress": true,
+				"needManualReboot": true,
+				"shouldOnlyUseDefaults": true
+			}
+		},
+		"BOARD_WEMOSD1MINI": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_AUTO",
+						"int": "D5",
+						"rotation": "DEG_270",
+						"scl": "D1",
+						"sda": "D2"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_AUTO",
+						"int": "D6",
+						"rotation": "DEG_270",
+						"scl": "D1",
+						"sda": "D2"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "A0"
+				},
+				"LED": {
+					"LED_PIN": "2",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 0,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_NODEMCU": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "D5",
+						"rotation": "DEG_270",
+						"scl": "D1",
+						"sda": "D2"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "D6",
+						"rotation": "DEG_270",
+						"scl": "D1",
+						"sda": "D2"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "A0"
+				},
+				"LED": {
+					"LED_PIN": "2",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 0,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_ESP01": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "255",
+						"rotation": "DEG_270",
+						"scl": "0",
+						"sda": "2"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "255",
+						"rotation": "DEG_270",
+						"scl": "0",
+						"sda": "2"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_INTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "255"
+				},
+				"LED": {
+					"LED_PIN": "255",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 0,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_TTGO_TBASE": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "14",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "13",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 0,
+					"pin": "A0"
+				},
+				"LED": {
+					"LED_PIN": "2",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 0,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_WROOM32": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "23",
+						"rotation": "DEG_270",
+						"scl": "22",
+						"sda": "21"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_BNO085",
+						"int": "25",
+						"rotation": "DEG_270",
+						"scl": "22",
+						"sda": "21"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "36"
+				},
+				"LED": {
+					"LED_PIN": "255",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_LOLIN_C3_MINI": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "6",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "8",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "3"
+				},
+				"LED": {
+					"LED_PIN": "7",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_BEETLE32C3": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "6",
+						"rotation": "DEG_270",
+						"scl": "9",
+						"sda": "8"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "7",
+						"rotation": "DEG_270",
+						"scl": "9",
+						"sda": "8"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "3"
+				},
+				"LED": {
+					"LED_PIN": "10",
+					"LED_INVERTED": false
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_ESP32C3DEVKITM1": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "6",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "7",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "3"
+				},
+				"LED": {
+					"LED_PIN": "LED_BUILTIN",
+					"LED_INVERTED": false
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_ESP32C6DEVKITC1": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "6",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "7",
+						"rotation": "DEG_270",
+						"scl": "4",
+						"sda": "5"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "3"
+				},
+				"LED": {
+					"LED_PIN": "LED_BUILTIN",
+					"LED_INVERTED": false
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_WEMOSWROOM02": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "0",
+						"rotation": "DEG_270",
+						"scl": "14",
+						"sda": "2"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "4",
+						"rotation": "DEG_270",
+						"scl": "14",
+						"sda": "2"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 220,
+					"shieldR": 180,
+					"pin": "A0"
+				},
+				"LED": {
+					"LED_PIN": "16",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_XIAO_ESP32C3": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "5",
+						"rotation": "DEG_270",
+						"scl": "7",
+						"sda": "6"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "10",
+						"rotation": "DEG_270",
+						"scl": "7",
+						"sda": "6"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 100,
+					"r2": 100,
+					"shieldR": 0,
+					"pin": "2"
+				},
+				"LED": {
+					"LED_PIN": "4",
+					"LED_INVERTED": false
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		},
+		"BOARD_ESP32S3_SUPERMINI": {
+			"values": {
+				"SENSORS": [
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "5",
+						"rotation": "DEG_270",
+						"scl": "6",
+						"sda": "7"
+					},
+					{
+						"protocol": "I2C",
+						"imu": "IMU_ICM45686",
+						"int": "4",
+						"rotation": "DEG_270",
+						"scl": "6",
+						"sda": "7"
+					}
+				],
+				"BATTERY": {
+					"type": "BAT_EXTERNAL",
+					"r1": 10,
+					"r2": 40.2,
+					"shieldR": 0,
+					"pin": "A2"
+				},
+				"LED": {
+					"LED_PIN": "LED_BUILTIN",
+					"LED_INVERTED": true
+				}
+			},
+			"flashingRules": {
+				"applicationOffset": 65536,
+				"needBootPress": false,
+				"needManualReboot": false,
+				"shouldOnlyUseDefaults": false
+			}
+		}
+	}
+}

board-defaults.schema.json

@@ -0,0 +1,244 @@
+{
+	"$id": "board-defaults.schema.json",
+	"$schema": "https://json-schema.org/draft/2020-12/schema",
+
+	"$defs": {
+		"pin": {
+			"type": "string",
+			"pattern": "^[AD]?[0-9]\\d*$"
+		},
+
+		"LED_PIN": {
+			"type": "string",
+			"pattern": "^([AD]?[0-9]\\d*|LED_BUILTIN)$"
+		},
+
+		"IMU_TYPE": {
+			"type": "string",
+			"enum": [
+				"IMU_AUTO",
+				"IMU_MPU9250",
+				"IMU_MPU6500",
+				"IMU_BNO080",
+				"IMU_BNO085",
+				"IMU_BNO055",
+				"IMU_MPU6050",
+				"IMU_BNO086",
+				"IMU_BMI160",
+				"IMU_ICM20948",
+				"IMU_ICM42688",
+				"IMU_BMI270",
+				"IMU_LSM6DS3TRC",
+				"IMU_LSM6DSV",
+				"IMU_LSM6DSO",
+				"IMU_LSM6DSR",
+				"IMU_MPU6050_SF",
+				"IMU_ICM45686",
+				"IMU_ICM45605"
+			],
+			"description": "Imu Type"
+		},
+
+		"PROTOCOL": {
+			"type": "string",
+			"enum": ["I2C", "SPI"],
+			"description": "Protocol"
+		},
+
+		"IMU_ROTATION": {
+			"type": "string",
+			"enum": ["DEG_0", "DEG_90", "DEG_180", "DEG_270"],
+			"description": "Protocol"
+		},
+
+		"I2C_IMU": {
+			"type": "object",
+			"description": "I2C Imu",
+			"properties": {
+				"protocol": { "const": "I2C" },
+				"imu": { "$ref": "#/$defs/IMU_TYPE" },
+				"sda": { "$ref": "#/$defs/pin", "description": "SDA Pin" },
+				"scl": { "$ref": "#/$defs/pin", "description": "SCL Pin" },
+				"int": { "$ref": "#/$defs/pin", "description": "INT Pin" },
+				"address": {
+					"type": "number",
+					"description": "IMU Address"
+				},
+				"rotation": {
+					"$ref": "#/$defs/IMU_ROTATION",
+					"description": "IMU Rotation"
+				}
+			},
+			"required": ["protocol", "imu", "sda", "scl"]
+		},
+
+		"SPI_IMU": {
+			"type": "object",
+			"description": "SPI Imu",
+			"properties": {
+				"protocol": { "const": "SPI" },
+				"imu": { "$ref": "#/$defs/IMU_TYPE" },
+				"int": { "$ref": "#/$defs/pin", "description": "INT Pin" },
+				"rotation": {
+					"$ref": "#/$defs/IMU_ROTATION",
+					"description": "IMU Rotation"
+				},
+				"cs": { "$ref": "#/$defs/pin", "description": "CS Pin" }
+			},
+			"required": ["protocol", "imu", "cs"]
+		},
+
+		"IMU": {
+			"type": "object",
+			"discriminator": {
+				"propertyName": "protocol"
+			},
+			"oneOf": [
+				{ "$ref": "#/$defs/I2C_IMU" },
+				{ "$ref": "#/$defs/SPI_IMU" }
+			],
+			"required": ["protocol"]
+		},
+
+		"BOARD_TYPES": {
+			"type": "string",
+			"enum": [
+				"BOARD_SLIMEVR",
+				"BOARD_SLIMEVR_V1_2",
+				"BOARD_SLIMEVR_DEV",
+				"BOARD_NODEMCU",
+				"BOARD_WEMOSD1MINI",
+				"BOARD_ESP01",
+				"BOARD_TTGO_TBASE",
+				"BOARD_WROOM32",
+				"BOARD_LOLIN_C3_MINI",
+				"BOARD_BEETLE32C3",
+				"BOARD_ESP32C3DEVKITM1",
+				"BOARD_ESP32C6DEVKITC1",
+				"BOARD_WEMOSWROOM02",
+				"BOARD_XIAO_ESP32C3",
+				"BOARD_ESP32S3_SUPERMINI"
+			],
+			"description": "Board Type"
+		},
+
+		"BATTERY": {
+			"type": "object",
+			"discriminator": {
+				"propertyName": "type"
+			},
+			"description": "Battery Settings",
+			"oneOf": [
+				{
+					"type": "object",
+					"properties": {
+						"type": { "const": "BAT_EXTERNAL" },
+						"shieldR": {
+							"type": "number",
+							"description": "Battery Shield Resistor (Ohms)"
+						},
+						"r1": { "type": "number", "description": "R1 (Ohms)" },
+						"r2": { "type": "number", "description": "R2 (Ohms)" },
+						"pin": {
+							"$ref": "#/$defs/pin",
+							"description": "Battery Pin"
+						}
+					}
+				},
+				{
+					"type": "object",
+					"properties": {
+						"type": { "const": "BAT_INTERNAL" }
+					}
+				},
+				{
+					"type": "object",
+					"properties": {
+						"type": { "const": "BAT_MCP3021" }
+					}
+				},
+				{
+					"type": "object",
+					"properties": {
+						"type": { "const": "BAT_INTERNAL_MCP3021" }
+					}
+				}
+			],
+			"required": ["type"]
+		},
+
+		"BASIC_IMU_BOARD_CONFIG": {
+			"type": "object",
+			"properties": {
+				"SENSORS": {
+					"type": "array",
+					"items": { "$ref": "#/$defs/IMU" },
+					"minItems": 1,
+					"maxItems": 2,
+					"description": "Sensors List"
+				},
+				"LED": {
+					"type": "object",
+					"description": "Led Settings",
+					"properties": {
+						"LED_PIN": {
+							"$ref": "#/$defs/LED_PIN",
+							"description": "Led pin"
+						},
+						"LED_INVERTED": {
+							"type": "boolean",
+							"description": "Led inverted"
+						}
+					},
+					"required": ["LED_PIN", "LED_INVERTED"]
+				},
+				"BATTERY": { "$ref": "#/$defs/BATTERY" }
+			},
+			"required": ["SENSORS", "LED", "BATTERY"]
+		},
+
+		"BoardConfig": {
+			"type": "object",
+			"properties": {
+				"values": {
+					"$ref": "#/$defs/BASIC_IMU_BOARD_CONFIG"
+				},
+				"flashingRules": {
+					"type": "object",
+					"properties": {
+						"needBootPress": { "type": "boolean" },
+						"needManualReboot": { "type": "boolean" },
+						"shouldOnlyUseDefaults": { "type": "boolean" },
+						"applicationOffset": { "type": "integer" }
+					},
+					"required": [
+						"needBootPress",
+						"needManualReboot",
+						"shouldOnlyUseDefaults",
+						"applicationOffset"
+					]
+				},
+				"ignored": {
+					"type": "boolean"
+				}
+			},
+			"required": ["values", "flashingRules"]
+		}
+	},
+
+	"type": "object",
+	"properties": {
+		"toolchain": {
+			"type": "string",
+			"enum": ["platformio"]
+		},
+		"defaults": {
+			"type": "object",
+			"propertyNames": {
+				"$ref": "#/$defs/BOARD_TYPES"
+			},
+			"additionalProperties": { "$ref": "#/$defs/BoardConfig" }
+		}
+	},
+	"required": ["toolchain", "defaults"]
+}

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}"
@@ -32,12 +32,13 @@ def get_matrix() -> List[DeviceConfiguration]:
     matrix: List[DeviceConfiguration] = []
 
     config = configparser.ConfigParser()
-    config.read("./platformio-tools.ini")
+    config.read("./platformio.ini")
     for section in config.sections():
-        if section == "env":
+        split = section.split(":")
+        if len(split) != 2 or split[0] != 'env':
             continue
 
-        board = section.split(":")[1]
+        board = split[1]
         platform = config[section]["platform"]
         platformio_board = config[section]["board"]
 
@@ -51,36 +52,15 @@ def get_matrix() -> List[DeviceConfiguration]:
 
 def prepare() -> None:
     print(f"🡢 {COLOR_CYAN}Preparation{COLOR_RESET}")
-
-    print(f"  🡢 {COLOR_GRAY}Backing up platformio.ini{COLOR_RESET}")
-    shutil.copy("./platformio.ini", "platformio.ini.bak")
-
-    print(
-        f"  🡢 {COLOR_GRAY}Switching platformio.ini to platformio-tools.ini{COLOR_RESET}")
-    shutil.copy("./platformio-tools.ini", "platformio.ini")
-
     if os.path.exists("./build"):
         print(f"  🡢 {COLOR_GRAY}Removing existing build folder...{COLOR_RESET}")
         shutil.rmtree("./build")
-
     print(f"  🡢 {COLOR_GRAY}Creating build folder...{COLOR_RESET}")
     os.mkdir("./build")
 
     print(f"  🡢 {COLOR_GREEN}Success!{COLOR_RESET}")
 
 
-def cleanup() -> None:
-    print(f"🡢 {COLOR_CYAN}Cleanup{COLOR_RESET}")
-
-    print(f"  🡢 {COLOR_GRAY}Restoring platformio.ini...{COLOR_RESET}")
-    shutil.copy("platformio.ini.bak", "platformio.ini")
-
-    print(f"  🡢 {COLOR_GRAY}Removing platformio.ini.bak...{COLOR_RESET}")
-    os.remove("platformio.ini.bak")
-
-    print(f"  🡢 {COLOR_GREEN}Success!{COLOR_RESET}")
-
-
 def build() -> int:
     print(f"🡢 {COLOR_CYAN}Build{COLOR_RESET}")
 
@@ -95,7 +75,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}")
@@ -135,7 +115,6 @@ def build_for_device(device: DeviceConfiguration) -> bool:
 def main() -> None:
     prepare()
     code = build()
-    cleanup()
 
     sys.exit(code)
 

flake.lock

@@ -0,0 +1,61 @@
+{
+  "nodes": {
+    "flake-utils": {
+      "inputs": {
+        "systems": "systems"
+      },
+      "locked": {
+        "lastModified": 1731533236,
+        "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
+        "type": "github"
+      },
+      "original": {
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "type": "github"
+      }
+    },
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1760878510,
+        "narHash": "sha256-K5Osef2qexezUfs0alLvZ7nQFTGS9DL2oTVsIXsqLgs=",
+        "owner": "NixOS",
+        "repo": "nixpkgs",
+        "rev": "5e2a59a5b1a82f89f2c7e598302a9cacebb72a67",
+        "type": "github"
+      },
+      "original": {
+        "owner": "NixOS",
+        "ref": "nixos-unstable",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "root": {
+      "inputs": {
+        "flake-utils": "flake-utils",
+        "nixpkgs": "nixpkgs"
+      }
+    },
+    "systems": {
+      "locked": {
+        "lastModified": 1681028828,
+        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
+        "owner": "nix-systems",
+        "repo": "default",
+        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nix-systems",
+        "repo": "default",
+        "type": "github"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}

flake.nix

@@ -0,0 +1,80 @@
+{
+  description = "PlatformIO development environment";
+
+  inputs = {
+    nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable";
+    flake-utils.url = "github:numtide/flake-utils";
+  };
+
+  outputs = { self, nixpkgs, flake-utils }:
+    flake-utils.lib.eachDefaultSystem (system:
+      let
+        pkgs = nixpkgs.legacyPackages.${system};
+      in
+      {
+        devShells.default = pkgs.mkShell {
+          buildInputs = with pkgs; [
+            # PlatformIO
+            platformio
+            platformio-core
+
+            # Python for PlatformIO with needed packages
+            python3
+            python3Packages.pip
+            python3Packages.virtualenv
+
+            # Pre-install Python packages that need compilation
+            python3Packages.jsonschema
+            python3Packages.rpds-py
+            python3Packages.attrs
+            python3Packages.referencing
+
+            # Rust toolchain (in case compilation is needed)
+            rustc
+            cargo
+
+            # Build tools
+            gcc
+            gnumake
+            cmake
+
+            # Serial communication
+            picocom
+            minicom
+
+            # USB access (for programming devices)
+            libusb1
+            pkg-config
+          ];
+
+          shellHook = ''
+            # Set PlatformIO core directory to project-local directory
+            export PLATFORMIO_CORE_DIR=$PWD/.nix-platformio
+
+            # Create and activate Python virtual environment
+            if [ ! -d .venv ]; then
+              echo "Creating Python virtual environment..."
+              python3 -m venv .venv --system-site-packages
+            fi
+            source .venv/bin/activate
+
+            # Prefer binary wheels over building from source
+            export PIP_PREFER_BINARY=1
+
+            echo "PlatformIO development environment loaded"
+            echo "Python virtual environment activated: .venv"
+            echo "PlatformIO version: $(pio --version)"
+            echo "Python version: $(python --version)"
+            echo ""
+            echo "Available commands:"
+            echo "  pio init      - Initialize a new PlatformIO project"
+            echo "  pio run       - Build the project"
+            echo "  pio run -t upload  - Upload to device"
+            echo "  pio device monitor - Open serial monitor"
+            echo "  pip install <package>  - Install Python packages in venv"
+            echo ""
+          '';
+        };
+      }
+    );
+}

lib/ICM42688/ICM42688.h

@@ -1,192 +0,0 @@
-/* 01/14/2022 Copyright Tlera Corporation
-
-    Created by Kris Winer
-
-  This sketch uses SDA/SCL on pins 21/20 (ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board.
-  The ICM42688 is a combo sensor with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.
-
-  Library may be used freely and without limit with attribution.
-
-*/
-
-#ifndef ICM42688_h
-#define ICM42688_h
-
-/* ICM42688 registers
-https://media.digikey.com/pdf/Data%20Sheets/TDK%20PDFs/ICM-42688-P_DS_Rev1.2.pdf
-*/
-// User Bank 0
-#define ICM42688_DEVICE_CONFIG             0x11
-#define ICM42688_DRIVE_CONFIG              0x13
-#define ICM42688_INT_CONFIG                0x14
-#define ICM42688_FIFO_CONFIG               0x16
-#define ICM42688_TEMP_DATA1                0x1D
-#define ICM42688_TEMP_DATA0                0x1E
-#define ICM42688_ACCEL_DATA_X1             0x1F
-#define ICM42688_ACCEL_DATA_X0             0x20
-#define ICM42688_ACCEL_DATA_Y1             0x21
-#define ICM42688_ACCEL_DATA_Y0             0x22
-#define ICM42688_ACCEL_DATA_Z1             0x23
-#define ICM42688_ACCEL_DATA_Z0             0x24
-#define ICM42688_GYRO_DATA_X1              0x25
-#define ICM42688_GYRO_DATA_X0              0x26
-#define ICM42688_GYRO_DATA_Y1              0x27
-#define ICM42688_GYRO_DATA_Y0              0x28
-#define ICM42688_GYRO_DATA_Z1              0x29
-#define ICM42688_GYRO_DATA_Z0              0x2A
-#define ICM42688_TMST_FSYNCH               0x2B
-#define ICM42688_TMST_FSYNCL               0x2C
-#define ICM42688_INT_STATUS                0x2D
-#define ICM42688_FIFO_COUNTH               0x2E
-#define ICM42688_FIFO_COUNTL               0x2F
-#define ICM42688_FIFO_DATA                 0x30
-#define ICM42688_APEX_DATA0                0x31
-#define ICM42688_APEX_DATA1                0x32
-#define ICM42688_APEX_DATA2                0x33
-#define ICM42688_APEX_DATA3                0x34
-#define ICM42688_APEX_DATA4                0x35
-#define ICM42688_APEX_DATA5                0x36
-#define ICM42688_INT_STATUS2               0x37
-#define ICM42688_INT_STATUS3               0x38
-#define ICM42688_SIGNAL_PATH_RESET         0x4B
-#define ICM42688_INTF_CONFIG0              0x4C
-#define ICM42688_INTF_CONFIG1              0x4D
-#define ICM42688_PWR_MGMT0                 0x4E
-#define ICM42688_GYRO_CONFIG0              0x4F
-#define ICM42688_ACCEL_CONFIG0             0x50
-#define ICM42688_GYRO_CONFIG1              0x51
-#define ICM42688_GYRO_ACCEL_CONFIG0        0x52
-#define ICM42688_ACCEL_CONFIG1             0x53
-#define ICM42688_TMST_CONFIG               0x54
-#define ICM42688_APEX_CONFIG0              0x56
-#define ICM42688_SMD_CONFIG                0x57
-#define ICM42688_FIFO_CONFIG1              0x5F
-#define ICM42688_FIFO_CONFIG2              0x60
-#define ICM42688_FIFO_CONFIG3              0x61
-#define ICM42688_FSYNC_CONFIG              0x62
-#define ICM42688_INT_CONFIG0               0x63
-#define ICM42688_INT_CONFIG1               0x64
-#define ICM42688_INT_SOURCE0               0x65
-#define ICM42688_INT_SOURCE1               0x66
-#define ICM42688_INT_SOURCE3               0x68
-#define ICM42688_INT_SOURCE4               0x69
-#define ICM42688_FIFO_LOST_PKT0            0x6C
-#define ICM42688_FIFO_LOST_PKT1            0x6D
-#define ICM42688_SELF_TEST_CONFIG          0x70
-#define ICM42688_WHO_AM_I                  0x75 // should return 0x47
-#define ICM42688_REG_BANK_SEL              0x76
-
-// User Bank 1
-#define ICM42688_SENSOR_CONFIG0            0x03
-#define ICM42688_GYRO_CONFIG_STATIC2       0x0B
-#define ICM42688_GYRO_CONFIG_STATIC3       0x0C
-#define ICM42688_GYRO_CONFIG_STATIC4       0x0D
-#define ICM42688_GYRO_CONFIG_STATIC5       0x0E
-#define ICM42688_GYRO_CONFIG_STATIC6       0x0F
-#define ICM42688_GYRO_CONFIG_STATIC7       0x10
-#define ICM42688_GYRO_CONFIG_STATIC8       0x11
-#define ICM42688_GYRO_CONFIG_STATIC9       0x12
-#define ICM42688_GYRO_CONFIG_STATIC10      0x13
-#define ICM42688_XG_ST_DATA                0x5F
-#define ICM42688_YG_ST_DATA                0x60
-#define ICM42688_ZG_ST_DATA                0x61
-#define ICM42688_TMSTAL0                   0x63
-#define ICM42688_TMSTAL1                   0x64
-#define ICM42688_TMSTAL2                   0x62
-#define ICM42688_INTF_CONFIG4              0x7A
-#define ICM42688_INTF_CONFIG5              0x7B
-#define ICM42688_INTF_CONFIG6              0x7C
-
-// User Bank 2
-#define ICM42688_ACCEL_CONFIG_STATIC2      0x03
-#define ICM42688_ACCEL_CONFIG_STATIC3      0x04
-#define ICM42688_ACCEL_CONFIG_STATIC4      0x05
-#define ICM42688_XA_ST_DATA                0x3B
-#define ICM42688_YA_ST_DATA                0x3C
-#define ICM42688_ZA_ST_DATA                0x3D
-
-// User Bank 4
-#define ICM42688_APEX_CONFIG1              0x40
-#define ICM42688_APEX_CONFIG2              0x41
-#define ICM42688_APEX_CONFIG3              0x42
-#define ICM42688_APEX_CONFIG4              0x43
-#define ICM42688_APEX_CONFIG5              0x44
-#define ICM42688_APEX_CONFIG6              0x45
-#define ICM42688_APEX_CONFIG7              0x46
-#define ICM42688_APEX_CONFIG8              0x47
-#define ICM42688_APEX_CONFIG9              0x48
-#define ICM42688_ACCEL_WOM_X_THR           0x4A
-#define ICM42688_ACCEL_WOM_Y_THR           0x4B
-#define ICM42688_ACCEL_WOM_Z_THR           0x4C
-#define ICM42688_INT_SOURCE6               0x4D
-#define ICM42688_INT_SOURCE7               0x4E
-#define ICM42688_INT_SOURCE8               0x4F
-#define ICM42688_INT_SOURCE9               0x50
-#define ICM42688_INT_SOURCE10              0x51
-#define ICM42688_OFFSET_USER0              0x77
-#define ICM42688_OFFSET_USER1              0x78
-#define ICM42688_OFFSET_USER2              0x79
-#define ICM42688_OFFSET_USER3              0x7A
-#define ICM42688_OFFSET_USER4              0x7B
-#define ICM42688_OFFSET_USER5              0x7C
-#define ICM42688_OFFSET_USER6              0x7D
-#define ICM42688_OFFSET_USER7              0x7E
-#define ICM42688_OFFSET_USER8              0x7F
-
-#define ICM42688_ADDRESS           0x68   // Address of ICM42688 accel/gyro when ADO = 0
-
-#define AFS_2G  0x03
-#define AFS_4G  0x02
-#define AFS_8G  0x01
-#define AFS_16G 0x00 // default
-
-#define GFS_2000DPS   0x00   // default
-#define GFS_1000DPS   0x01
-#define GFS_500DPS    0x02
-#define GFS_250DPS    0x03
-#define GFS_125DPS    0x04
-#define GFS_62_50DPS  0x05
-#define GFS_31_25DPS  0x06
-#define GFS_15_625DPS 0x07
-
-// Low Noise mode
-#define AODR_32kHz    0x01   
-#define AODR_16kHz    0x02
-#define AODR_8kHz     0x03
-#define AODR_4kHz     0x04
-#define AODR_2kHz     0x05
-#define AODR_1kHz     0x06  // default
-//Low Noise or Low Power modes
-#define AODR_500Hz    0x0F
-#define AODR_200Hz    0x07
-#define AODR_100Hz    0x08
-#define AODR_50Hz     0x09
-#define AODR_25Hz     0x0A
-#define AODR_12_5Hz   0x0B
-// Low Power mode
-#define AODR_6_25Hz   0x0C  
-#define AODR_3_125Hz  0x0D
-#define AODR_1_5625Hz 0x0E
-
-#define GODR_32kHz  0x01   
-#define GODR_16kHz  0x02
-#define GODR_8kHz   0x03
-#define GODR_4kHz   0x04
-#define GODR_2kHz   0x05
-#define GODR_1kHz   0x06 // default
-#define GODR_500Hz  0x0F
-#define GODR_200Hz  0x07
-#define GODR_100Hz  0x08
-#define GODR_50Hz   0x09
-#define GODR_25Hz   0x0A
-#define GODR_12_5Hz 0x0B
-
-#define aMode_OFF 0x01
-#define aMode_LP  0x02
-#define aMode_LN  0x03
-
-#define gMode_OFF 0x00
-#define gMode_SBY 0x01
-#define gMode_LN  0x03
-
-#endif

lib/ICM42688/MMC5983MA.h

@@ -1,63 +0,0 @@
-/* 06/14/2020 Copyright Tlera Corporation
-
-    Created by Kris Winer
-
-  This sketch uses SDA/SCL on pins 21/20 (Ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board.
-  The MMC5983MA is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.
-
-  Library may be used freely and without limit with attribution.
-
-*/
-
-#ifndef MMC5983MA_h
-#define MMC5983MA_h
-
-//Register map for MMC5983MA'
-//http://www.memsic.com/userfiles/files/DataSheets/Magnetic-Sensors-Datasheets/MMC5983MA_Datasheet.pdf
-#define MMC5983MA_XOUT_0        0x00
-#define MMC5983MA_XOUT_1        0x01
-#define MMC5983MA_YOUT_0        0x02
-#define MMC5983MA_YOUT_1        0x03
-#define MMC5983MA_ZOUT_0        0x04
-#define MMC5983MA_ZOUT_1        0x05
-#define MMC5983MA_XYZOUT_2      0x06
-#define MMC5983MA_TOUT          0x07
-#define MMC5983MA_STATUS        0x08
-#define MMC5983MA_CONTROL_0     0x09
-#define MMC5983MA_CONTROL_1     0x0A
-#define MMC5983MA_CONTROL_2     0x0B
-#define MMC5983MA_CONTROL_3     0x0C
-#define MMC5983MA_PRODUCT_ID    0x2F
-
-#define MMC5983MA_ADDRESS       0x30
-
-// Sample rates
-#define MODR_ONESHOT   0x00
-#define MODR_1Hz       0x01
-#define MODR_10Hz      0x02
-#define MODR_20Hz      0x03
-#define MODR_50Hz      0x04
-#define MODR_100Hz     0x05
-#define MODR_200Hz     0x06 // BW = 0x01 only
-#define MODR_1000Hz    0x07 // BW = 0x11 only
-
-//Bandwidths
-#define MBW_100Hz 0x00  // 8 ms measurement time
-#define MBW_200Hz 0x01  // 4 ms
-#define MBW_400Hz 0x02  // 2 ms
-#define MBW_800Hz 0x03  // 0.5 ms
-
-// Set/Reset as a function of measurements
-#define MSET_1     0x00 // Set/Reset each data measurement
-#define MSET_25    0x01 // each 25 data measurements
-#define MSET_75    0x02
-#define MSET_100   0x03
-#define MSET_250   0x04
-#define MSET_500   0x05
-#define MSET_1000  0x06
-#define MSET_2000  0x07
-
-#define MMC5983MA_mRes (1.0f / 16384.0f) // mag sensitivity if using 18 bit data
-#define MMC5983MA_offset 131072.0f // mag range unsigned to signed
-
-#endif

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) 2022 TheDevMinerTV
+    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,28 +20,18 @@
     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
     THE SOFTWARE.
 */
+#pragma once
 
-#include "CalibrationConfig.h"
+#include <cstdint>
+#include <string>
 
-namespace SlimeVR {
-    namespace Configuration {
-        const char* calibrationConfigTypeToString(CalibrationConfigType type) {
-            switch (type) {
-            case NONE:
-                return "NONE";
-            case BMI160:
-                return "BMI160";
-            case MPU6050:
-                return "MPU6050";
-            case MPU9250:
-                return "MPU9250";
-            case ICM20948:
-                return "ICM20948";
-            case ICM42688:
-                return "ICM42688";
-            default:
-                return "UNKNOWN";
-            }
-        }
-    }
-}
+class PinInterface
+{
+public:
+	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;
+};

lib/i2cscan/i2cscan.cpp

@@ -1,114 +1,161 @@
 #include "i2cscan.h"
+
+#include <array>
+#include <cstdint>
+#include <string>
+
 #include "../../src/globals.h"
+#include "../../src/consts.h"
+
+namespace I2CSCAN {
+    enum class ScanState : uint8_t {
+        IDLE,
+        SCANNING,
+        DONE
+    };
+
+	namespace {
+		ScanState scanState = ScanState::IDLE;
+    	uint8_t currentSDA = 0;
+    	uint8_t currentSCL = 0;
+    	uint8_t currentAddress = 1;
+    	bool found = false;
+		uint8_t txFails = 0;
+    	std::vector<uint8_t> validPorts;
 
 #ifdef ESP8266
-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
+		std::array<uint8_t, 7> portArray = {16, 5, 4, 2, 14, 12, 13};
+		std::array<std::string, 7> portMap = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"};
+		std::array<uint8_t, 1> portExclude = {LED_PIN};
 #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"};
+		std::array<uint8_t, 9> portArray = {2, 3, 4, 5, 6, 7, 8, 9, 10};
+		std::array<std::string, 9> portMap = {"2", "3", "4", "5", "6", "7", "8", "9", "10"};
+		std::array<uint8_t, 5> portExclude = {18, 19, 20, 21, LED_PIN};
+#elif defined(ESP32C6)
+		std::array<uint8_t, 20> portArray = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23};
+		std::array<std::string, 20> portMap = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"};
+		std::array<uint8_t, 5> 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"};
-uint8_t portExclude[] = {LED_PIN};
+		std::array<uint8_t, 16> portArray = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
+		std::array<std::string, 16> portMap = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
+		std::array<uint8_t, 1> portExclude = {LED_PIN};
 #endif
 
-namespace I2CSCAN
-{
+		bool selectNextPort() {
+			currentSCL++;
 
-    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;
-    }
+			if(validPorts[currentSCL] == validPorts[currentSDA]) currentSCL++;
 
-    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(!found) {
-            Serial.println("[ERR] I2C: No I2C devices found");
+			if (currentSCL < validPorts.size()) {
+				Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]); //NOLINT
+				return true;
+			}
+
+			currentSCL = 0;
+			currentSDA++;
+
+			if (currentSDA >= validPorts.size()) {
+				if (!found) {
+					Serial.println("[ERROR] I2C: No I2C devices found"); //NOLINT
+				}
+	#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;
+		}
+		template <uint8_t size1, uint8_t size2>
+		uint8_t countCommonElements(
+			const std::array<uint8_t, size1>& array1,
+			const std::array<uint8_t, size2>& array2) {
+
+			uint8_t count = 0;
+			for (const auto& elem1 : array1) {
+				for (const auto& elem2 : array2) {
+					if (elem1 == elem2) {
+						count++;
+					}
+				}
+			}
+
+			return count;
+		}
+	}  // anonymous namespace
+
+    void scani2cports() {
+        if (scanState != ScanState::IDLE) {
+			if (scanState == ScanState::DONE) {
+				Serial.println("[DEBUG] I2C scan finished previously, resetting and scanning again..."); //NOLINT
+			} else {
+				return; // Already scanning, do not start again
+			}
         }
 
-#if ESP32
-        Wire.end();
+        // Filter out excluded ports
+		validPorts.clear();
+		uint8_t excludes = countCommonElements<portArray.size(), portExclude.size()>(portArray, portExclude);
+		validPorts.reserve(portArray.size() - excludes); // Reserve space to avoid reallocations
+
+		for (const auto& port : portArray) {
+			if (std::find(portExclude.begin(), portExclude.end(), port) == portExclude.end()) {
+				validPorts.push_back(port); // Port is valid, add it to the list
+			}
+		}
+
+		// Reset scan variables and start scanning
+        found = false;
+        currentSDA = 0;
+        currentSCL = 1;
+        currentAddress = 1;
+		txFails = 0;
+        scanState = ScanState::SCANNING;
+	}
+
+    void update() {
+        if (scanState != ScanState::SCANNING) {
+            return;
+        }
+
+#ifdef ESP32
+		if (currentAddress == 1) {
+            Wire.end();
+		}
 #endif
 
-        // Reset the I2C interface back to it's original values
-        Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
-    }
+        Wire.beginTransmission(currentAddress);
+        const uint8_t error = Wire.endTransmission();
 
-    bool inArray(uint8_t value, uint8_t* array, size_t arraySize)
-    {
-        for (size_t i = 0; i < arraySize; i++)
-        {
-            if (value == array[i]) 
-            {
-                return true;
-            }
+        if (error == 0) {
+            Serial.printf("[INFO ] 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) { // Unable to start transaction, log and warn
+            Serial.printf("[WARN ] I2C (@ %s(%d) : %s(%d)): Unable to start transaction at address 0x%02x!\n",
+                            portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
+            txFails++;
         }
 
-        return false;
-    }
-    
-    bool checkI2C(uint8_t i, uint8_t j)
-    {
-        bool found = false;
+        currentAddress++;
 
-#if ESP32
-        Wire.end();
+        if (currentAddress <= 127) {
+			if (txFails > 5) {
+#if BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV || BOARD == BOARD_SLIMEVR || BOARD == BOARD_SLIMEVR_V1_2
+				Serial.printf("[ERROR] I2C: Too many transaction errors (%d), please power off the tracker and contact SlimeVR support!\n", txFails);
+#else
+				Serial.printf("[ERROR] I2C: Too many transaction errors (%d), please power off the tracker and check the IMU connections!\n", txFails);
 #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;
         }
-        return found;
+
+        currentAddress = 1;
+        selectNextPort();
     }
 
     bool hasDevOnBus(uint8_t addr) {
@@ -118,7 +165,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 +190,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
-        #endif
 
-        pinMode(SDA, INPUT_PULLUP); // Make SDA (data) and SCL (clock) pins Inputs with pullup.
+    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
+#endif
+
+        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,11 +6,12 @@
 
 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);
     int clearBus(uint8_t SDA, uint8_t SCL);
-    boolean inArray(uint8_t value, uint8_t* arr, size_t arrSize);
+    bool inArray(uint8_t value, const uint8_t *array, size_t arraySize);
 }
 
-#endif // _I2CSCAN_H_
+#endif // _I2CSCAN_H_

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.h

@@ -1,50 +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.
-*/
-
-#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;
-
-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;
-};
-
-#include "mahony.hpp"
-
-#endif /* _MAHONY_H_ */

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

@@ -1,66 +0,0 @@
-[env]
-lib_deps=
-  https://github.com/SlimeVR/CmdParser.git
-  https://github.com/SlimeVR/base64_arduino.git
-monitor_speed = 115200
-framework = arduino
-build_flags =
-  !python scripts/get_git_commit.py
-  -O2
-  -std=gnu++17
-build_unflags =
-  -Os
-  -std=gnu++11
-
-[env:BOARD_SLIMEVR]
-platform = espressif8266 @ 4.2.0
-board = esp12e
-
-[env:BOARD_SLIMEVR_DEV]
-platform = espressif8266 @ 4.2.0
-board = esp12e
-
-[env:BOARD_NODEMCU]
-platform = espressif8266 @ 4.2.0
-board = esp12e
-
-[env:BOARD_WEMOSD1MINI]
-platform = espressif8266 @ 4.2.0
-board = esp12e
-
-[env:BOARD_TTGO_TBASE]
-platform = espressif8266 @ 4.2.0
-board = esp12e
-
-[env:BOARD_WEMOSWROOM02]
-platform = espressif8266 @ 4.2.0
-board = esp12e
-
-[env:BOARD_WROOM32]
-platform = espressif32 @ 6.1.0
-board = esp32dev
-
-[env:BOARD_ESP01]
-platform = espressif32 @ 6.1.0
-board = esp32dev
-
-[env:BOARD_LOLIN_C3_MINI]
-platform = espressif32 @ 6.1.0
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-board = lolin_c3_mini
-
-[env:BOARD_BEETLE32C3]
-platform = espressif32 @ 6.1.0
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-board = dfrobot_beetle_esp32c3
-
-[env:BOARD_ES32C3DEVKITM1]
-platform = espressif32 @ 6.1.0
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-board = esp32-c3-devkitm-1

platformio.ini

@@ -11,17 +11,24 @@
 ; https://docs.slimevr.dev/firmware/configuring-project.html#1-configuring-platformioini
 ; ================================================
 
+[platformio]
+build_cache_dir = cache
+default_envs = BOARD_WEMOSD1MINI
+
 [env]
 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
 ;monitor_rts = 0
 ;monitor_dtr = 0
 framework = arduino
-build_flags = 
+extra_scripts = pre:scripts/preprocessor.py
+build_flags =
   !python scripts/get_git_commit.py
 ;If you want to set hardcoded WiFi SSID and password, uncomment and edit the lines below
 ;To uncomment, only remove ";" and leave the two spaces in front of the tags
@@ -35,14 +42,17 @@ build_flags =
 ;  -DWIFI_STATIC_GATEWAY=192,168,XXX,XXX
 ;  -DWIFI_STATIC_SUBNET=255,255,255,0
 
+;  -DSERVER_IP='"192.168.XXX.XXX"'
+;  -DSERVER_PORT=6969
+
 ; Uncomment below if your board are using 40MHz crystal instead of 26MHz for ESP8266
 ;  -DF_CRYSTAL=40000000
 
 ; Enable -O2 GCC optimization
   -O2
-  -std=gnu++17
+  -std=gnu++2a
 
-build_unflags = -Os -std=gnu++11
+build_unflags = -Os -std=gnu++11 -std=gnu++17
 
 ; If you want to enable OTA Updates, uncomment and set OTA password here and in credentials.h
 ; You can set upload_port to device's ip after it's set up for the first time
@@ -52,36 +62,62 @@ build_unflags = -Os -std=gnu++11
 ;upload_flags =
 ;  --auth=SlimeVR-OTA
 
-; Settings for different boards
-
-[env:esp12e]
-platform = espressif8266 @ 4.2.0
-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
-
 ; Uncomment below if you want to build for ESP-01
 ;[env:esp01_1m]
-;platform = espressif8266 @ 4.2.0
+;platform = espressif8266 @ 4.2.1
 ;board = esp01_1m
 ;board_build.arduino.ldscript = "eagle.flash.1m64.ld"
 
-; Uncomment below if you want to build for ESP8285 (ESP8266 with embedded Flash)
-;[env:esp8285]
-;platform = espressif8266 @ 4.2.0
-;board = esp8285
-;board_build.arduino.ldscript = "eagle.flash.1m64.ld"
-;board_build.flash_mode = dout
+[env:BOARD_WEMOSD1MINI]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_WEMOSD1MINI
+upload_speed = 921600
 
-; Uncomment below if you want to build for esp32
-; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards
-; [env:esp32]
-; platform = espressif32 @ 6.1.0
-; board = esp32dev
-; 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
+[env:BOARD_NODEMCU]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_NODEMCU
+upload_speed = 921600
 
+[env:BOARD_TTGO_TBASE]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_TTGO_TBASE
+upload_speed = 921600
+
+[env:BOARD_WEMOSWROOM02]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_WEMOSWROOM02
+upload_speed = 921600
+
+[env:BOARD_WROOM32]
+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
+board = esp32dev
+custom_slime_board = BOARD_WROOM32
+
+[env:BOARD_ESP01]
+platform = espressif8266 @ 4.2.1
+board = esp01_1m
+board_build.arduino.ldscript = "eagle.flash.1m64.ld"
+custom_slime_board = BOARD_ESP01
+upload_speed = 921600
+
+[env:BOARD_LOLIN_C3_MINI]
+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
+custom_slime_board = BOARD_LOLIN_C3_MINI
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = lolin_c3_mini
+monitor_filters = colorize, esp32_exception_decoder
 ; If you want to use a ESP32C3, you can use this (experimental)
 ; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards
 ; There are 2 main Boardtypes:
@@ -91,9 +127,126 @@ upload_speed = 921600
 ;    -DARDUINO_USB_MODE=1
 ;    -DARDUINO_USB_CDC_ON_BOOT=1
 
-;[env:esp32c3]
-;platform = espressif32 @ 6.1.0
-;build_flags = 
-;  ${env.build_flags}
-;  -DESP32C3
-;board = lolin_c3_mini
+[env:BOARD_BEETLE32C3]
+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
+custom_slime_board = BOARD_BEETLE32C3
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = dfrobot_beetle_esp32c3
+monitor_filters = colorize, esp32_exception_decoder
+; If you want to use a ESP32C3, you can use this (experimental)
+; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards
+; There are 2 main Boardtypes:
+; 1. Boards that use a USB 2 Serial Chipset ( esp32-c3-devkitm-1, ttgo-t-oi-plus )
+; 2. Boards that relay on the USB interface of the ESP32C3 ( lolin_c3_mini , dfrobot_beetle_esp32c3)
+;    On this board there are 2 type some of them need to have set the build flag (menuconfig)
+;    -DARDUINO_USB_MODE=1
+;    -DARDUINO_USB_CDC_ON_BOOT=1
+
+[env:BOARD_ESP32C3DEVKITM1]
+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
+custom_slime_board = BOARD_ESP32C3DEVKITM1
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = esp32-c3-devkitm-1
+monitor_filters = colorize, esp32_exception_decoder
+; If you want to use a ESP32C3, you can use this (experimental)
+; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards
+; There are 2 main Boardtypes:
+; 1. Boards that use a USB 2 Serial Chipset ( esp32-c3-devkitm-1, ttgo-t-oi-plus )
+; 2. Boards that relay on the USB interface of the ESP32C3 ( lolin_c3_mini , dfrobot_beetle_esp32c3)
+;    On this board there are 2 type some of them need to have set the build flag (menuconfig)
+;    -DARDUINO_USB_MODE=1
+;    -DARDUINO_USB_CDC_ON_BOOT=1
+
+[env:BOARD_ESP32C6DEVKITC1]
+platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
+custom_slime_board = BOARD_ESP32C6DEVKITC1
+build_flags =
+  ${env.build_flags}
+  -DESP32C6
+  -DARDUINO_USB_MODE=1
+  -DARDUINO_USB_CDC_ON_BOOT=1
+board = esp32-c6-devkitc-1
+
+[env:BOARD_XIAO_ESP32C3]
+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
+custom_slime_board = BOARD_XIAO_ESP32C3
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = seeed_xiao_esp32c3
+monitor_filters = colorize, esp32_exception_decoder
+
+[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
+custom_slime_board = BOARD_ESP32S3_SUPERMINI
+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
+
+[env:BOARD_SLIMEVR]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_SLIMEVR
+build_flags =
+  ${env.build_flags}
+  -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
+custom_slime_board = BOARD_SLIMEVR_V1_2
+build_flags =
+  ${env.build_flags}
+  -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
+custom_slime_board = BOARD_SLIMEVR_DEV
+build_flags =
+  ${env.build_flags}
+  -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}
+  -D BOARD=BOARD_GLOVE_IMU_SLIMEVR_DEV
+  -DESP32C3
+  -D PRODUCT_NAME='"SlimeVR Glove (dev)"'
+board = lolin_c3_mini
+monitor_filters = colorize, esp32_exception_decoder

scripts/get_git_commit.py

@@ -9,11 +9,41 @@ 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}\"'"
+
+fwVersion = os.environ.get("FIRMWARE_VERSION")
+if fwVersion is not None and fwVersion != "":
+	output += f" -DFIRMWARE_VERSION='\"{fwVersion}\"'"
+elif tag != "":
+	output += f" -DFIRMWARE_VERSION='\"{tag}\"'"
+elif branch != "":
+	output += f" -DFIRMWARE_VERSION='\"{branch}\"'"
+else:
+	output += f" -DFIRMWARE_VERSION='\"git-{revision}\"'"
+
+print(output)

scripts/preprocessor.py

@@ -0,0 +1,210 @@
+import json
+import re
+import os
+from pathlib import Path
+from typing import Union, Optional, Dict, Any, List
+
+Import("env")
+
+try:
+    import jsonschema
+except:
+    env.Execute(
+        env.VerboseAction(
+            '$PYTHONEXE -m pip install "jsonschema==4.22.0"',
+            "Installing jsonschema for validation",
+        )
+    )
+
+from jsonschema import Draft202012Validator, exceptions as jsonschema_exceptions
+
+
+
+def _load_json(maybe_path_or_dict: Union[str, Path, dict]) -> dict:
+    """Load JSON file or accept dict directly."""
+    if isinstance(maybe_path_or_dict, dict):
+        return maybe_path_or_dict
+    p = Path(maybe_path_or_dict)
+    if not p.exists():
+        raise FileNotFoundError(f"File not found: {p}")
+    try:
+        return json.loads(p.read_text(encoding="utf-8"))
+    except json.JSONDecodeError as e:
+        raise ValueError(f"Invalid JSON file {p}: {e}")
+
+# Allow:
+# - unquoted alphanumerics, underscores, dots, dashes
+# - or single-quoted with optional escaped double quotes inside
+VALID_DEFINE_VALUE = re.compile(
+    r"^(?:[A-Za-z0-9_.\-]*|'(\\\"[A-Za-z0-9_.\-\s]*\\\"|[A-Za-z0-9_.\-\s]*)')$"
+)
+
+def _validate_define_value(value: str, key: str) -> None:
+    if key == "SENSOR_DESC_LIST":
+        return
+
+    """Validate the formatted define value to prevent injection."""
+    if not VALID_DEFINE_VALUE.fullmatch(value):
+        raise ValueError(
+            f"Invalid characters in value for {key!r}: {value!r} "
+            "(only letters, digits, _, ., -, spaces, and optional quoted forms like '\"text\"' allowed)"
+        )
+
+def _format_raw_value(value: Any) -> str:
+    """Format booleans for C/C++, otherwise str(value)."""
+    if isinstance(value, bool):
+        return "true" if value else "false"
+    return str(value)
+
+def format_value(val: Any, typ: str, key: str = "<unknown>") -> str:
+    """Format a value according to type, with built-in validation."""
+    if typ == "pin":
+        if isinstance(val, str) and re.search(r"[AD]", val):
+            result = f'{val}'
+        else:
+            result = _format_raw_value(val)
+
+    elif typ == "string":
+        result = f"'\\\"{val}\\\"'"
+
+    elif typ in ("raw", "number"):
+        result = _format_raw_value(val)
+
+    else:
+        raise ValueError(f"Value type '{typ}' is not supported")
+
+
+    _validate_define_value(result, key)
+    return result
+
+
+def _build_board_flags(defaults: dict, board_name: str) -> List[str]:
+    """Construct list of -D flags for one board."""
+    if "defaults" not in defaults:
+        raise ValueError("Missing top-level 'defaults' key in defaults JSON.")
+    if board_name not in defaults["defaults"]:
+        raise ValueError(f"Invalid board selected - {board_name}")
+
+    board_defaults = defaults["defaults"][board_name]
+    values = board_defaults.get("values", {})
+
+    args: Dict[str, Dict[str, Any]] = {}
+
+    def add(key: str, value: Any, value_type: str):
+        if value is not None:
+            args[key] = {"value": value, "type": value_type}
+
+    add('BOARD', board_name, 'raw')
+    add('LED_PIN', values.get('LED').get('LED_PIN'), 'pin')
+    add('LED_INVERTED', values.get('LED').get('LED_INVERTED'), 'raw')
+
+    sensors = values.get('SENSORS')
+    if sensors:
+        sensor_list = []
+
+        add('PIN_IMU_SDA', 255, 'pin') # FIXME fix the I2C Scanner so it use the sensor list and not be called when no I2C sensor
+        add('PIN_IMU_SCL', 255, 'pin')
+        add('PIN_IMU_INT_2', 255, 'pin') # FIXME: fix the CONFIG serial command so it use the sensor list
+
+        for index, sensor in enumerate(sensors):
+            if sensor.get('protocol') == 'I2C':
+                params = [
+                    format_value(sensor.get('imu'), 'raw'),
+                    format_value(sensor.get('address', 'PRIMARY_IMU_ADDRESS_ONE' if index == 0 else 'SECONDARY_IMU_ADDRESS_TWO'), 'number'),
+                    format_value(sensor.get('rotation'), 'raw'),
+                    f"DIRECT_WIRE({format_value(sensor.get('scl'), 'pin')}, {format_value(sensor.get('sda'), 'pin')})",
+                    'false' if index == 0 else 'true',
+                    f"DIRECT_PIN({format_value(sensor.get('int', 255), 'pin')})",
+                    '0'
+                ]
+                sensor_list.append(f"SENSOR_DESC_ENTRY({','.join(params)})")
+                add('PIN_IMU_SDA', sensor.get('sda'), 'pin')
+                add('PIN_IMU_SCL', sensor.get('scl'), 'pin')
+
+            if sensor.get('protocol') == 'SPI':
+                params = [
+                    format_value(sensor.get('imu'), 'raw'),
+                    f"DIRECT_PIN({format_value(sensor.get('cs'), 'pin')})",
+                    format_value(sensor.get('rotation'), 'raw'),
+                    "DIRECT_SPI(24'000'000, MSBFIRST, SPI_MODE3)",
+                    'false' if index == 0 else 'true',
+                    f"DIRECT_PIN({format_value(sensor.get('int', 255), 'pin')})",
+                    '0'
+                ]
+                sensor_list.append(f"SENSOR_DESC_ENTRY({','.join(params)})")
+
+            if index == 0: # FIXME: fix the CONFIG serial command so it use the sensor list
+                add('PIN_IMU_INT', sensor.get('int'), 'pin')
+            elif index == 1:
+                add('PIN_IMU_INT_2', sensor.get('int'), 'pin')
+        add('SENSOR_DESC_LIST', f"'{' '.join(sensor_list)}'", 'raw')
+
+
+    battery = values.get('BATTERY')
+    if battery:
+        add('BATTERY_MONITOR', battery.get('type'), 'raw')
+        add('PIN_BATTERY_LEVEL', battery.get('pin', 255), 'pin')
+        add('BATTERY_SHIELD_RESISTANCE', battery.get('shieldR', 180), 'number')
+        add('BATTERY_SHIELD_R1', battery.get('r1', 100), 'number')
+        add('BATTERY_SHIELD_R2', battery.get('r2', 220), 'number')
+
+    parts: List[str] = []
+    for key, meta in args.items():
+        formatted = format_value(meta["value"], meta["type"], key)
+        parts.append(f"-D{key}={formatted}")
+
+    return parts
+
+
+def build_boards(
+    schema_obj,
+    defaults_obj,
+    board_name: Optional[str] = None,
+) -> Dict[str, List[str]]:
+    """
+    Validate defaults.json against board-defaults.schema.json using jsonschema,
+    and return { board_name: [list of -D flags] }.
+    """
+    validator = Draft202012Validator(schema_obj)
+    errors = sorted(validator.iter_errors(defaults_obj), key=lambda e: e.path)
+
+    if errors:
+        print("✖ JSON Schema validation failed:")
+        for err in errors:
+            path = "/".join(map(str, err.path)) or "(root)"
+            print(f"  • Path: {path}")
+            print(f"    Error: {err.message}")
+            if err.context:
+                for ctx in err.context:
+                    print(f"      ↳ {ctx.message}")
+        raise ValueError(f"{len(errors)} schema validation errors found.")
+
+    out: Dict[str, List[str]] = {}
+    if board_name:
+        out[board_name] = _build_board_flags(defaults_obj, board_name)
+    else:
+        for name in defaults_obj.get("defaults", {}).keys():
+            out[name] = _build_board_flags(defaults_obj, name)
+
+    return out
+
+schema_obj = _load_json("./board-defaults.schema.json")
+defaults_obj = _load_json("./board-defaults.json")
+slime_board = env.GetProjectOption("custom_slime_board", None)
+if slime_board:
+    if 'SLIMEVR_OVERRIDE_DEFAULTS' in os.environ and slime_board in defaults_obj['defaults']:
+        print(">>> OVERIDING BOARD DEFAULTS ", os.environ['SLIMEVR_OVERRIDE_DEFAULTS'])
+        defaults_obj['defaults'][slime_board]['values'] = json.loads(os.environ['SLIMEVR_OVERRIDE_DEFAULTS'])
+
+    output_flags = build_boards(
+        schema_obj,
+        defaults_obj,
+        slime_board,
+    )
+    output_flags = output_flags.get(slime_board, []) if isinstance(output_flags, dict) else []
+
+    separator = '\n  '
+    print(f">>> Appending build flags:\n  {separator.join(output_flags)}")
+    env.Append(BUILD_FLAGS=output_flags)
+else:
+    print(">>> custom_slime_board not set - skipping")

src/FSHelper.cpp

@@ -0,0 +1,90 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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 "./FSHelper.h"
+
+#include <functional>
+
+namespace SlimeVR::Utils {
+SlimeVR::Logging::Logger m_Logger("FSHelper");
+
+bool ensureDirectory(const char* directory) {
+	if (!LittleFS.exists(directory)) {
+		if (!LittleFS.mkdir(directory)) {
+			m_Logger.error("Failed to create directory: %s", directory);
+			return false;
+		}
+	}
+
+	auto dir = LittleFS.open(directory, "r");
+	auto isDirectory = dir.isDirectory();
+	dir.close();
+
+	if (!isDirectory) {
+		if (!LittleFS.remove(directory)) {
+			m_Logger.error("Failed to remove directory: %s", directory);
+			return false;
+		}
+
+		if (!LittleFS.mkdir(directory)) {
+			m_Logger.error("Failed to create directory: %s", directory);
+			return false;
+		}
+	}
+
+	return true;
+}
+
+File openFile(const char* path, const char* mode) {
+	return File(LittleFS.open(path, mode));
+}
+
+void forEachFile(const char* directory, std::function<void(File file)> callback) {
+	if (!ensureDirectory(directory)) {
+		return;
+	}
+
+#ifdef ESP32
+	auto dir = LittleFS.open(directory);
+	while (auto f = dir.openNextFile()) {
+		if (f.isDirectory()) {
+			continue;
+		}
+
+		callback(File(f));
+	}
+
+	dir.close();
+#else
+	auto dir = LittleFS.openDir(directory);
+	while (dir.next()) {
+		auto fd = dir.openFile("r");
+		if (!fd.isFile()) {
+			continue;
+		}
+
+		callback(File(fd));
+	}
+#endif
+}
+}  // namespace SlimeVR::Utils

src/FSHelper.h

@@ -0,0 +1,66 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 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 UTILS_FSHELPER_H
+#define UTILS_FSHELPER_H
+
+#include <LittleFS.h>
+#include <logging/Logger.h>
+
+#include <functional>
+
+namespace SlimeVR::Utils {
+
+class File {
+public:
+	File(fs::File file)
+		: m_File(file) {}
+
+	~File() {
+		if (m_File) {
+			m_File.close();
+		}
+	}
+
+	const char* name() const { return m_File.name(); }
+	size_t size() const { return m_File.size(); }
+	bool isDirectory() { return m_File.isDirectory(); }
+	bool seek(size_t pos) { return m_File.seek(pos); }
+	bool read(uint8_t* buffer, size_t size) { return m_File.read(buffer, size); }
+	bool write(const uint8_t* buffer, size_t size) {
+		return m_File.write(buffer, size);
+	}
+	void close() { return m_File.close(); }
+
+private:
+	fs::File m_File;
+};
+
+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 SlimeVR::Utils
+
+#endif

src/GlobalVars.h

@@ -20,17 +20,18 @@
 	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 "network/wifihandler.h"
+#include "network/wifiprovisioning.h"
 #include "sensors/SensorManager.h"
+#include "status/LEDManager.h"
 #include "status/StatusManager.h"
 
 extern Timer<> globalTimer;
@@ -40,5 +41,6 @@ extern SlimeVR::Configuration::Configuration configuration;
 extern SlimeVR::Sensors::SensorManager sensorManager;
 extern SlimeVR::Network::Manager networkManager;
 extern SlimeVR::Network::Connection networkConnection;
-
-#endif
+extern BatteryMonitor battery;
+extern SlimeVR::WiFiNetwork wifiNetwork;
+extern SlimeVR::WifiProvisioning wifiProvisioning;

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,135 +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 BATTERY_MONITOR == BAT_EXTERNAL
-                voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * batteryADCMultiplier;
-            #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 *= batteryADCMultiplier;
-                        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,93 +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 ADCResulution 1023.0  // ESP8266 has 12bit 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
-#if ESP32
-    #define ADCResulution 4095.0  // ESP32 has 12bit ADC
-    #define ADCVoltageMax 3.3     // ESP32 input is 3.3 V = 4095.0
-#endif
-#ifndef ADCResulution
-    #define ADCResulution 1023.0
+#ifndef ADCResolution
+#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 batteryADCMultiplier ADCVoltageMax / ADCResulution * (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 batteryADCMultiplier 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; }
 
 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,110 @@
+/*
+ * 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"
+
+// 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
+
+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)
+
+#include "glove_default.h"
+
+#endif  // BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV

src/boards/defines_helpers.cpp

@@ -0,0 +1,38 @@
+/*
+	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
+
+#ifndef LED_PIN
+extern const uint8_t __attribute__((weak)) LED_PIN = LED_BUILTIN;
+#endif
+
+#ifndef LED_INVERTED
+extern const bool __attribute__((weak)) LED_INVERTED = true;
+#endif

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/CalibrationConfig.h

@@ -1,110 +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_CONFIGURATION_CALIBRATIONCONFIG_H
-#define SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H
-
-#include <stdint.h>
-
-namespace SlimeVR {
-    namespace Configuration {
-        struct BMI160CalibrationConfig {
-            // accelerometer offsets and correction matrix
-            float A_B[3];
-            float A_Ainv[3][3];
-
-            // magnetometer offsets and correction matrix
-            float M_B[3];
-            float M_Ainv[3][3];
-
-            // raw offsets, determined from gyro at rest
-            float G_off[3];
-
-            // calibration temperature for dynamic compensation
-            float temperature;
-        };
-
-        struct MPU6050CalibrationConfig {
-            // accelerometer offsets and correction matrix
-            float A_B[3];
-
-            // raw offsets, determined from gyro at rest
-            float G_off[3];
-        };
-
-        struct MPU9250CalibrationConfig {
-            // accelerometer offsets and correction matrix
-            float A_B[3];
-            float A_Ainv[3][3];
-
-            // magnetometer offsets and correction matrix
-            float M_B[3];
-            float M_Ainv[3][3];
-
-            // raw offsets, determined from gyro at rest
-            float G_off[3];
-        };
-
-        struct ICM20948CalibrationConfig {
-            // gyroscope bias
-            int32_t G[3];
-
-            // accelerometer bias
-            int32_t A[3];
-
-            // compass bias
-            int32_t C[3];
-        };
-
-        struct ICM42688CalibrationConfig {
-            // accelerometer offsets and correction matrix
-            float A_B[3];
-            float A_Ainv[3][3];
-
-            // magnetometer offsets and correction matrix
-            float M_B[3];
-            float M_Ainv[3][3];
-
-            // raw offsets, determined from gyro at rest
-            float G_off[3];
-        };
-
-        enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, ICM42688 };
-
-        const char* calibrationConfigTypeToString(CalibrationConfigType type);
-
-        struct CalibrationConfig {
-            CalibrationConfigType type;
-
-            union {
-                BMI160CalibrationConfig bmi160;
-                MPU6050CalibrationConfig mpu6050;
-                MPU9250CalibrationConfig mpu9250;
-                ICM20948CalibrationConfig icm20948;
-                ICM42688CalibrationConfig icm42688;
-            } data;
-        };
-    }
-}
-
-#endif

src/configuration/Configuration.cpp

@@ -1,467 +1,489 @@
 /*
-    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 <cstdint>
+#include <cstring>
+
+#include "../FSHelper.h"
 #include "consts.h"
+#include "sensors/SensorToggles.h"
 #include "utils.h"
 
 #define DIR_CALIBRATIONS "/calibrations"
 #define DIR_TEMPERATURE_CALIBRATIONS "/tempcalibrations"
+#define DIR_TOGGLES_OLD "/toggles"
+#define DIR_TOGGLES "/sensortoggles"
 
-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");
 
-                File 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();
+	}
 
-            loadCalibrations();
+	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_Calibrations.size(); i++) {
-                CalibrationConfig config = m_Calibrations[i];
-                if (config.type == CalibrationConfigType::NONE) {
-                    continue;
-                }
-
-                char path[17];
-                sprintf(path, DIR_CALIBRATIONS"/%d", i);
-
-                m_Logger.trace("Saving calibration data for %d", i);
-
-                File file = LittleFS.open(path, "w");
-                file.write((uint8_t*)&config, sizeof(CalibrationConfig));
-                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_Calibrations.clear();
-            m_Config.version = 1;
-            save();
-
-            m_Logger.debug("Reset configuration");
-        }
-
-        int32_t Configuration::getVersion() const {
-            return m_Config.version;
-        }
-
-        size_t Configuration::getCalibrationCount() const {
-            return m_Calibrations.size();
-        }
-
-        CalibrationConfig Configuration::getCalibration(size_t sensorID) const {
-            if (sensorID >= m_Calibrations.size()) {
-                return {};
-            }
-
-            return m_Calibrations.at(sensorID);
-        }
-
-        void Configuration::setCalibration(size_t sensorID, const CalibrationConfig& config) {
-            size_t currentCalibrations = m_Calibrations.size();
-
-            if (sensorID >= currentCalibrations) {
-                m_Calibrations.resize(sensorID + 1);
-            }
-
-            m_Calibrations[sensorID] = config;
-        }
-
-        void Configuration::loadCalibrations() {
-#ifdef ESP32
-            {
-                File calibrations = LittleFS.open(DIR_CALIBRATIONS);
-                if (!calibrations) {
-                    m_Logger.warn("No calibration data found, creating new directory...");
-
-                    if (!LittleFS.mkdir(DIR_CALIBRATIONS)) {
-                        m_Logger.error("Failed to create directory: " DIR_CALIBRATIONS);
-                        return;
-                    }
-
-                    calibrations = LittleFS.open(DIR_CALIBRATIONS);
-                }
-
-                if (!calibrations.isDirectory()) {
-                    calibrations.close();
-
-                    m_Logger.warn("Found file instead of directory: " DIR_CALIBRATIONS);
-
-                    if (!LittleFS.remove(DIR_CALIBRATIONS)) {
-                        m_Logger.error("Failed to remove directory: " DIR_CALIBRATIONS);
-                        return;
-                    }
-
-                    if (!LittleFS.mkdir(DIR_CALIBRATIONS)) {
-                        m_Logger.error("Failed to create directory: " DIR_CALIBRATIONS);
-                        return;
-                    }
-
-                    calibrations = LittleFS.open(DIR_CALIBRATIONS);
-                }
-
-                m_Logger.debug("Found calibration data directory");
-
-                while (File f = calibrations.openNextFile()) {
-                    if (f.isDirectory()) {
-                        continue;
-                    }
-
-                    m_Logger.trace("Found calibration data file: %s", f.name());
-
-                    uint8_t sensorId = strtoul(f.name(), nullptr, 10);
-                    CalibrationConfig calibrationConfig;
-                    f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig));
-                    f.close();
-
-                    m_Logger.debug("Found sensor calibration for %s at index %d", calibrationConfigTypeToString(calibrationConfig.type), sensorId);
-
-                    setCalibration(sensorId, calibrationConfig);
-                }
-
-                calibrations.close();
-            }
-#else
-            {
-                if (!LittleFS.exists(DIR_CALIBRATIONS)) {
-                    m_Logger.warn("No calibration data found, creating new directory...");
-
-                    if (!LittleFS.mkdir(DIR_CALIBRATIONS)) {
-                        m_Logger.error("Failed to create directory: " DIR_CALIBRATIONS);
-                        return;
-                    }
-
-                    // There's no calibrations here, so we're done
-                    return;
-                }
-
-                Dir calibrations = LittleFS.openDir(DIR_CALIBRATIONS);
-                while (calibrations.next()) {
-                    File f = calibrations.openFile("r");
-                    if (!f.isFile()) {
-                        continue;
-                    }
-
-                    CalibrationConfig calibrationConfig;
-                    f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig));
-
-                    uint8_t sensorId = strtoul(calibrations.fileName().c_str(), nullptr, 10);
-                    m_Logger.debug("Found sensor calibration for %s at index %d", calibrationConfigTypeToString(calibrationConfig.type), sensorId);
-
-                    setCalibration(sensorId, calibrationConfig);
-                }
-            }
-#endif
-        }
-
-        bool Configuration::loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config) {
-#ifdef ESP32
-            {
-                File calibrations = LittleFS.open(DIR_TEMPERATURE_CALIBRATIONS);
-                if (!calibrations) {
-                    m_Logger.warn("No temperature calibration data found, creating new directory...");
-
-                    if (!LittleFS.mkdir(DIR_TEMPERATURE_CALIBRATIONS)) {
-                        m_Logger.error("Failed to create directory: " DIR_TEMPERATURE_CALIBRATIONS);
-                        return false;
-                    }
-
-                    calibrations = LittleFS.open(DIR_TEMPERATURE_CALIBRATIONS);
-                }
-
-                if (!calibrations.isDirectory()) {
-                    calibrations.close();
-
-                    m_Logger.warn("Found file instead of directory: " DIR_TEMPERATURE_CALIBRATIONS);
-
-                    if (!LittleFS.remove(DIR_TEMPERATURE_CALIBRATIONS)) {
-                        m_Logger.error("Failed to remove directory: " DIR_TEMPERATURE_CALIBRATIONS);
-                        return false;
-                    }
-
-                    if (!LittleFS.mkdir(DIR_TEMPERATURE_CALIBRATIONS)) {
-                        m_Logger.error("Failed to create directory: " DIR_TEMPERATURE_CALIBRATIONS);
-                        return false;
-                    }
-
-                    return false;
-                }
-                
-                calibrations.close();
-
-                m_Logger.debug("Found temperature calibration data directory");
-
-                char path[32];
-                sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId);
-                if (LittleFS.exists(path)) {
-                    File f = LittleFS.open(path, "r");
-                    if (f.isDirectory()) {
-                        return false;
-                    }
-
-                    if (f.size() == sizeof(GyroTemperatureCalibrationConfig)) {
-                        CalibrationConfigType storedConfigType;
-                        f.read((uint8_t*)&storedConfigType, sizeof(CalibrationConfigType));
-                        if (storedConfigType != config.type) {
-                            f.close();
-                            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));
-                        f.close();
-                        m_Logger.debug("Found sensor temperature calibration for %s sensorId:%d", calibrationConfigTypeToString(config.type), sensorId);
-                        return true;
-                    } else {
-                        m_Logger.debug("Found incompatible sensor temperature calibration (size mismatch) sensorId:%d, skipping", sensorId);
-                    }
-                }
-                return false;
-            }
-#else
-            {
-                if (!LittleFS.exists(DIR_TEMPERATURE_CALIBRATIONS)) {
-                    m_Logger.warn("No temperature calibration data found, creating new directory...");
-
-                    if (!LittleFS.mkdir(DIR_TEMPERATURE_CALIBRATIONS)) {
-                        m_Logger.error("Failed to create directory: " DIR_TEMPERATURE_CALIBRATIONS);
-                        return false;
-                    }
-
-                    // There's no calibrations here, so we're done
-                    return false;
-                }
-
-                char path[32];
-                sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId);
-                if (LittleFS.exists(path)) {
-                    File f = LittleFS.open(path, "r");
-                    if (!f.isFile()) {
-                        return false;
-                    }
-
-                    if (f.size() == sizeof(GyroTemperatureCalibrationConfig)) {
-                        CalibrationConfigType storedConfigType;
-                        f.read((uint8_t*)&storedConfigType, sizeof(CalibrationConfigType));
-                        if (storedConfigType != config.type) {
-                            f.close();
-                            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));
-                        f.close();
-                        m_Logger.debug("Found sensor temperature calibration for %s sensorId:%d", calibrationConfigTypeToString(config.type), sensorId);
-                        return true;
-                    } else {
-                        m_Logger.debug("Found incompatible sensor temperature calibration (size mismatch) sensorId:%d, skipping", sensorId);
-                    }
-                }
-                
-                return false;
-            }
-#endif
-        }
-
-        bool Configuration::saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config) {
-            if (config.type == CalibrationConfigType::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 Calibrations:", m_Calibrations.size());
-
-            for (size_t i = 0; i < m_Calibrations.size(); i++) {
-                const CalibrationConfig& c = m_Calibrations[i];
-                m_Logger.info("    - [%3d] %s", i, calibrationConfigTypeToString(c.type));
-
-                switch (c.type) {
-                case CalibrationConfigType::NONE:
-                    break;
-
-                case CalibrationConfigType::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 CalibrationConfigType::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 CalibrationConfigType::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 CalibrationConfigType::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 CalibrationConfigType::ICM42688:
-                    m_Logger.info("            A_B   : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.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.icm42688.A_Ainv[i]));
-                    }
-
-                    m_Logger.info("            M_B   : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.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.icm42688.M_Ainv[i]));
-                    }
-
-                    m_Logger.info("            G_off  : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.G_off));
-
-                    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();
+
+		if (i < m_SensorToggles.size()) {
+			sprintf(path, DIR_TOGGLES "/%zu", i);
+
+			m_Logger.trace("Saving sensor toggle state for %d", i);
+
+			file = LittleFS.open(path, "w");
+			auto toggleValues = m_SensorToggles[i].getValues();
+			file.write((uint8_t*)&toggleValues, sizeof(SensorToggleValues));
+			file.close();
+		} else {
+			m_Logger.trace(
+				"Skipping saving toggles for sensor %d, no toggles present",
+				i
+			);
+		}
+	}
+
+	{
+		File file = LittleFS.open("/config.bin", "w");
+		file.write((uint8_t*)&m_Config, sizeof(DeviceConfig));
+		file.close();
+	}
+
+	// Clean up old toggles directory
+	if (LittleFS.exists(DIR_TOGGLES_OLD)) {
+		char path[17] = DIR_TOGGLES_OLD;
+		char* end = path + strlen(DIR_TOGGLES_OLD);
+		Utils::forEachFile(DIR_TOGGLES_OLD, [&](SlimeVR::Utils::File file) {
+			sprintf(end, "/%s", file.name());
+			LittleFS.remove(path);
+			file.close();
+		});
+		LittleFS.rmdir(DIR_TOGGLES_OLD);
+	}
+
+	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);
+	});
+
+	if (LittleFS.exists(DIR_TOGGLES_OLD)) {
+		SlimeVR::Utils::forEachFile(DIR_TOGGLES_OLD, [&](SlimeVR::Utils::File f) {
+			SensorToggleValues values;
+			// Migration for pre 0.7.0 togglestate, the values started at offset 20 and
+			// there were 3 of them
+			f.seek(20);
+			f.read(reinterpret_cast<uint8_t*>(&values), 3);
+
+			uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+			m_Logger.debug("Found sensor toggle state at index %d", sensorId);
+
+			setSensorToggles(sensorId, SensorToggleState{values});
+		});
+	}
+
+	SlimeVR::Utils::forEachFile(DIR_TOGGLES, [&](SlimeVR::Utils::File f) {
+		if (f.size() > sizeof(SensorToggleValues)) {
+			return;
+		}
+		SensorToggleValues values;
+		// With the magic of C++ default initialization, the rest of the values should
+		// be their default after reading
+		f.read(reinterpret_cast<uint8_t*>(&values), f.size());
+
+		uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+		m_Logger.debug("Found sensor toggle state at index %d", sensorId);
+
+		setSensorToggles(sensorId, SensorToggleState{values});
+	});
+}
+
+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 getCalibrationCount() const;
-            CalibrationConfig getCalibration(size_t sensorID) const;
-            void setCalibration(size_t sensorID, const CalibrationConfig& 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 loadCalibrations();
-            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<CalibrationConfig> m_Calibrations;
+	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/DeviceConfig.h

@@ -1,37 +1,33 @@
 /*
-    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_DEVICECONFIG_H
 #define SLIMEVR_CONFIGURATION_DEVICECONFIG_H
 
-#include "CalibrationConfig.h"
-
-namespace SlimeVR {
-    namespace Configuration {
-        struct DeviceConfig {
-            int32_t version;
-        };
-    }
-}
+namespace SlimeVR::Configuration {
+struct DeviceConfig {
+	int32_t version;
+};
+}  // namespace SlimeVR::Configuration
 
 #endif

src/configuration/SensorConfig.cpp

@@ -0,0 +1,62 @@
+/*
+	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 "SensorConfig.h"
+
+namespace SlimeVR::Configuration {
+const char* calibrationConfigTypeToString(SensorConfigType type) {
+	switch (type) {
+		case SensorConfigType::NONE:
+			return "NONE";
+		case SensorConfigType::BMI160:
+			return "BMI160";
+		case SensorConfigType::MPU6050:
+			return "MPU6050";
+		case SensorConfigType::MPU9250:
+			return "MPU9250";
+		case SensorConfigType::ICM20948:
+			return "ICM20948";
+		case SensorConfigType::SFUSION:
+			return "SoftFusion (common)";
+		case SensorConfigType::BNO0XX:
+			return "BNO0XX";
+		case SensorConfigType::RUNTIME_CALIBRATION:
+			return "SoftFusion (runtime calibration)";
+		default:
+			return "UNKNOWN";
+	}
+}
+
+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;
+}
+
+bool SensorConfigBits::operator!=(const SensorConfigBits& rhs) const {
+	return !(*this == rhs);
+}
+
+}  // namespace SlimeVR::Configuration

src/configuration/SensorConfig.h

@@ -0,0 +1,200 @@
+/*
+	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_CONFIGURATION_SENSORCONFIG_H
+#define SLIMEVR_CONFIGURATION_SENSORCONFIG_H
+
+#include <stdint.h>
+
+#include "consts.h"
+
+namespace SlimeVR::Configuration {
+struct BMI160SensorConfig {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+
+	// calibration temperature for dynamic compensation
+	float temperature;
+};
+
+struct SoftFusionSensorConfig {
+	SensorTypeID ImuType;
+	uint16_t MotionlessDataLen;
+
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+
+	// calibration temperature for dynamic compensation
+	float temperature;
+
+	// real measured sensor sampling rate
+	float A_Ts;
+	float G_Ts;
+	float M_Ts;
+
+	// gyro sensitivity multiplier
+	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 {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+};
+
+struct MPU9250SensorConfig {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+};
+
+struct ICM20948SensorConfig {
+	// gyroscope bias
+	int32_t G[3];
+
+	// accelerometer bias
+	int32_t A[3];
+
+	// compass bias
+	int32_t C[3];
+};
+
+struct ICM42688SensorConfig {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+};
+
+struct BNO0XXSensorConfig {
+	bool magEnabled;
+};
+
+enum class SensorConfigType {
+	NONE,
+	BMI160,
+	MPU6050,
+	MPU9250,
+	ICM20948,
+	SFUSION,
+	BNO0XX,
+	RUNTIME_CALIBRATION,
+};
+
+const char* calibrationConfigTypeToString(SensorConfigType type);
+
+struct SensorConfig {
+	SensorConfigType type;
+
+	union {
+		BMI160SensorConfig bmi160;
+		SoftFusionSensorConfig sfusion;
+		MPU6050SensorConfig mpu6050;
+		MPU9250SensorConfig mpu9250;
+		ICM20948SensorConfig icm20948;
+		BNO0XXSensorConfig bno0XX;
+		RuntimeCalibrationSensorConfig runtimeCalibration;
+	} data;
+};
+
+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,60 +1,105 @@
 /*
-    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
-#define IMU_UNKNOWN 0
-#define IMU_MPU9250 1
-#define IMU_MPU6500 2
-#define IMU_BNO080 3
-#define IMU_BNO085 4
-#define IMU_BNO055 5
-#define IMU_MPU6050 6
-#define IMU_BNO086 7
-#define IMU_BMI160 8
-#define IMU_ICM20948 9
-#define IMU_ICM42688 10
-#define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+
+#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
+#define IMU_BNO080 BNO080Sensor
+#define IMU_BNO085 BNO085Sensor
+#define IMU_BNO055 BNO055Sensor
+#define IMU_MPU6050 MPU6050Sensor
+#define IMU_BNO086 BNO086Sensor
+#define IMU_BMI160 SoftFusionBMI160
+#define IMU_ICM20948 ICM20948Sensor
+#define IMU_ICM42688 SoftFusionICM42688
+#define IMU_BMI270 SoftFusionBMI270
+#define IMU_LSM6DS3TRC SoftFusionLSM6DS3TRC
+#define IMU_LSM6DSV SoftFusionLSM6DSV
+#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 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_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+#define BOARD_XIAO_ESP32C3 17
+#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_GENERIC_NRF 24
+#define BOARD_SLIMEVR_BUTTERFLY_DEV 25
+#define BOARD_SLIMEVR_BUTTERFLY 26
+#define BOARD_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
 
 #define BAT_EXTERNAL 1
 #define BAT_INTERNAL 2
@@ -63,28 +108,34 @@
 
 #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
 
-#define DEG_0 0.f
-#define DEG_90 -PI / 2
-#define DEG_180 PI
-#define DEG_270 PI / 2
+// 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)
+#define DEG_90 DEG_X(90.0f)
+#define DEG_180 DEG_X(180.0f)
+#define DEG_270 DEG_X(270.0f)
 
 #define CONST_EARTH_GRAVITY 9.80665
 
@@ -95,24 +146,41 @@
 #define BMI160_MAG_TYPE_HMC 1
 #define BMI160_MAG_TYPE_QMC 2
 
-#define MCU_UKNOWN 0
+#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_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_NRF52 9
+#define MCU_NRF54L 10
+#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_UKNOWN
+#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,51 +1,57 @@
 /*
-    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 BNO_USE_MAGNETOMETER_CORRECTION false // Set to true to enable magnetometer correction for BNO08x IMUs. Only works with USE_6_AXIS set to true.
-#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
+#define EXT_SERIAL_COMMANDS false  // Set to true to enable extra serial debug commands
+
+// 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
@@ -56,7 +62,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
@@ -70,7 +76,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,11 +86,34 @@
 
 #define I2C_SPEED 400000
 
+#define COMPLIANCE_MODE true
+#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
+
 // Send inspection packets over the network to a profiler
 // Not recommended for production
 #define ENABLE_INSPECTION false
 
-#define FIRMWARE_BUILD_NUMBER 16
-#define FIRMWARE_VERSION "0.3.3"
+#define PROTOCOL_VERSION 22
 
-#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,167 +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, 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_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
 #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
-    { "A4:E5:7C:B6:00:01", SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 },
-    { "A4:E5:7C:B6:00:02", SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 },
-    { "A4:E5:7C:B6:00:03", SENSORID_PRIMARY, .spins = 10, .x = 11, .y =  2.2, .z =  -1 },
-    { "A4:E5:7C:B6:00:04", SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 },
-    { "A4:E5:7C:B6:00:05", 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,35 @@
 #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 IMU_USE_EXTERNAL_CLOCK
+#define IMU_USE_EXTERNAL_CLOCK true  // Use external clock for IMU (ICM-45686 only)
 #endif
 
-#endif // SLIMEVR_GLOBALS_H_
+#ifndef VENDOR_NAME
+#define VENDOR_NAME "Unknown"
+#endif
+
+#ifndef VENDOR_URL
+#define VENDOR_URL ""
+#endif
+
+#ifndef PRODUCT_NAME
+#define PRODUCT_NAME "DIY 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,44 +1,53 @@
 /*
-    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;
+SlimeVR::WiFiNetwork wifiNetwork;
+SlimeVR::WifiProvisioning wifiProvisioning;
+
+#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
+SlimeVR::Debugging::TimeTakenMeasurer sensorMeasurer{"Sensors"};
+#endif
 
 int sensorToCalibrate = -1;
 bool blinking = false;
@@ -47,100 +56,141 @@ 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...");
+
+	char vendorBuffer[512];
+	size_t writtenLength;
+
+	if (strlen(VENDOR_URL) == 0) {
+		sprintf(
+			vendorBuffer,
+			"Vendor: %s, product: %s%n",
+			VENDOR_NAME,
+			PRODUCT_NAME,
+			&writtenLength
+		);
+	} else {
+		sprintf(
+			vendorBuffer,
+			"Vendor: %s (%s), product: %s%n",
+			VENDOR_NAME,
+			VENDOR_URL,
+			PRODUCT_NAME,
+			&writtenLength
+		);
+	}
+
+	if (strlen(UPDATE_ADDRESS) > 0 && strlen(UPDATE_NAME) > 0) {
+		sprintf(
+			vendorBuffer + writtenLength,
+			", firmware update url: %s, name: %s",
+			UPDATE_ADDRESS,
+			UPDATE_NAME
+		);
+	}
+	logger.info("%s", vendorBuffer);
+
+	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();
-
-#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 || IMU == IMU_BMI160|| IMU == IMU_ICM42688
-    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. Only has been tested on IMUs above. Testing advised when adding other IMUs.
-#endif
-    // 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/CalibrationConfig.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::CalibrationConfigType 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::CalibrationConfigType _type, float _sensitivityLSB) :
-        type(_type),
-        sensitivityLSB(_sensitivityLSB),
-        minTemperatureRange(1000),
-        maxTemperatureRange(-1000)
-    { }
-    
-    bool hasData() {
-        return minTemperatureRange != 1000;
-    }
+	GyroTemperatureCalibrationConfig(
+		SlimeVR::Configuration::SensorConfigType _type,
+		float _sensitivityLSB
+	)
+		: type(_type)
+		, sensitivityLSB(_sensitivityLSB)
+		, minTemperatureRange(1000)
+		, maxTemperatureRange(-1000) {}
 
-    bool fullyCalibrated() {
-        return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs;
-    }
+	bool hasData() { return minTemperatureRange != 1000; }
 
-    float getCalibrationDonePercent() {
-        return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f;
-    }
+	bool fullyCalibrated() {
+		return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs;
+	}
 
-    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;
-    }
+	float getCalibrationDonePercent() {
+		return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f;
+	}
 
-    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 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;
+	}
 };
 
 class GyroTemperatureCalibrator {
 public:
-    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;
+	uint8_t sensorId;
+	GyroTemperatureCalibrationConfig config;
 
-    GyroTemperatureCalibrator(SlimeVR::Configuration::CalibrationConfigType _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);
-    }
+	// 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;
 
-    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();
+	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 reset() {
-        config.reset();
-        configSaved = false;
-        configSaveFailed = false;
-    }
+	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();
 
-    bool isCalibrating() {
-        return calibrationRunning;
-    }
+	void reset() {
+		config.reset();
+		configSaved = false;
+		configSaveFailed = false;
+	}
+
+	bool isCalibrating() { return calibrationRunning; }
 
 private:
-    GyroTemperatureCalibrationState state;
-    uint32_t samplesPerStep;
-    SlimeVR::Logging::Logger m_Logger;
-    
-    float lastApproximatedTemperature = 0.0f;
-    float lastApproximatedOffsets[3];
+	GyroTemperatureCalibrationState state;
+	uint32_t samplesPerStep;
+	SlimeVR::Logging::Logger m_Logger;
 
-    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;
+	float lastApproximatedTemperature = 0.0f;
+	float lastApproximatedOffsets[3];
 
-    void resetCurrentTemperatureState();
+	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();
 };
 
-#endif
+#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;
-    uint32_t restMinTimeMicros;
-    sensor_real_t restFilterTau;
-    sensor_real_t restThGyr;
-    sensor_real_t restThAcc;
-    RestDetectionParams():
-        biasClip(2.0f),
-        biasSigmaRest(0.03f),
-        restMinTimeMicros(1.5 * 1e6),
-        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(uint32_t dtMicros, 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) {
-            restTimeMicros = 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) {
-            restTimeMicros = 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(uint32_t dtMicros, 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)) {
-            restTimeMicros = 0;
-            restDetected = false;
-        } else {
-            restTimeMicros += dtMicros;
-            if (restTimeMicros >= params.restMinTimeMicros) {
-                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)) {
-            restTimeMicros = 0;
-            restDetected = false;
-        } else {
-            restTimeMicros += dtMicros;
-            if (restTimeMicros >= params.restMinTimeMicros) {
-                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;
-        restTimeMicros = 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;
-    uint32_t restTimeMicros;
-    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) {
@@ -89,9 +65,9 @@ bool Connection::endPacket() {
 		MUST_TRANSFER_BOOL((innerPacketSize > 0));
 
 		m_IsBundle = false;
-		
+
 		if (m_BundlePacketInnerCount == 0) {
-			sendPacketType(PACKET_BUNDLE);
+			sendPacketType(SendPacketType::Bundle);
 			sendPacketNumber();
 		}
 		sendShort(innerPacketSize);
@@ -128,13 +104,13 @@ bool Connection::endBundle() {
 	MUST_TRANSFER_BOOL(m_IsBundle);
 
 	m_IsBundle = false;
-	
+
 	MUST_TRANSFER_BOOL((m_BundlePacketInnerCount > 0));
 
 	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);
@@ -191,20 +165,24 @@ bool Connection::sendPacketNumber() {
 }
 
 bool Connection::sendShortString(const char* str) {
-	uint8_t size = strlen(str);
+	size_t size = strlen(str);
 
-	MUST_TRANSFER_BOOL(sendByte(size));
-	MUST_TRANSFER_BOOL(sendBytes((const uint8_t*)str, size));
+	assert(size <= 255);
+
+	MUST_TRANSFER_BOOL(sendByte(static_cast<uint8_t>(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,86 +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) {
+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((uint8_t)sensor->getSensorState()));
-	MUST(sendByte(sensor->getSensorType()));
-
-	MUST(endPacket());
+			.tpsCounterAveragedTps = sensor.m_tpsCounter.getAveragedTPS(),
+			.dataCounterAveragedTps = sensor.m_dataCounter.getAveragedTPS(),
+		}
+	));
 }
 
 // PACKET_ROTATION_DATA 17
@@ -310,103 +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);
+	sendPacketCallback(SendPacketType::FeatureFlags, [&]() {
+		return write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size());
+	});
+}
 
-	MUST(beginPacket());
+// PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24
 
-	MUST(sendPacketType(PACKET_FEATURE_FLAGS));
-	MUST(sendPacketNumber());
-	MUST(write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size()));
-
-	MUST(endPacket());
+void Connection::sendAcknowledgeConfigChange(
+	uint8_t sensorId,
+	SensorToggles configType
+) {
+	MUST(m_Connected);
+	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(IMU));
-	MUST(sendInt(HARDWARE_MCU));
-	MUST(sendInt(0));
-	MUST(sendInt(0));
-	MUST(sendInt(0));
-	MUST(sendInt(FIRMWARE_BUILD_NUMBER));
-	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
@@ -426,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(
@@ -471,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
 
@@ -511,7 +501,7 @@ void Connection::returnLastPacket(int len) {
 	MUST(endPacket());
 }
 
-void Connection::updateSensorState(std::vector<Sensor *> & sensors) {
+void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors) {
 	if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
 		return;
 	}
@@ -519,13 +509,13 @@ void Connection::updateSensorState(std::vector<Sensor *> & sensors) {
 	m_LastSensorInfoPacketTimestamp = millis();
 
 	for (int i = 0; i < (int)sensors.size(); i++) {
-		if (m_AckedSensorState[i] != sensors[i]->getSensorState()) {
-			sendSensorInfo(sensors[i]);
+		if (isSensorStateUpdated(i, sensors[i])) {
+			sendSensorInfo(*sensors[i]);
 		}
 	}
 }
 
-void Connection::maybeRequestFeatureFlags() {	
+void Connection::maybeRequestFeatureFlags() {
 	if (m_ServerFeatures.isAvailable() || m_FeatureFlagsRequestAttempts >= 15) {
 		return;
 	}
@@ -539,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();
@@ -547,7 +545,7 @@ void Connection::searchForServer() {
 		}
 
 		// receive incoming UDP packets
-		int len = m_UDP.read(m_Packet, sizeof(m_Packet));
+		[[maybe_unused]] int len = m_UDP.read(m_Packet, sizeof(m_Packet));
 
 #ifdef DEBUG_NETWORK
 		m_Logger.trace(
@@ -561,7 +559,8 @@ void Connection::searchForServer() {
 
 		// 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;
@@ -571,9 +570,9 @@ void Connection::searchForServer() {
 			m_ServerPort = m_UDP.remotePort();
 			m_LastPacketTimestamp = millis();
 			m_Connected = true;
-			
+
 			m_FeatureFlagsRequestAttempts = 0;
-			m_ServerFeatures = ServerFeatures { };
+			m_ServerFeatures = ServerFeatures{};
 
 			statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
 			ledManager.off();
@@ -603,31 +602,60 @@ 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);
 
+	// Reset server address to broadcast if disconnected
+	m_ServerHost = IPAddress(255, 255, 255, 255);
+
 	statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
 }
 
 void Connection::update() {
-	std::vector<Sensor *> & 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");
 
+		// Reset server address to broadcast if disconnected
+		m_ServerHost = IPAddress(255, 255, 255, 255);
+
 		return;
 	}
 
@@ -636,7 +664,6 @@ void Connection::update() {
 		return;
 	}
 
-	m_LastPacketTimestamp = millis();
 	int len = m_UDP.read(m_Packet, sizeof(m_Packet));
 
 #ifdef DEBUG_NETWORK
@@ -651,45 +678,61 @@ void Connection::update() {
 	(void)packetSize;
 #endif
 
-	switch (convert_chars<int>(m_Packet)) {
-		case PACKET_RECEIVE_HEARTBEAT:
+	if (static_cast<ReceivePacketType>(m_Packet[3]) == ReceivePacketType::Handshake) {
+		m_Logger.warn("Handshake received again, ignoring");
+		return;
+	}
+
+	m_LastPacketTimestamp = millis();
+	switch (static_cast<ReceivePacketType>(m_Packet[3])) {
+		case ReceivePacketType::HeartBeat:
 			sendHeartbeat();
 			break;
 
-		case PACKET_RECEIVE_VIBRATE:
+		case ReceivePacketType::Vibrate:
 			break;
 
-		case PACKET_RECEIVE_HANDSHAKE:
-			// Assume handshake successful
-			m_Logger.warn("Handshake received again, ignoring");
+		case ReceivePacketType::Handshake:
+			// handled above
 			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");
@@ -697,21 +740,56 @@ void Connection::update() {
 			}
 
 			bool hadFlags = m_ServerFeatures.isAvailable();
-			
+
 			uint32_t flagsLength = len - 12;
 			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 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;
+			}
+
+			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(flag, newState);
+				}
+			} else {
+				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, flag);
+			configuration.save();
+			break;
+		}
 	}
 }
 
-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network

src/network/connection.h

@@ -26,17 +26,38 @@
 #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:
+	Connection() {
+#ifdef SERVER_IP
+		m_ServerHost.fromString(SERVER_IP);
+#endif
+
+#ifdef SERVER_PORT
+		m_ServerPort = SERVER_PORT;
+#endif
+	}
+
 	void searchForServer();
 	void update();
 	void reset();
@@ -74,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,
@@ -107,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<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);
@@ -135,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);
@@ -146,7 +209,9 @@ private:
 	void sendTrackerDiscovery();
 
 	// PACKET_SENSOR_INFO 15
-	void sendSensorInfo(Sensor* sensor);
+	void sendSensorInfo(::Sensor& sensor);
+
+	void sendAcknowledgeConfigChange(uint8_t sensorId, SensorToggles configType);
 
 	bool m_Connected = false;
 	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection");
@@ -160,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;
@@ -174,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

@@ -1,103 +1,105 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2023 SlimeVR Contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2023 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_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 {
-        // Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64).
-        PROTOCOL_BUNDLE_SUPPORT,
+	enum EServerFeatureFlags : uint32_t {
+		// Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64).
+		PROTOCOL_BUNDLE_SUPPORT,
 
-        // Add new flags here
+		// Add new flags here
 
-        BITS_TOTAL,
-    };
+		BITS_TOTAL,
+	};
 
-    bool has(EServerFeatureFlags flag) {
-        uint32_t bit = static_cast<uint32_t>(flag);
-        return m_Available && (m_Flags[bit / 8] & (1 << (bit % 8)));
-    }
+	bool has(EServerFeatureFlags flag) {
+		uint32_t bit = static_cast<uint32_t>(flag);
+		return m_Available && (m_Flags[bit / 8] & (1 << (bit % 8)));
+	}
 
-    /**
-     * 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;
-    }
+	/**
+	 * 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; }
 
-    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));
-        return res;
-    }
+	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)
+		);
+		return res;
+	}
 
 private:
-    bool m_Available = false;
+	bool m_Available = false;
 
-    uint8_t m_Flags[static_cast<uint32_t>(EServerFeatureFlags::BITS_TOTAL) / 8 + 1];
+	uint8_t m_Flags[static_cast<uint32_t>(EServerFeatureFlags::BITS_TOTAL) / 8 + 1];
 };
 
 class FirmwareFeatures {
 public:
-    enum EFirmwareFeatureFlags: uint32_t {
-        // EXAMPLE_FEATURE,
+	enum EFirmwareFeatureFlags : uint32_t {
+		// EXAMPLE_FEATURE,
 		B64_WIFI_SCANNING = 1,
+		SENSOR_CONFIG = 2,
+		// Add new flags here
 
-        // Add new flags here
+		BITS_TOTAL,
+	};
 
-        BITS_TOTAL,
-    };
-
-    // Flags to send
-    static constexpr const std::initializer_list<EFirmwareFeatureFlags> flagsEnabled = {
-        // EXAMPLE_FEATURE,
+	// Flags to send
+	static constexpr const std::initializer_list<EFirmwareFeatureFlags> flagsEnabled = {
+		// EXAMPLE_FEATURE,
 		B64_WIFI_SCANNING,
+		SENSOR_CONFIG
+		// Add enabled flags here
+	};
 
-        // Add enabled flags here
-    };
+	static constexpr auto flags = [] {
+		constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1;
+		std::array<uint8_t, flagsLength> packed{};
 
-    static constexpr auto flags = []{
-        constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1;
-        std::array<uint8_t, flagsLength> packed{};
+		for (uint32_t bit : flagsEnabled) {
+			packed[bit / 8] |= 1 << (bit % 8);
+		}
 
-        for (uint32_t bit : flagsEnabled) {
-            packed[bit / 8] |= 1 << (bit % 8);
-        }
-
-        return packed;
-    }();
+		return packed;
+	}();
 };
 
 #endif

src/network/manager.cpp

@@ -24,17 +24,16 @@
 
 #include "GlobalVars.h"
 
-namespace SlimeVR {
-namespace Network {
+namespace SlimeVR::Network {
 
-void Manager::setup() { ::WiFiNetwork::setUp(); }
+void Manager::setup() { wifiNetwork.setUp(); }
 
 void Manager::update() {
-	WiFiNetwork::upkeep();
+	wifiNetwork.upkeep();
 
 	auto wasConnected = m_IsConnected;
 
-	m_IsConnected = ::WiFiNetwork::isConnected();
+	m_IsConnected = wifiNetwork.isConnected();
 
 	if (!m_IsConnected) {
 		return;
@@ -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,43 +24,213 @@
 #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
+#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,
+	// PositionData = 27,
+	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,230 +1,387 @@
 /*
-    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 "globals.h"
-#include "logging/Logger.h"
+#include "network/wifihandler.h"
+
 #include "GlobalVars.h"
+#include "globals.h"
 #if !ESP8266
 #include "esp_wifi.h"
+#include "esp_wifi_types.h"
 #endif
 
-unsigned long lastWifiReportTime = 0;
-unsigned long wifiConnectionTimeout = millis();
-bool isWifiConnected = false;
-uint8_t wifiState = SLIME_WIFI_NOT_SETUP;
-bool hadWifi = false;
-unsigned long last_rssi_sample = 0;
+namespace SlimeVR {
 
-// TODO: Cleanup with proper classes
-SlimeVR::Logging::Logger wifiHandlerLogger("WiFiHandler");
-
-void reportWifiError() {
-    if(lastWifiReportTime + 1000 < millis()) {
-        lastWifiReportTime = millis();
-        Serial.print(".");
-    }
+void WiFiNetwork::reportWifiProgress() {
+	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
+void WiFiNetwork::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
 }
 
-bool WiFiNetwork::isConnected() {
-    return isWifiConnected;
+bool WiFiNetwork::isConnected() const {
+	return wifiState == WiFiReconnectionStatus::Success;
 }
 
-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) {
+	wifiProvisioning.stopProvisioning();
+	tryConnecting(false, SSID, pass);
+	retriedOnG = false;
+	// Reset state, will get back into provisioning if can't connect
+	hadWifi = false;
+	wifiState = WiFiReconnectionStatus::ServerCredAttempt;
 }
 
-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
-    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);
+	WiFi.hostname("SlimeVR FBT Tracker");
+	wifiHandlerLogger.info(
+		"Loaded credentials for SSID '%s' and pass length %d",
+		getSSID().c_str(),
+		getPassword().length()
+	);
+
+	trySavedCredentials();
 
 #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());
+void WiFiNetwork::onConnected() {
+	wifiState = WiFiReconnectionStatus::Success;
+	wifiProvisioning.stopProvisioning();
+	statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false);
+	hadWifi = true;
+	wifiHandlerLogger.info(
+		"Connected successfully to SSID '%s', IP address %s",
+		getSSID().c_str(),
+		WiFi.localIP().toString().c_str()
+	);
+	// Reset it, in case we just connected with server creds
 }
 
-uint8_t WiFiNetwork::getWiFiState() {
-    return wifiState;
+String WiFiNetwork::getSSID() {
+#if ESP8266
+	return WiFi.SSID();
+#else
+	// Necessary, because without a WiFi.begin(), ESP32 is not kind enough to load the
+	// SSID on its own, for whatever reason
+	wifi_config_t wifiConfig;
+	esp_wifi_get_config((wifi_interface_t)ESP_IF_WIFI_STA, &wifiConfig);
+	return {reinterpret_cast<char*>(wifiConfig.sta.ssid)};
+#endif
 }
 
+String WiFiNetwork::getPassword() {
+#if ESP8266
+	return WiFi.psk();
+#else
+	// Same as above
+	wifi_config_t wifiConfig;
+	esp_wifi_get_config((wifi_interface_t)ESP_IF_WIFI_STA, &wifiConfig);
+	return {reinterpret_cast<char*>(wifiConfig.sta.password)};
+#endif
+}
+
+WiFiNetwork::WiFiReconnectionStatus 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) {
-                            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
-                            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
-                        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
-                        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
-                        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;
+	wifiProvisioning.upkeepProvisioning();
+
+	if (WiFi.status() == WL_CONNECTED) {
+		if (!isConnected()) {
+			onConnected();
+			return;
+		}
+
+		if (millis() - lastRssiSample >= 2000) {
+			lastRssiSample = millis();
+			uint8_t signalStrength = WiFi.RSSI();
+			networkConnection.sendSignalStrength(signalStrength);
+		}
+		return;
+	}
+
+	if (isConnected()) {
+		statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true);
+		wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting...");
+		trySavedCredentials();
+		return;
+	}
+
+	if (wifiState != WiFiReconnectionStatus::Failed) {
+		reportWifiProgress();
+	}
+
+	if (millis() - wifiConnectionTimeout
+			< static_cast<uint32_t>(WiFiTimeoutSeconds * 1000)
+		&& WiFi.status() == WL_DISCONNECTED) {
+		return;
+	}
+
+	switch (wifiState) {
+		case WiFiReconnectionStatus::NotSetup:  // Wasn't set up
+			return;
+		case WiFiReconnectionStatus::SavedAttempt:  // Couldn't connect with
+													// first set of
+													// credentials
+			if (!trySavedCredentials()) {
+				tryHardcodedCredentials();
+			}
+			return;
+		case WiFiReconnectionStatus::HardcodeAttempt:  // Couldn't connect with
+													   // second set of credentials
+			if (!tryHardcodedCredentials()) {
+				wifiState = WiFiReconnectionStatus::Failed;
+			}
+			return;
+		case WiFiReconnectionStatus::ServerCredAttempt:  // Couldn't connect with
+														 // server-sent credentials.
+			if (!tryServerCredentials()) {
+				wifiState = WiFiReconnectionStatus::Failed;
+			}
+			return;
+		case WiFiReconnectionStatus::Failed:  // Couldn't connect with second set of
+											  // credentials or server credentials
+// Return to the default PHY Mode N.
+#if ESP8266
+			if constexpr (USE_ATTENUATION) {
+				WiFi.setOutputPower(20.0 - ATTENUATION_N);
+			}
+			WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+#endif
+			// Start smart config
+			if (!hadWifi && !WiFi.smartConfigDone()
+				&& millis() - wifiConnectionTimeout
+					   >= static_cast<uint32_t>(WiFiTimeoutSeconds * 1000)) {
+				if (WiFi.status() != WL_IDLE_STATUS) {
+					wifiHandlerLogger.error(
+						"Can't connect from any credentials, error: %d, reason: %s.",
+						static_cast<int>(statusToFailure(WiFi.status())),
+						statusToReasonString(WiFi.status())
+					);
+					wifiConnectionTimeout = millis();
+				}
+				wifiProvisioning.startProvisioning();
+			}
+			return;
+	}
 }
+
+const char* WiFiNetwork::statusToReasonString(wl_status_t status) {
+	switch (status) {
+		case WL_DISCONNECTED:
+			return "Timeout";
+#ifdef ESP8266
+		case WL_WRONG_PASSWORD:
+			return "Wrong password";
+		case WL_CONNECT_FAILED:
+			return "Connection failed";
+#elif ESP32
+		case WL_CONNECT_FAILED:
+			return "Wrong password";
+#endif
+
+		case WL_NO_SSID_AVAIL:
+			return "SSID not found";
+		default:
+			return "Unknown";
+	}
+}
+
+WiFiNetwork::WiFiFailureReason WiFiNetwork::statusToFailure(wl_status_t status) {
+	switch (status) {
+		case WL_DISCONNECTED:
+			return WiFiFailureReason::Timeout;
+#ifdef ESP8266
+		case WL_WRONG_PASSWORD:
+			return WiFiFailureReason::WrongPassword;
+#elif ESP32
+		case WL_CONNECT_FAILED:
+			return WiFiFailureReason::WrongPassword;
+#endif
+
+		case WL_NO_SSID_AVAIL:
+			return WiFiFailureReason::SSIDNotFound;
+		default:
+			return WiFiFailureReason::Unknown;
+	}
+}
+
+void WiFiNetwork::showConnectionAttemptFailed(const char* type) const {
+	wifiHandlerLogger.error(
+		"Can't connect from %s credentials, error: %d, reason: %s.",
+		type,
+		static_cast<int>(statusToFailure(WiFi.status())),
+		statusToReasonString(WiFi.status())
+	);
+}
+
+bool WiFiNetwork::trySavedCredentials() {
+	if (getSSID().length() == 0) {
+		wifiHandlerLogger.debug("Skipping saved credentials attempt on 0-length SSID..."
+		);
+		wifiState = WiFiReconnectionStatus::HardcodeAttempt;
+		return false;
+	}
+
+	if (wifiState == WiFiReconnectionStatus::SavedAttempt) {
+		showConnectionAttemptFailed("saved");
+
+		if (WiFi.status() != WL_DISCONNECTED) {
+			return false;
+		}
+
+		if (retriedOnG) {
+			return false;
+		}
+
+		retriedOnG = true;
+		wifiHandlerLogger.debug("Trying saved credentials with PHY Mode G...");
+		return tryConnecting(true);
+	}
+
+	retriedOnG = false;
+
+	wifiState = WiFiReconnectionStatus::SavedAttempt;
+	return tryConnecting();
+}
+
+bool WiFiNetwork::tryHardcodedCredentials() {
+#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
+	if (wifiState == WiFiReconnectionStatus::HardcodeAttempt) {
+		showConnectionAttemptFailed("hardcoded");
+
+		if (WiFi.status() != WL_DISCONNECTED) {
+			return false;
+		}
+
+		if (retriedOnG) {
+			return false;
+		}
+
+		retriedOnG = true;
+		wifiHandlerLogger.debug("Trying hardcoded credentials with PHY Mode G...");
+		// Don't need to save hardcoded credentials
+		WiFi.persistent(false);
+		auto result = tryConnecting(true, WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
+		WiFi.persistent(true);
+		return result;
+	}
+
+	retriedOnG = false;
+
+	wifiState = WiFiReconnectionStatus::HardcodeAttempt;
+	// Don't need to save hardcoded credentials
+	WiFi.persistent(false);
+	auto result = tryConnecting(false, WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
+	WiFi.persistent(true);
+	return result;
+#else
+	wifiState = WiFiReconnectionStatus::HardcodeAttempt;
+	return false;
+#endif
+}
+
+bool WiFiNetwork::tryServerCredentials() {
+	if (WiFi.status() != WL_DISCONNECTED) {
+		return false;
+	}
+
+	if (retriedOnG) {
+		return false;
+	}
+
+	retriedOnG = true;
+
+	return tryConnecting(true);
+}
+
+bool WiFiNetwork::tryConnecting(bool phyModeG, const char* SSID, const char* pass) {
+#if ESP8266
+	if (phyModeG) {
+		WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+		if constexpr (USE_ATTENUATION) {
+			WiFi.setOutputPower(20.0 - ATTENUATION_G);
+		}
+	} else {
+		WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+		if constexpr (USE_ATTENUATION) {
+			WiFi.setOutputPower(20.0 - ATTENUATION_N);
+		}
+	}
+#else
+	if (phyModeG) {
+		return false;
+	}
+#endif
+
+	setStaticIPIfDefined();
+	if (SSID == nullptr) {
+		WiFi.begin();
+	} else {
+		WiFi.begin(SSID, pass);
+	}
+	wifiConnectionTimeout = millis();
+	return true;
+}
+
+}  // namespace SlimeVR

src/network/wifihandler.h

@@ -1,52 +1,97 @@
 /*
-    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_
+#pragma once
 
+#include "logging/Logger.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();
-}
+namespace SlimeVR {
+
+class WiFiNetwork {
+public:
+	enum class WiFiReconnectionStatus {
+		NotSetup = 0,
+		SavedAttempt,
+		HardcodeAttempt,
+		ServerCredAttempt,
+		Failed,
+		Success
+	};
+
+	enum class WiFiFailureReason {
+		Timeout = 0,
+		SSIDNotFound = 1,
+		WrongPassword = 2,
+		Unknown = 3,
+	};
+
+	[[nodiscard]] bool isConnected() const;
+	void setUp();
+	void upkeep();
+	void setWiFiCredentials(const char* SSID, const char* pass);
+	static IPAddress getAddress();
+	WiFiReconnectionStatus getWiFiState();
+
+private:
+	static constexpr float WiFiTimeoutSeconds = 11;
+
+	void reportWifiProgress();
+	void setStaticIPIfDefined();
+	void onConnected();
+
+	static String getSSID();
+	static String getPassword();
+
+	bool trySavedCredentials();
+	bool tryHardcodedCredentials();
+	bool tryServerCredentials();
+	bool tryConnecting(
+		bool phyModeG = false,
+		const char* SSID = nullptr,
+		const char* pass = nullptr
+	);
+
+	void showConnectionAttemptFailed(const char* type) const;
+
+	static const char* statusToReasonString(wl_status_t status);
+	static WiFiFailureReason statusToFailure(wl_status_t status);
+
+	unsigned long lastWifiReportTime = 0;
+	unsigned long wifiConnectionTimeout = millis();
+	bool isWifiConnected = false;
+	WiFiReconnectionStatus wifiState = WiFiReconnectionStatus::NotSetup;
+	bool retriedOnG = false;
+	bool hadWifi = false;
+	unsigned long lastRssiSample = 0;
+
+	uint8_t lastFailStatus = 0;
+
+	SlimeVR::Logging::Logger wifiHandlerLogger{"WiFiHandler"};
+};
 
 /** 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
-} wifi_reconnection_statuses;
-
-#endif // SLIMEVR_WIFI_H_
+}  // namespace SlimeVR

src/network/wifiprovisioning.cpp

@@ -1,57 +1,59 @@
 /*
-    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.
 // TODO: New implementation: https://github.com/SlimeVR/SlimeVR-Tracker-ESP/issues/71
 
-// TODO: Cleanup with proper classes
-SlimeVR::Logging::Logger wifiProvisioningLogger("WiFiProvisioning");
-bool provisioning = false;
+namespace SlimeVR {
 
-void WiFiNetwork::upkeepProvisioning() {
-    // Called even when not provisioning to do things like provide neighbours or other upkeep
+void WifiProvisioning::upkeepProvisioning() {
+	// 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");
-    }
+void WifiProvisioning::startProvisioning() {
+	if (WiFi.beginSmartConfig()) {
+		provisioning = true;
+		wifiProvisioningLogger.info("SmartConfig started");
+	}
 }
 
-void WiFiNetwork::stopProvisioning() {
-    WiFi.stopSmartConfig();
-    provisioning = false;
+void WifiProvisioning::stopProvisioning() {
+	WiFi.stopSmartConfig();
+	provisioning = false;
 }
 
-void WiFiNetwork::provideNeighbours() {
-    // TODO: SmartConfig can't do this, created for future
+void WifiProvisioning::provideNeighbours() {
+	// TODO: SmartConfig can't do this, created for future
 }
 
-bool WiFiNetwork::isProvisioning() {
-    return provisioning && !WiFi.smartConfigDone();
+bool WifiProvisioning::isProvisioning() const {
+	return provisioning && !WiFi.smartConfigDone();
 }
+
+}  // namespace SlimeVR

src/network/wifiprovisioning.h

@@ -1,34 +1,45 @@
 /*
-    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();
-}
+#include "logging/Logger.h"
 
-#endif // SLIMEVR_WIFIPROVISIONING_H_
+namespace SlimeVR {
+
+class WifiProvisioning {
+public:
+	void upkeepProvisioning();
+	void startProvisioning();
+	void stopProvisioning();
+	bool isProvisioning() const;
+	void provideNeighbours();
+
+private:
+	SlimeVR::Logging::Logger wifiProvisioningLogger{"WiFiProvisioning"};
+	bool provisioning = false;
+};
+
+}  // namespace SlimeVR
+
+#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