Add nrf const to consts.h (#505 )

Bump actions/upload-artifact from 6 to 7 (#517 )
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>
2026-04-06 02:01:57 +02:00 · 2026-03-24 19:20:30 +02:00 · 2026-03-02 15:01:06 +03:00 · 2026-01-25 07:07:30 +02:00 · 2026-01-07 08:56:52 +03:00 · 2025-12-15 11:09:31 +03:00
159 changed files with 11368 additions and 8911 deletions
--- a/.github/CODEOWNERS
+++ b/.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/
--- a/.github/workflows/actions.yml
+++ b/.github/workflows/actions.yml
@@ -4,6 +4,8 @@ on:
  push:
    branches:
      - main
+    tags:
+      - "*"
  pull_request:
  workflow_dispatch:
  create:
@@ -12,35 +14,38 @@ jobs:
  format:
    runs-on: ubuntu-latest
    steps:
-      - uses: actions/checkout@v4
-      - uses: jidicula/clang-format-action@v4.13.0
+      - 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++'
+  #      Disable clang-tidy for now
+  #      - name: Get clang-tidy
+  #        run: |
+  #          apt-get update
+  #          apt-get install -y clang-tidy
+  #      - uses: ZehMatt/clang-tidy-annotations@v1.0.0
+  #        with:
+  #          build_dir: 'build'
+  #          cmake_args: '-G Ninja -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++'

  build:
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-latest

    steps:
-      - uses: actions/checkout@v4
-      - uses: actions/cache@v4
+      - 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@v5
+        uses: actions/setup-python@v6
        with:
          python-version: "3.12"

@@ -53,7 +58,7 @@ jobs:
        run: python ./ci/build.py

      - name: Upload binaries
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v7
        with:
          name: binaries
          path: ./build/*.bin
@@ -66,3 +71,4 @@ jobs:
          generate_release_notes: true
          files: |
            ./build/BOARD_SLIMEVR-firmware.bin
+            ./build/BOARD_SLIMEVR_V1_2-firmware.bin
--- a/.github/workflows/release.yml
+++ b/.github/workflows/release.yml
@@ -1,20 +0,0 @@
-name: Releases
-
-on:
-  push:
-    tags:
-    - '*'
-
-jobs:
-
-  build:
-    runs-on: ubuntu-latest
-    permissions:
-      contents: write
-    steps:
-    - uses: actions/checkout@v4
-    - uses: ncipollo/release-action@v1
-      with:
-        artifacts: "./build/*.bin"
-        draft: true
-        token: ${{ secrets.GITHUB_TOKEN }}
--- a/.gitignore
+++ b/.gitignore
@@ -4,3 +4,7 @@ build/
 venv/
 cache/
 .idea/
+compile_commands.json
+node_modules/
+dist/
+.nix-platformio
--- a/board-defaults.json
+++ b/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
+			}
+		}
+	}
+}
--- a/board-defaults.schema.json
+++ b/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"]
+}
--- a/boards/esp32s3_supermini.json
+++ b/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"
+}
+  
--- a/boards/variants/esp32s3_supermini/pins_arduino.h
+++ b/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 */
--- a/ci/build.py
+++ b/ci/build.py
@@ -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)

--- a/flake.lock
+++ b/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
+}
--- a/flake.nix
+++ b/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 ""
+          '';
+        };
+      }
+    );
+}
--- a/lib/bmi160/BMI160.cpp
+++ b/lib/bmi160/BMI160.cpp
--- a/lib/bmi160/BMI160.h
+++ b/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_ */
--- a/lib/bno080/BNO080.cpp
+++ b/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
--- a/lib/bno080/BNO080.h
+++ b/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;
 };
--- a/lib/bno080/PinInterface.h
+++ b/lib/bno080/PinInterface.h
@@ -1,6 +1,6 @@
 /*
    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+    Copyright (c) 2024 Eiren Rain & SlimeVR contributors

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
@@ -20,31 +20,18 @@
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.
 */
+#pragma once

-#ifndef _MAHONY_H_
-#define _MAHONY_H_
-
-#include "helper_3dmath.h"
-
-template<typename T>
-class Mahony {
-
-    // These are the free parameters in the Mahony filter and fusion scheme,
-    // Kp for proportional feedback, Ki for integral
-    // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
-    static constexpr float Kp = 10.0f;
-    static constexpr float Ki = 0.0f;
+#include <cstdint>
+#include <string>

+class PinInterface
+{
 public:
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat);
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat);
-    
-private:
-    T ix = 0.0;
-    T iy = 0.0;
-    T iz = 0.0;
+	virtual bool init() { return true; };
+	virtual int digitalRead() = 0;
+	virtual void pinMode(uint8_t mode) = 0;
+	virtual void digitalWrite(uint8_t val) = 0;
+
+	[[nodiscard]] virtual std::string toString() const = 0;
 };
-
-#include "mahony.hpp"
-
-#endif /* _MAHONY_H_ */
--- a/lib/i2cscan/i2cscan.cpp
+++ b/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;
    }
 }
--- a/lib/i2cscan/i2cscan.h
+++ b/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_
--- a/lib/magneto/madgwick.h
+++ b/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_ */
--- a/lib/magneto/madgwick.hpp
+++ b/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;
-}
--- a/lib/magneto/mahony.hpp
+++ b/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;
-}
--- a/lib/math/quat.cpp
+++ b/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;
+}
--- a/lib/math/quat.h
+++ b/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);
--- a/lib/ota/ota.cpp
+++ b/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();
    }
 }
--- a/lib/vqf/vqf.cpp
+++ b/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;
+}
--- a/lib/vqf/vqf.h
+++ b/lib/vqf/vqf.h
--- a/platformio-tools.ini
+++ b/platformio-tools.ini
@@ -1,91 +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++2a
-build_unflags =
-  -Os
-  -std=gnu++11 -std=gnu++17
-
-[env:BOARD_SLIMEVR]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-
-[env:BOARD_SLIMEVR_DEV]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-
-[env:BOARD_NODEMCU]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-
-[env:BOARD_WEMOSD1MINI]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-
-[env:BOARD_TTGO_TBASE]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-
-[env:BOARD_WEMOSWROOM02]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-
-[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
-
-[env:BOARD_ESP01]
-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
-
-[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
-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
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-board = dfrobot_beetle_esp32c3
-
-[env:BOARD_ES32C3DEVKITM1]
-platform = espressif32 @ 6.7.0
-platform_packages =
-  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
-  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-board = esp32-c3-devkitm-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
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-board = seeed_xiao_esp32c3
--- a/platformio.ini
+++ b/platformio.ini
@@ -13,18 +13,22 @@

 [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
@@ -58,39 +62,62 @@ build_unflags = -Os -std=gnu++11 -std=gnu++17
 ;upload_flags =
 ;  --auth=SlimeVR-OTA

-; Settings for different boards
-
-[env:esp12e]
-platform = espressif8266 @ 4.2.1
-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.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.1
-;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.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
-; 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:
@@ -100,12 +127,126 @@ upload_speed = 921600
 ;    -DARDUINO_USB_MODE=1
 ;    -DARDUINO_USB_CDC_ON_BOOT=1

-;[env: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
-;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
--- a/scripts/get_git_commit.py
+++ b/scripts/get_git_commit.py
@@ -18,13 +18,7 @@ else:

 tag = ""
 try:
-	tag = (
-		subprocess.check_output(["git", "--no-pager", "tag", "--sort", "-taggerdate", "--points-at" , "HEAD"])
-			.split("\n")[0]
-			.strip()
-			.decode("utf-8")
-	)
-
+	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:
@@ -40,13 +34,16 @@ try:
 except Exception:
 	branch = ""

-output = f"'-DGIT_REV=\"{revision}\"'"
+output = f"-DGIT_REV='\"{revision}\"'"

-if tag != "":
-	output += f" '-DFIRMWARE_VERSION=\"{tag}\"'"
+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}\"'"
+	output += f" -DFIRMWARE_VERSION='\"{branch}\"'"
 else:
-	output += f" '-DFIRMWARE_VERSION=\"git-{revision}\"'"
+	output += f" -DFIRMWARE_VERSION='\"git-{revision}\"'"

 print(output)
--- a/scripts/preprocessor.py
+++ b/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")
--- a/src/FSHelper.cpp
+++ b/src/FSHelper.cpp
@@ -25,8 +25,7 @@

 #include <functional>

-namespace SlimeVR {
-namespace Utils {
+namespace SlimeVR::Utils {
 SlimeVR::Logging::Logger m_Logger("FSHelper");

 bool ensureDirectory(const char* directory) {
@@ -88,5 +87,4 @@ void forEachFile(const char* directory, std::function<void(File file)> callback)
 	}
 #endif
 }
-}  // namespace Utils
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Utils
--- a/src/FSHelper.h
+++ b/src/FSHelper.h
@@ -29,8 +29,7 @@

 #include <functional>

-namespace SlimeVR {
-namespace Utils {
+namespace SlimeVR::Utils {

 class File {
 public:
@@ -62,7 +61,6 @@ bool ensureDirectory(const char* directory);
 File openFile(const char* path, const char* mode);

 void forEachFile(const char* directory, std::function<void(File file)> callback);
-}  // namespace Utils
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Utils

 #endif
--- a/src/GlobalVars.h
+++ b/src/GlobalVars.h
@@ -20,18 +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;
@@ -42,5 +42,5 @@ extern SlimeVR::Sensors::SensorManager sensorManager;
 extern SlimeVR::Network::Manager networkManager;
 extern SlimeVR::Network::Connection networkConnection;
 extern BatteryMonitor battery;
-
-#endif
+extern SlimeVR::WiFiNetwork wifiNetwork;
+extern SlimeVR::WifiProvisioning wifiProvisioning;
--- a/src/batterymonitor.cpp
+++ b/src/batterymonitor.cpp
@@ -74,7 +74,7 @@ void BatteryMonitor::Loop() {
 		voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * ADCVoltageMax / ADCResolution
 				* ADCMultiplier;
 #endif
-#if ESP32 && BATTERY_MONITOR == BAT_EXTERNAL
+#if defined(ESP32) && BATTERY_MONITOR == BAT_EXTERNAL
 		voltage
 			= ((float)analogReadMilliVolts(PIN_BATTERY_LEVEL)) / 1000 * ADCMultiplier;
 #endif
--- a/src/batterymonitor.h
+++ b/src/batterymonitor.h
@@ -52,9 +52,6 @@
 #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
--- a/src/boards/boards_default.h
+++ b/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
--- a/src/boards/defines_helpers.cpp
+++ b/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
--- a/src/boards/defines_helpers.h
+++ b/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
--- a/src/boards/glove_default.h
+++ b/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
--- a/src/configuration/Configuration.cpp
+++ b/src/configuration/Configuration.cpp
@@ -25,15 +25,20 @@

 #include <LittleFS.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 {
+namespace SlimeVR::Configuration {
 void Configuration::setup() {
 	if (m_Loaded) {
 		return;
@@ -110,13 +115,29 @@ void Configuration::save() {
 		}

 		char path[17];
-		sprintf(path, DIR_CALIBRATIONS "/%d", i);
+		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
+			);
+		}
 	}

 	{
@@ -125,6 +146,18 @@ void Configuration::save() {
 		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");
 }

@@ -132,6 +165,7 @@ void Configuration::reset() {
 	LittleFS.format();

 	m_Sensors.clear();
+	m_SensorToggles.clear();
 	m_Config.version = 1;
 	save();

@@ -160,6 +194,39 @@ void Configuration::setSensor(size_t sensorID, const SensorConfig& config) {
 	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;
@@ -172,8 +239,47 @@ void Configuration::loadSensors() {
 			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(
@@ -380,5 +486,4 @@ void Configuration::print() {
 		}
 	}
 }
-}  // namespace Configuration
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Configuration
--- a/src/configuration/Configuration.h
+++ b/src/configuration/Configuration.h
@@ -27,11 +27,11 @@
 #include <vector>

 #include "../motionprocessing/GyroTemperatureCalibrator.h"
+#include "../sensors/SensorToggles.h"
 #include "DeviceConfig.h"
 #include "logging/Logger.h"

-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 class Configuration {
 public:
 	void setup();
@@ -46,6 +46,9 @@ public:
 	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,
@@ -64,10 +67,10 @@ private:

 	DeviceConfig m_Config{};
 	std::vector<SensorConfig> m_Sensors;
+	std::vector<SensorToggleState> m_SensorToggles;

 	Logging::Logger m_Logger = Logging::Logger("Configuration");
 };
-}  // namespace Configuration
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Configuration

 #endif
--- a/src/configuration/SensorConfig.cpp
+++ b/src/configuration/SensorConfig.cpp
@@ -23,8 +23,7 @@

 #include "SensorConfig.h"

-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 const char* calibrationConfigTypeToString(SensorConfigType type) {
 	switch (type) {
 		case SensorConfigType::NONE:
@@ -41,27 +40,23 @@ const char* calibrationConfigTypeToString(SensorConfigType type) {
 			return "SoftFusion (common)";
 		case SensorConfigType::BNO0XX:
 			return "BNO0XX";
+		case SensorConfigType::RUNTIME_CALIBRATION:
+			return "SoftFusion (runtime calibration)";
 		default:
 			return "UNKNOWN";
 	}
 }

-// 1st bit specifies if magnetometer is enabled or disabled
-// 2nd bit specifies if magnetometer is supported
-uint16_t configDataToNumber(SensorConfig* sensorConfig, bool magSupported) {
-	uint16_t data = 0;
-	data += magSupported << 1;
-	switch (sensorConfig->type) {
-		case SensorConfigType::BNO0XX: {
-			auto config = &sensorConfig->data.bno0XX;
-			data += config->magEnabled;
-			break;
-		}
-		case SensorConfigType::NONE:
-		default:
-			break;
-	}
-	return data;
+bool SensorConfigBits::operator==(const SensorConfigBits& rhs) const {
+	return magEnabled == rhs.magEnabled && magSupported == rhs.magSupported
+		&& calibrationEnabled == rhs.calibrationEnabled
+		&& calibrationSupported == rhs.calibrationSupported
+		&& tempGradientCalibrationEnabled == rhs.tempGradientCalibrationEnabled
+		&& tempGradientCalibrationSupported == rhs.tempGradientCalibrationSupported;
 }
-}  // namespace Configuration
-}  // namespace SlimeVR
+
+bool SensorConfigBits::operator!=(const SensorConfigBits& rhs) const {
+	return !(*this == rhs);
+}
+
+}  // namespace SlimeVR::Configuration
--- a/src/configuration/SensorConfig.h
+++ b/src/configuration/SensorConfig.h
@@ -28,8 +28,7 @@

 #include "consts.h"

-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 struct BMI160SensorConfig {
 	// accelerometer offsets and correction matrix
 	float A_B[3];
@@ -47,7 +46,7 @@ struct BMI160SensorConfig {
 };

 struct SoftFusionSensorConfig {
-	ImuID ImuType;
+	SensorTypeID ImuType;
 	uint16_t MotionlessDataLen;

 	// accelerometer offsets and correction matrix
@@ -73,6 +72,32 @@ struct SoftFusionSensorConfig {
 	float G_Sens[3];

 	uint8_t MotionlessData[60];
+
+	// temperature sampling rate (placed at the end to not break existing configs)
+	float T_Ts;
+};
+
+struct RuntimeCalibrationSensorConfig {
+	SensorTypeID ImuType;
+	uint16_t MotionlessDataLen;
+
+	bool sensorTimestepsCalibrated;
+	float A_Ts;
+	float G_Ts;
+	float M_Ts;
+	float T_Ts;
+
+	bool motionlessCalibrated;
+	uint8_t MotionlessData[60];
+
+	uint8_t gyroPointsCalibrated;
+	float gyroMeasurementTemperature1;
+	float G_off1[3];
+	float gyroMeasurementTemperature2;
+	float G_off2[3];
+
+	bool accelCalibrated[3];
+	float A_off[3];
 };

 struct MPU6050SensorConfig {
@@ -131,7 +156,8 @@ enum class SensorConfigType {
 	MPU9250,
 	ICM20948,
 	SFUSION,
-	BNO0XX
+	BNO0XX,
+	RUNTIME_CALIBRATION,
 };

 const char* calibrationConfigTypeToString(SensorConfigType type);
@@ -146,11 +172,29 @@ struct SensorConfig {
 		MPU9250SensorConfig mpu9250;
 		ICM20948SensorConfig icm20948;
 		BNO0XXSensorConfig bno0XX;
+		RuntimeCalibrationSensorConfig runtimeCalibration;
 	} data;
 };

-uint16_t configDataToNumber(SensorConfig* sensorConfig, bool magSupported);
-}  // namespace Configuration
-}  // namespace SlimeVR
+struct SensorConfigBits {
+	bool magEnabled : 1;
+	bool magSupported : 1;
+	bool calibrationEnabled : 1;
+	bool calibrationSupported : 1;
+	bool tempGradientCalibrationEnabled : 1;
+	bool tempGradientCalibrationSupported : 1;
+
+	// Remove if the above fields exceed a byte, necessary to make the struct 16
+	// bit
+	uint8_t padding;
+
+	bool operator==(const SensorConfigBits& rhs) const;
+	bool operator!=(const SensorConfigBits& rhs) const;
+};
+
+// If this fails, you forgot to do the above
+static_assert(sizeof(SensorConfigBits) == 2);
+
+}  // namespace SlimeVR::Configuration

 #endif
--- a/src/consts.h
+++ b/src/consts.h
@@ -25,7 +25,8 @@

 // List of constants used in other places

-enum class ImuID {
+#include <cstdint>
+enum class SensorTypeID : uint8_t {
 	Unknown = 0,
 	MPU9250,
 	MPU6500,
@@ -42,9 +43,13 @@ enum class ImuID {
 	LSM6DSV,
 	LSM6DSO,
 	LSM6DSR,
+	ICM45686,
+	ICM45605,
+	ADC_RESISTANCE,
 	Empty = 255
 };

+#define IMU_AUTO SensorAuto
 #define IMU_UNKNOWN ErroneousSensor
 #define IMU_MPU9250 MPU9250Sensor
 #define IMU_MPU6500 MPU6050Sensor
@@ -53,7 +58,7 @@ enum class ImuID {
 #define IMU_BNO055 BNO055Sensor
 #define IMU_MPU6050 MPU6050Sensor
 #define IMU_BNO086 BNO086Sensor
-#define IMU_BMI160 BMI160Sensor
+#define IMU_BMI160 SoftFusionBMI160
 #define IMU_ICM20948 ICM20948Sensor
 #define IMU_ICM42688 SoftFusionICM42688
 #define IMU_BMI270 SoftFusionBMI270
@@ -62,28 +67,38 @@ enum class ImuID {
 #define IMU_LSM6DSO SoftFusionLSM6DSO
 #define IMU_LSM6DSR SoftFusionLSM6DSR
 #define IMU_MPU6050_SF SoftFusionMPU6050
+#define IMU_ICM45686 SoftFusionICM45686
+#define IMU_ICM45605 SoftFusionICM45605

 #define IMU_DEV_RESERVED 250  // Reserved, should not be used in any release firmware

 #define BOARD_UNKNOWN 0
-#define BOARD_SLIMEVR_LEGACY 1
-#define BOARD_SLIMEVR_DEV 2
+#define BOARD_SLIMEVR_LEGACY 1  // More ancient development version of SlimeVR
+#define BOARD_SLIMEVR_DEV 2  // Ancient development version of SlimeVR
 #define BOARD_NODEMCU 3
 #define BOARD_CUSTOM 4
 #define BOARD_WROOM32 5
 #define BOARD_WEMOSD1MINI 6
 #define BOARD_TTGO_TBASE 7
 #define BOARD_ESP01 8
-#define BOARD_SLIMEVR 9
+#define BOARD_SLIMEVR 9  // SlimeVR v1.0 & v1.1
 #define BOARD_LOLIN_C3_MINI 10
 #define BOARD_BEETLE32C3 11
-#define BOARD_ES32C3DEVKITM1 12
+#define BOARD_ESP32C3DEVKITM1 12
 #define BOARD_OWOTRACK 13  // Only used by owoTrack mobile app
 #define BOARD_WRANGLER 14  // Only used by wrangler app
 #define BOARD_MOCOPI 15  // Used by mocopi/moslime
 #define BOARD_WEMOSWROOM02 16
 #define BOARD_XIAO_ESP32C3 17
 #define BOARD_HARITORA 18  // Used by Haritora/SlimeTora
+#define BOARD_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
@@ -140,8 +155,22 @@ enum class ImuID {
 #define MCU_ESP32_C3 6
 #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
 #elif defined(ESP32)
@@ -152,4 +181,6 @@ enum class ImuID {

 #define CURRENT_CONFIGURATION_VERSION 1

+#include "sensors/sensorposition.h"
+
 #endif  // SLIMEVR_CONSTS_H_
--- a/src/debug.h
+++ b/src/debug.h
@@ -39,6 +39,8 @@
 		   // disable if problems. Server does nothing with value so disabled atm
 #define SEND_ACCELERATION true  // send linear acceleration to the server

+#define EXT_SERIAL_COMMANDS false  // Set to true to enable extra serial debug commands
+
 // Debug information

 #define LOG_LEVEL LOG_LEVEL_DEBUG
@@ -94,10 +96,24 @@
 // Not recommended for production
 #define ENABLE_INSPECTION false

-#define PROTOCOL_VERSION 18
+#define PROTOCOL_VERSION 22

 #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_
--- a/src/debugging/TimeTaken.cpp
+++ b/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
--- a/src/debugging/TimeTaken.h
+++ b/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
--- a/src/defines.h
+++ b/src/defines.h
@@ -26,205 +26,53 @@
 // ================================================

 // Set parameters of IMU and board used
-#define IMU IMU_BNO085
-#define SECOND_IMU IMU
-#define BOARD BOARD_SLIMEVR
+#ifndef IMU
+#define IMU IMU_AUTO
+#endif
+#ifndef SECOND_IMU
+#define SECOND_IMU IMU_AUTO
+#endif
+#ifndef BOARD
+#define BOARD BOARD_SLIMEVR_V1_2
+#endif
+#ifndef IMU_ROTATION
 #define IMU_ROTATION DEG_270
+#endif
+#ifndef SECOND_IMU_ROTATION
 #define SECOND_IMU_ROTATION DEG_270
-
-#define PRIMARY_IMU_OPTIONAL false
-#define SECONDARY_IMU_OPTIONAL true
-
-#define MAX_IMU_COUNT 2
-
-// Axis mapping example
-/*
-#include "sensors/axisremap.h"
-#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN,
-AXIS_REMAP_USE_Z, \ AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)
-
-IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL,
-PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
-*/
-
-#ifndef IMU_DESC_LIST
-#define IMU_DESC_LIST              \
-	IMU_DESC_ENTRY(                \
-		IMU,                       \
-		PRIMARY_IMU_ADDRESS_ONE,   \
-		IMU_ROTATION,              \
-		PIN_IMU_SCL,               \
-		PIN_IMU_SDA,               \
-		PRIMARY_IMU_OPTIONAL,      \
-		PIN_IMU_INT                \
-	)                              \
-	IMU_DESC_ENTRY(                \
-		SECOND_IMU,                \
-		SECONDARY_IMU_ADDRESS_TWO, \
-		SECOND_IMU_ROTATION,       \
-		PIN_IMU_SCL,               \
-		PIN_IMU_SDA,               \
-		SECONDARY_IMU_OPTIONAL,    \
-		PIN_IMU_INT_2              \
-	)
 #endif

+#ifndef PRIMARY_IMU_OPTIONAL
+#define PRIMARY_IMU_OPTIONAL false
+#endif
+#ifndef SECONDARY_IMU_OPTIONAL
+#define SECONDARY_IMU_OPTIONAL true
+#endif
+
+// Set I2C address here or directly in IMU_DESC_ENTRY for each IMU used
+// If not set, default address is used based on the IMU and Sensor ID
+// #define PRIMARY_IMU_ADDRESS_ONE 0x4a
+// #define SECONDARY_IMU_ADDRESS_TWO 0x4b
+
+#ifndef BATTERY_MONITOR
 // Battery monitoring options (comment to disable):
 //   BAT_EXTERNAL for ADC pin,
 //   BAT_INTERNAL for internal - can detect only low battery,
 //   BAT_MCP3021 for external ADC connected over I2C
 #define BATTERY_MONITOR BAT_EXTERNAL
+#endif

-// 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
+// --- OVERRIDES FOR DEFAULT PINS

-// 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
+// #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

-// Board-specific configurations
-#if BOARD == BOARD_SLIMEVR
-#define PIN_IMU_SDA 14
-#define PIN_IMU_SCL 12
-#define PIN_IMU_INT 16
-#define PIN_IMU_INT_2 13
-#define PIN_BATTERY_LEVEL 17
-#define LED_PIN 2
-#define LED_INVERTED true
-#ifndef BATTERY_SHIELD_RESISTANCE
-#define BATTERY_SHIELD_RESISTANCE 0
-#endif
-#ifndef BATTERY_SHIELD_R1
-#define BATTERY_SHIELD_R1 10
-#endif
-#ifndef BATTERY_SHIELD_R2
-#define BATTERY_SHIELD_R2 40.2
-#endif
-#elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV
-#define PIN_IMU_SDA 4
-#define PIN_IMU_SCL 5
-#define PIN_IMU_INT 10
-#define PIN_IMU_INT_2 13
-#define PIN_BATTERY_LEVEL 17
-#define LED_PIN 2
-#define LED_INVERTED true
-#ifndef BATTERY_SHIELD_RESISTANCE
-#define BATTERY_SHIELD_RESISTANCE 0
-#endif
-#ifndef BATTERY_SHIELD_R1
-#define BATTERY_SHIELD_R1 10
-#endif
-#ifndef BATTERY_SHIELD_R2
-#define BATTERY_SHIELD_R2 40.2
-#endif
-#elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI
-#define PIN_IMU_SDA D2
-#define PIN_IMU_SCL D1
-#define PIN_IMU_INT D5
-#define PIN_IMU_INT_2 D6
-#define PIN_BATTERY_LEVEL A0
-//  #define LED_PIN 2
-//  #define LED_INVERTED true
-#ifndef BATTERY_SHIELD_RESISTANCE
-#define BATTERY_SHIELD_RESISTANCE 180
-#endif
-#ifndef BATTERY_SHIELD_R1
-#define BATTERY_SHIELD_R1 100
-#endif
-#ifndef BATTERY_SHIELD_R2
-#define BATTERY_SHIELD_R2 220
-#endif
-#elif BOARD == BOARD_ESP01
-#define PIN_IMU_SDA 2
-#define PIN_IMU_SCL 0
-#define PIN_IMU_INT 255
-#define PIN_IMU_INT_2 255
-#define PIN_BATTERY_LEVEL 255
-#define LED_PIN LED_OFF
-#define LED_INVERTED false
-#elif BOARD == BOARD_TTGO_TBASE
-#define PIN_IMU_SDA 5
-#define PIN_IMU_SCL 4
-#define PIN_IMU_INT 14
-#define PIN_IMU_INT_2 13
-#define PIN_BATTERY_LEVEL A0
-//  #define LED_PIN 2
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_CUSTOM
-// Define pins by the examples above
-#elif BOARD == BOARD_WROOM32
-#define PIN_IMU_SDA 21
-#define PIN_IMU_SCL 22
-#define PIN_IMU_INT 23
-#define PIN_IMU_INT_2 25
-#define PIN_BATTERY_LEVEL 36
-//  #define LED_PIN 2
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_LOLIN_C3_MINI
-#define PIN_IMU_SDA 5
-#define PIN_IMU_SCL 4
-#define PIN_IMU_INT 6
-#define PIN_IMU_INT_2 8
-#define PIN_BATTERY_LEVEL 3
-#define LED_PIN 7
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_BEETLE32C3
-#define PIN_IMU_SDA 8
-#define PIN_IMU_SCL 9
-#define PIN_IMU_INT 6
-#define PIN_IMU_INT_2 7
-#define PIN_BATTERY_LEVEL 3
-#define LED_PIN 10
-#define LED_INVERTED false
-#elif BOARD == BOARD_ES32C3DEVKITM1
-#define PIN_IMU_SDA 5
-#define PIN_IMU_SCL 4
-#define PIN_IMU_INT 6
-#define PIN_IMU_INT_2 7
-#define PIN_BATTERY_LEVEL 3
-#define LED_PIN \
-	LED_OFF  // RGB LED Protocol would need to be implementetet did not brother for the
-			 // test, because the board ideal for tracker ifself
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_WEMOSWROOM02
-#define PIN_IMU_SDA 2
-#define PIN_IMU_SCL 14
-#define PIN_IMU_INT 0
-#define PIN_IMU_INT_2 4
-#define PIN_BATTERY_LEVEL A0
-#define LED_PIN 16
-#define LED_INVERTED true
-#elif BOARD == BOARD_XIAO_ESP32C3
-#define PIN_IMU_SDA 6  // D4
-#define PIN_IMU_SCL 7  // D5
-#define PIN_IMU_INT 5  // D3
-#define PIN_IMU_INT_2 10  // D10
-#define LED_PIN 4  // D2
-#define LED_INVERTED false
-#define PIN_BATTERY_LEVEL 2  // D0 / A0
-#ifndef BATTERY_SHIELD_RESISTANCE
-#define BATTERY_SHIELD_RESISTANCE 0
-#endif
-#ifndef BATTERY_SHIELD_R1
-#define BATTERY_SHIELD_R1 100
-#endif
-#ifndef BATTERY_SHIELD_R2
-#define BATTERY_SHIELD_R2 100
-#endif
-#endif
+// ------------------------------
--- a/src/defines_bmi160.h
+++ b/src/defines_bmi160.h
@@ -1,73 +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
--- a/src/defines_sensitivity.h
+++ b/src/defines_sensitivity.h
@@ -1,92 +0,0 @@
-/*
-	SlimeVR Code is placed under the MIT license
-	Copyright (c) 2022 SlimeVR Contributors
-
-	Permission is hereby granted, free of charge, to any person obtaining a copy
-	of this software and associated documentation files (the "Software"), to deal
-	in the Software without restriction, including without limitation the rights
-	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-	copies of the Software, and to permit persons to whom the Software is
-	furnished to do so, subject to the following conditions:
-
-	The above copyright notice and this permission notice shall be included in
-	all copies or substantial portions of the Software.
-
-	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-	THE SOFTWARE.
-*/
-
-#ifndef DEFINES_SENSITIVITY_H
-#define DEFINES_SENSITIVITY_H
-
-// ==========================
-// Sensitivity calibration
-// Only for: BMI160
-// ==========================
-
-// This type of calibration attempts to reduce "extra degrees measures",
-// useful if you do 360 spins
-
-// Trackers are identified by sensor id and MAC address of the device,
-// which is displayed in serial when you upload firmware
-
-// Number of spins determines calibration accuracy
-
-// 1. Put the axis you want to calibrate vertically
-// 2. Put the tracker into an orientation that you can repeat later
-// 3. Do a full reset, rotation must now be 0 0 0
-// 4. Spin the tracker clockwise <.spins> times around the axis
-// 5. Put the tracker back into the reset position
-// 6. Write down the resulting Y (yaw) value into the table below
-// Reflash, do the same rotation and check if the resulting angle is now 0
-
-#define SENSORID_PRIMARY 0
-#define SENSORID_AUX 1
-struct SensitivityOffsetXYZ {
-	const char* mac;
-	unsigned char sensorId;
-	double spins;
-	double x;
-	double y;
-	double z;
-};
-const SensitivityOffsetXYZ sensitivityOffsets[] = {
-	// example values
-	{.mac = "A4:E5:7C:B6:00:01",
-	 .sensorId = SENSORID_PRIMARY,
-	 .spins = 10,
-	 .x = 2.63,
-	 .y = 37.82,
-	 .z = 31.11},
-	{.mac = "A4:E5:7C:B6:00:02",
-	 .sensorId = SENSORID_PRIMARY,
-	 .spins = 10,
-	 .x = -2.38,
-	 .y = -26.8,
-	 .z = -42.78},
-	{.mac = "A4:E5:7C:B6:00:03",
-	 .sensorId = SENSORID_PRIMARY,
-	 .spins = 10,
-	 .x = 11,
-	 .y = 2.2,
-	 .z = -1},
-	{.mac = "A4:E5:7C:B6:00:04",
-	 .sensorId = SENSORID_PRIMARY,
-	 .spins = 10,
-	 .x = -7,
-	 .y = -53.7,
-	 .z = -57},
-	{.mac = "A4:E5:7C:B6:00:05",
-	 .sensorId = SENSORID_PRIMARY,
-	 .spins = 10,
-	 .x = -10.63,
-	 .y = -8.25,
-	 .z = -18.6},
-};
-
-#endif
--- a/src/globals.h
+++ b/src/globals.h
@@ -20,16 +20,17 @@
 	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
@@ -43,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
--- a/src/logging/Level.cpp
+++ b/src/logging/Level.cpp
@@ -1,7 +1,6 @@
 #include "Level.h"

-namespace SlimeVR {
-namespace Logging {
+namespace SlimeVR::Logging {
 const char* levelToString(Level level) {
 	switch (level) {
 		case TRACE:
@@ -20,5 +19,4 @@ const char* levelToString(Level level) {
 			return "UNKNOWN";
 	}
 }
-}  // namespace Logging
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Logging
--- a/src/logging/Level.h
+++ b/src/logging/Level.h
@@ -7,8 +7,7 @@
 #define LOG_LEVEL_ERROR 4
 #define LOG_LEVEL_FATAL 5

-namespace SlimeVR {
-namespace Logging {
+namespace SlimeVR::Logging {
 enum Level {
 	TRACE = LOG_LEVEL_TRACE,
 	DEBUG = LOG_LEVEL_DEBUG,
@@ -19,8 +18,7 @@ enum Level {
 };

 const char* levelToString(Level level);
-}  // namespace Logging
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Logging

 #define LOGGING_LEVEL_H
 #endif
--- a/src/logging/Logger.cpp
+++ b/src/logging/Logger.cpp
@@ -1,55 +1,54 @@
 #include "Logger.h"

-namespace SlimeVR {
-namespace Logging {
+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, ...) {
+void Logger::trace(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(TRACE, format, args);
 	va_end(args);
 }

-void Logger::debug(const char* format, ...) {
+void Logger::debug(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(DEBUG, format, args);
 	va_end(args);
 }

-void Logger::info(const char* format, ...) {
+void Logger::info(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(INFO, format, args);
 	va_end(args);
 }

-void Logger::warn(const char* format, ...) {
+void Logger::warn(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(WARN, format, args);
 	va_end(args);
 }

-void Logger::error(const char* format, ...) {
+void Logger::error(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(ERROR, format, args);
 	va_end(args);
 }

-void Logger::fatal(const char* format, ...) {
+void Logger::fatal(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(FATAL, format, args);
 	va_end(args);
 }

-void Logger::log(Level level, const char* format, va_list args) {
+void Logger::log(Level level, const char* format, va_list args) const {
 	if (level < LOG_LEVEL) {
 		return;
 	}
@@ -66,5 +65,4 @@ void Logger::log(Level level, const char* format, va_list args) {

 	Serial.printf("[%-5s] [%s] %s\n", levelToString(level), buf, buffer);
 }
-}  // namespace Logging
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Logging
--- a/src/logging/Logger.h
+++ b/src/logging/Logger.h
@@ -6,8 +6,7 @@
 #include "Level.h"
 #include "debug.h"

-namespace SlimeVR {
-namespace Logging {
+namespace SlimeVR::Logging {
 class Logger {
 public:
 	Logger(const char* prefix)
@@ -27,48 +26,48 @@ public:

 	void setTag(const char* tag);

-	void trace(const char* str, ...);
-	void debug(const char* str, ...);
-	void info(const char* str, ...);
-	void warn(const char* str, ...);
-	void error(const char* str, ...);
-	void fatal(const char* str, ...);
+	void trace(const char* str, ...) const __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 traceArray(const char* str, const T* array, size_t size) {
+	inline void traceArray(const char* str, const T* array, size_t size) const {
 		logArray(TRACE, str, array, size);
 	}

 	template <typename T>
-	inline void debugArray(const char* str, const T* array, size_t size) {
+	inline void debugArray(const char* str, const T* array, size_t size) const {
 		logArray(DEBUG, str, array, size);
 	}

 	template <typename T>
-	inline void infoArray(const char* str, const T* array, size_t size) {
+	inline void infoArray(const char* str, const T* array, size_t size) const {
 		logArray(INFO, str, array, size);
 	}

 	template <typename T>
-	inline void warnArray(const char* str, const T* array, size_t size) {
+	inline void warnArray(const char* str, const T* array, size_t size) const {
 		logArray(WARN, str, array, size);
 	}

 	template <typename T>
-	inline void errorArray(const char* str, const T* array, size_t size) {
+	inline void errorArray(const char* str, const T* array, size_t size) const {
 		logArray(ERROR, str, array, size);
 	}

 	template <typename T>
-	inline void fatalArray(const char* str, const T* array, size_t size) {
+	inline void fatalArray(const char* str, const T* array, size_t size) const {
 		logArray(FATAL, str, array, size);
 	}

 private:
-	void log(Level level, const char* str, va_list args);
+	void log(Level level, const char* str, va_list args) const;

 	template <typename T>
-	void logArray(Level level, const char* str, const T* array, size_t size) {
+	void logArray(Level level, const char* str, const T* array, size_t size) const {
 		if (level < LOG_LEVEL) {
 			return;
 		}
@@ -92,7 +91,6 @@ private:
 	const char* const m_Prefix;
 	char* m_Tag;
 };
-}  // namespace Logging
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Logging

 #endif
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -27,19 +27,27 @@
 #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;
@@ -48,40 +56,68 @@ unsigned long loopTime = 0;
 unsigned long lastStatePrint = 0;
 bool secondImuActive = false;
 BatteryMonitor battery;
+TPSCounter tpsCounter;

 void setup() {
 	Serial.begin(serialBaudRate);
 	globalTimer = timer_create_default();

-#ifdef ESP32C3
-	// Wait for the Computer to be able to connect.
-	delay(2000);
-#endif
-
 	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();
-
-	I2CSCAN::clearBus(
-		PIN_IMU_SDA,
-		PIN_IMU_SCL
-	);  // Make sure the bus isn't stuck when resetting ESP without powering it down
+	// 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

-#if ESP32
+#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();
@@ -113,16 +149,27 @@ void setup() {
 	sensorManager.postSetup();

 	loopTime = micros();
+	tpsCounter.reset();
 }

 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) {
--- a/src/motionprocessing/GyroTemperatureCalibrator.cpp
+++ b/src/motionprocessing/GyroTemperatureCalibrator.cpp
@@ -227,7 +227,7 @@ bool GyroTemperatureCalibrator::loadConfig(float newSensitivity) {
 bool GyroTemperatureCalibrator::saveConfig() {
 	if (configuration.saveTemperatureCalibration(sensorId, config)) {
 		m_Logger.info(
-			"Saved temperature calibration config (%0.1f%) for sensorId:%i",
+			"Saved temperature calibration config (%0.1f%%) for sensorId:%i",
 			config.getCalibrationDonePercent(),
 			sensorId
 		);
--- a/src/network/connection.cpp
+++ b/src/network/connection.cpp
@@ -23,6 +23,8 @@

 #include "connection.h"

+#include <string_view>
+
 #include "GlobalVars.h"
 #include "logging/Logger.h"
 #include "packets.h"
@@ -30,40 +32,14 @@
 #define TIMEOUT 3000UL

 template <typename T>
-unsigned char* convert_to_chars(T src, unsigned char* target) {
-	union uwunion {
-		unsigned char c[sizeof(T)];
-		T v;
-	} un;
-	un.v = src;
-	for (size_t i = 0; i < sizeof(T); i++) {
-		target[i] = un.c[sizeof(T) - i - 1];
-	}
+uint8_t* convert_to_chars(T src, uint8_t* target) {
+	auto* rawBytes = reinterpret_cast<uint8_t*>(&src);
+	std::memcpy(target, rawBytes, sizeof(T));
+	std::reverse(target, target + sizeof(T));
 	return target;
 }

-template <typename T>
-T convert_chars(unsigned char* const src) {
-	union uwunion {
-		unsigned char c[sizeof(T)];
-		T v;
-	} un;
-	for (size_t i = 0; i < sizeof(T); i++) {
-		un.c[i] = src[sizeof(T) - i - 1];
-	}
-	return un.v;
-}
-
-namespace SlimeVR {
-namespace Network {
-
-#define MUST_TRANSFER_BOOL(b) \
-	if (!b)                   \
-		return false;
-
-#define MUST(b) \
-	if (!b)     \
-		return;
+namespace SlimeVR::Network {

 bool Connection::beginPacket() {
 	if (m_IsBundle) {
@@ -91,7 +67,7 @@ bool Connection::endPacket() {
 		m_IsBundle = false;

 		if (m_BundlePacketInnerCount == 0) {
-			sendPacketType(PACKET_BUNDLE);
+			sendPacketType(SendPacketType::Bundle);
 			sendPacketNumber();
 		}
 		sendShort(innerPacketSize);
@@ -189,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) {
@@ -218,87 +198,77 @@ int Connection::getWriteError() { return m_UDP.getWriteError(); }
 // PACKET_HEARTBEAT 0
 void Connection::sendHeartbeat() {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_HEARTBEAT));
-	MUST(sendPacketNumber());
-
-	MUST(endPacket());
+	MUST(sendPacketCallback(SendPacketType::HeartBeat, []() { return true; }));
 }

 // PACKET_ACCEL 4
 void Connection::sendSensorAcceleration(uint8_t sensorId, Vector3 vector) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ACCEL));
-	MUST(sendPacketNumber());
-	MUST(sendFloat(vector.x));
-	MUST(sendFloat(vector.y));
-	MUST(sendFloat(vector.z));
-	MUST(sendByte(sensorId));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::Accel,
+		AccelPacket{
+			.x = vector.x,
+			.y = vector.y,
+			.z = vector.z,
+			.sensorId = sensorId,
+		}
+	));
 }

 // PACKET_BATTERY_LEVEL 12
 void Connection::sendBatteryLevel(float batteryVoltage, float batteryPercentage) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_BATTERY_LEVEL));
-	MUST(sendPacketNumber());
-	MUST(sendFloat(batteryVoltage));
-	MUST(sendFloat(batteryPercentage));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::BatteryLevel,
+		BatteryLevelPacket{
+			.batteryVoltage = batteryVoltage,
+			.batteryPercentage = batteryPercentage,
+		}
+	));
 }

 // PACKET_TAP 13
 void Connection::sendSensorTap(uint8_t sensorId, uint8_t value) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_TAP));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendByte(value));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::Tap,
+		TapPacket{
+			.sensorId = sensorId,
+			.value = value,
+		}
+	));
 }

 // PACKET_ERROR 14
 void Connection::sendSensorError(uint8_t sensorId, uint8_t error) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ERROR));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendByte(error));
-
-	MUST(endPacket());
+	sendPacket(
+		SendPacketType::Error,
+		ErrorPacket{
+			.sensorId = sensorId,
+			.error = error,
+		}
+	);
 }

 // PACKET_SENSOR_INFO 15
 void Connection::sendSensorInfo(Sensor& sensor) {
 	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::SensorInfo,
+		SensorInfoPacket{
+			.sensorId = sensor.getSensorId(),
+			.sensorState = sensor.getSensorState(),
+			.sensorType = sensor.getSensorType(),
+			.sensorConfigData = sensor.getSensorConfigData(),
+			.hasCompletedRestCalibration = sensor.hasCompletedRestCalibration(),
+			.sensorPosition = sensor.getSensorPosition(),
+			.sensorDataType = sensor.getDataType(),

-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_SENSOR_INFO));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensor.getSensorId()));
-	MUST(sendByte(static_cast<uint8_t>(sensor.getSensorState())));
-	MUST(sendByte(static_cast<uint8_t>(sensor.getSensorType())));
-	MUST(sendShort(sensor.getSensorConfigData()));
-
-	MUST(endPacket());
+			.tpsCounterAveragedTps = sensor.m_tpsCounter.getAveragedTPS(),
+			.dataCounterAveragedTps = sensor.m_dataCounter.getAveragedTPS(),
+		}
+	));
 }

 // PACKET_ROTATION_DATA 17
@@ -309,118 +279,132 @@ void Connection::sendRotationData(
 	uint8_t accuracyInfo
 ) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ROTATION_DATA));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendByte(dataType));
-	MUST(sendFloat(quaternion->x));
-	MUST(sendFloat(quaternion->y));
-	MUST(sendFloat(quaternion->z));
-	MUST(sendFloat(quaternion->w));
-	MUST(sendByte(accuracyInfo));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::RotationData,
+		RotationDataPacket{
+			.sensorId = sensorId,
+			.dataType = dataType,
+			.x = quaternion->x,
+			.y = quaternion->y,
+			.z = quaternion->z,
+			.w = quaternion->w,
+			.accuracyInfo = accuracyInfo,
+		}
+	));
 }

 // PACKET_MAGNETOMETER_ACCURACY 18
 void Connection::sendMagnetometerAccuracy(uint8_t sensorId, float accuracyInfo) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_MAGNETOMETER_ACCURACY));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendFloat(accuracyInfo));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::MagnetometerAccuracy,
+		MagnetometerAccuracyPacket{
+			.sensorId = sensorId,
+			.accuracyInfo = accuracyInfo,
+		}
+	));
 }

 // PACKET_SIGNAL_STRENGTH 19
 void Connection::sendSignalStrength(uint8_t signalStrength) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_SIGNAL_STRENGTH));
-	MUST(sendPacketNumber());
-	MUST(sendByte(255));
-	MUST(sendByte(signalStrength));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::SignalStrength,
+		SignalStrengthPacket{
+			.sensorId = 255,
+			.signalStrength = signalStrength,
+		}
+	));
 }

 // PACKET_TEMPERATURE 20
 void Connection::sendTemperature(uint8_t sensorId, float temperature) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_TEMPERATURE));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendFloat(temperature));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::Temperature,
+		TemperaturePacket{
+			.sensorId = sensorId,
+			.temperature = temperature,
+		}
+	));
 }

 // PACKET_FEATURE_FLAGS 22
 void Connection::sendFeatureFlags() {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_FEATURE_FLAGS));
-	MUST(sendPacketNumber());
-	MUST(write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size()));
-
-	MUST(endPacket());
+	sendPacketCallback(SendPacketType::FeatureFlags, [&]() {
+		return write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size());
+	});
 }

 // PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24

-void Connection::sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType) {
+void Connection::sendAcknowledgeConfigChange(
+	uint8_t sensorId,
+	SensorToggles configType
+) {
 	MUST(m_Connected);
-
-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_ACKNOWLEDGE_CONFIG_CHANGE));
-	MUST(sendPacketNumber());
-	MUST(sendByte(sensorId));
-	MUST(sendShort(configType));
-
-	MUST(endPacket());
+	MUST(sendPacket(
+		SendPacketType::AcknowledgeConfigChange,
+		AcknowledgeConfigChangePacket{
+			.sensorId = sensorId,
+			.configType = configType,
+		}
+	));
 }

 void Connection::sendTrackerDiscovery() {
 	MUST(!m_Connected);
+	MUST(sendPacketCallback(
+		SendPacketType::Handshake,
+		[&]() {
+			uint8_t mac[6];
+			WiFi.macAddress(mac);

-	uint8_t mac[6];
-	WiFi.macAddress(mac);
+			MUST_TRANSFER_BOOL(sendInt(BOARD));
+			// This is kept for backwards compatibility,
+			// but the latest SlimeVR server will not initialize trackers
+			// with firmware build > 8 until it recieves a sensor info packet
+			MUST_TRANSFER_BOOL(sendInt(static_cast<int>(sensorManager.getSensorType(0)))
+			);
+			MUST_TRANSFER_BOOL(sendInt(HARDWARE_MCU));
+			// Backwards compatibility, unused IMU data
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(PROTOCOL_VERSION));
+			MUST_TRANSFER_BOOL(sendShortString(FIRMWARE_VERSION));
+			// MAC address string
+			MUST_TRANSFER_BOOL(sendBytes(mac, 6));
+			// Tracker type to hint the server if it's a glove or normal tracker or
+			// something else
+			MUST_TRANSFER_BOOL(sendByte(static_cast<uint8_t>(TRACKER_TYPE)));
+			static_assert(std::string_view{VENDOR_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(VENDOR_NAME));
+			static_assert(std::string_view{VENDOR_URL}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(VENDOR_URL));
+			static_assert(std::string_view{PRODUCT_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(PRODUCT_NAME));
+			static_assert(std::string_view{UPDATE_ADDRESS}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(UPDATE_ADDRESS));
+			static_assert(std::string_view{UPDATE_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(UPDATE_NAME));
+			return true;
+		},
+		0
+	));
+}

-	MUST(beginPacket());
-
-	MUST(sendPacketType(PACKET_HANDSHAKE));
-	// Packet number is always 0 for handshake
-	MUST(sendLong(0));
-	MUST(sendInt(BOARD));
-	// This is kept for backwards compatibility,
-	// but the latest SlimeVR server will not initialize trackers
-	// with firmware build > 8 until it recieves a sensor info packet
-	MUST(sendInt(static_cast<int>(sensorManager.getSensorType(0))));
-	MUST(sendInt(HARDWARE_MCU));
-	MUST(sendInt(0));
-	MUST(sendInt(0));
-	MUST(sendInt(0));
-	MUST(sendInt(PROTOCOL_VERSION));
-	MUST(sendShortString(FIRMWARE_VERSION));
-	// MAC address string
-	MUST(sendBytes(mac, 6));
-
-	MUST(endPacket());
+// PACKET_FLEX_DATA 24
+void Connection::sendFlexData(uint8_t sensorId, float flexLevel) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::FlexData,
+		FlexDataPacket{
+			.sensorId = sensorId,
+			.flexLevel = flexLevel,
+		}
+	));
 }

 #if ENABLE_INSPECTION
@@ -440,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(
@@ -485,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

@@ -533,7 +509,7 @@ void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors
 	m_LastSensorInfoPacketTimestamp = millis();

 	for (int i = 0; i < (int)sensors.size(); i++) {
-		if (m_AckedSensorState[i] != sensors[i]->getSensorState()) {
+		if (isSensorStateUpdated(i, sensors[i])) {
 			sendSensorInfo(*sensors[i]);
 		}
 	}
@@ -553,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();
@@ -571,13 +555,12 @@ void Connection::searchForServer() {
 			m_UDP.remotePort()
 		);
 		m_Logger.traceArray("UDP packet contents: ", m_Packet, len);
-#else
-		(void)len;
 #endif

 		// Handshake is different, it has 3 in the first byte, not the 4th, and data
 		// starts right after
-		if (m_Packet[0] == PACKET_HANDSHAKE) {
+		if (static_cast<ReceivePacketType>(m_Packet[0])
+			== ReceivePacketType::Handshake) {
 			if (strncmp((char*)m_Packet + 1, "Hey OVR =D 5", 12) != 0) {
 				m_Logger.error("Received invalid handshake packet");
 				continue;
@@ -621,37 +604,58 @@ void Connection::reset() {
 	m_Connected = false;
 	std::fill(
 		m_AckedSensorState,
-		m_AckedSensorState + MAX_IMU_COUNT,
+		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() {
-	auto& sensors = sensorManager.getSensors();
-
-	updateSensorState(sensors);
-	maybeRequestFeatureFlags();
-
 	if (!m_Connected) {
 		searchForServer();
 		return;
 	}

+	auto& sensors = sensorManager.getSensors();
+
+	updateSensorState(sensors);
+	maybeRequestFeatureFlags();
+
 	if (m_LastPacketTimestamp + TIMEOUT < millis()) {
 		statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);

 		m_Connected = false;
 		std::fill(
 			m_AckedSensorState,
-			m_AckedSensorState + MAX_IMU_COUNT,
+			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;
 	}

@@ -660,7 +664,6 @@ void Connection::update() {
 		return;
 	}

-	m_LastPacketTimestamp = millis();
 	int len = m_UDP.read(m_Packet, sizeof(m_Packet));

 #ifdef DEBUG_NETWORK
@@ -675,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");
@@ -736,37 +755,41 @@ void Connection::update() {
 			break;
 		}

-		case PACKET_SET_CONFIG_FLAG: {
+		case ReceivePacketType::SetConfigFlag: {
 			// Packet type (4) + Packet number (8) + sensor_id(1) + flag_id (2) + state
 			// (1)
 			if (len < 16) {
 				m_Logger.warn("Invalid sensor config flag packet: too short");
 				break;
 			}
-			uint8_t sensorId = m_Packet[12];
-			uint16_t flagId = m_Packet[13] << 8 | m_Packet[14];
-			bool newState = m_Packet[15] > 0;
+
+			SetConfigFlagPacket setConfigFlagPacket;
+			memcpy(&setConfigFlagPacket, m_Packet + 12, sizeof(SetConfigFlagPacket));
+
+			uint8_t sensorId = setConfigFlagPacket.sensorId;
+			SensorToggles flag = setConfigFlagPacket.flag;
+			bool newState = setConfigFlagPacket.newState;
 			if (sensorId == UINT8_MAX) {
 				for (auto& sensor : sensors) {
-					sensor->setFlag(flagId, newState);
+					sensor->setFlag(flag, newState);
 				}
 			} else {
 				auto& sensors = sensorManager.getSensors();
-				if (sensorId < sensors.size()) {
-					auto& sensor = sensors[sensorId];
-					sensor->setFlag(flagId, newState);
-				} else {
+
+				if (sensorId >= sensors.size()) {
 					m_Logger.warn("Invalid sensor config flag packet: invalid sensor id"
 					);
 					break;
 				}
+
+				auto& sensor = sensors[sensorId];
+				sensor->setFlag(flag, newState);
 			}
-			sendAcknowledgeConfigChange(sensorId, flagId);
+			sendAcknowledgeConfigChange(sensorId, flag);
 			configuration.save();
 			break;
 		}
 	}
 }

-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network
--- a/src/network/connection.h
+++ b/src/network/connection.h
@@ -26,14 +26,25 @@
 #include <Arduino.h>
 #include <WiFiUdp.h>

+#include <optional>
+
+#include "../configuration/SensorConfig.h"
 #include "featureflags.h"
 #include "globals.h"
+#include "packets.h"
 #include "quat.h"
 #include "sensors/sensor.h"
 #include "wifihandler.h"

-namespace SlimeVR {
-namespace Network {
+namespace SlimeVR::Network {
+
+#define MUST_TRANSFER_BOOL(b) \
+	if (!(b))                 \
+		return false;
+
+#define MUST(b) \
+	if (!(b))   \
+		return;

 class Connection {
 public:
@@ -84,6 +95,9 @@ public:
 	// PACKET_FEATURE_FLAGS 22
 	void sendFeatureFlags();

+	// PACKET_FLEX_DATA 26
+	void sendFlexData(uint8_t sensorId, float flexLevel);
+
 #if ENABLE_INSPECTION
 	void sendInspectionRawIMUData(
 		uint8_t sensorId,
@@ -123,8 +137,9 @@ public:
 	bool endBundle();

 private:
-	void updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors);
+	void updateSensorState(std::vector<std::unique_ptr<::Sensor>>& sensors);
 	void maybeRequestFeatureFlags();
+	bool isSensorStateUpdated(int i, std::unique_ptr<::Sensor>& sensor);

 	bool beginPacket();
 	bool endPacket();
@@ -132,7 +147,7 @@ private:
 	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);
@@ -143,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);
@@ -154,9 +209,9 @@ private:
 	void sendTrackerDiscovery();

 	// PACKET_SENSOR_INFO 15
-	void sendSensorInfo(Sensor& sensor);
+	void sendSensorInfo(::Sensor& sensor);

-	void sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType);
+	void sendAcknowledgeConfigChange(uint8_t sensorId, SensorToggles configType);

 	bool m_Connected = false;
 	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection");
@@ -170,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;
@@ -184,7 +242,6 @@ private:
 	unsigned char m_Buf[8];
 };

-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network

 #endif  // SLIMEVR_NETWORK_CONNECTION_H_
--- a/src/network/manager.cpp
+++ b/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
--- a/src/network/manager.h
+++ b/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_
--- a/src/network/packets.h
+++ b/src/network/packets.h
@@ -24,47 +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
-// #define PACKET_ROTATION_ACCELERATION 23 // Unification of rot and accel data in one
-// packet
-#define PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24
-#define PACKET_SET_CONFIG_FLAG 25
+#include <cstdint>

-#define PACKET_BUNDLE 100
+#include "../consts.h"
+#include "../sensors/sensor.h"

-#define PACKET_INSPECTION 105  // 0x69
+enum class SendPacketType : uint8_t {
+	HeartBeat = 0,
+	//  Rotation = 1,
+	//  Gyro = 2,
+	Handshake = 3,
+	Accel = 4,
+	// Mag = 5,
+	// RawCalibrationData = 6,
+	// CalibrationFinished = 7,
+	// RawMagnetometer = 9,
+	Serial = 11,
+	BatteryLevel = 12,
+	Tap = 13,
+	Error = 14,
+	SensorInfo = 15,
+	// Rotation2 = 16,
+	RotationData = 17,
+	MagnetometerAccuracy = 18,
+	SignalStrength = 19,
+	Temperature = 20,
+	// UserAction = 21,
+	FeatureFlags = 22,
+	// RotationAcceleration = 23,
+	AcknowledgeConfigChange = 24,
+	FlexData = 26,
+	// 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_
--- a/src/network/wifihandler.cpp
+++ b/src/network/wifihandler.cpp
@@ -20,31 +20,25 @@
 	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 	THE SOFTWARE.
 */
+#include "network/wifihandler.h"
+
 #include "GlobalVars.h"
 #include "globals.h"
-#include "logging/Logger.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() {
+void WiFiNetwork::reportWifiProgress() {
 	if (lastWifiReportTime + 1000 < millis()) {
 		lastWifiReportTime = millis();
 		Serial.print(".");
 	}
 }

-void setStaticIPIfDefined() {
+void WiFiNetwork::setStaticIPIfDefined() {
 #ifdef WIFI_USE_STATICIP
 	const IPAddress ip(WIFI_STATIC_IP);
 	const IPAddress gateway(WIFI_STATIC_GATEWAY);
@@ -53,16 +47,17 @@ void setStaticIPIfDefined() {
 #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);
+	wifiProvisioning.stopProvisioning();
+	tryConnecting(false, SSID, pass);
+	retriedOnG = false;
 	// Reset state, will get back into provisioning if can't connect
 	hadWifi = false;
-	wifiState = SLIME_WIFI_SERVER_CRED_ATTEMPT;
-	wifiConnectionTimeout = millis();
+	wifiState = WiFiReconnectionStatus::ServerCredAttempt;
 }

 IPAddress WiFiNetwork::getAddress() { return WiFi.localIP(); }
@@ -71,25 +66,14 @@ void WiFiNetwork::setUp() {
 	wifiHandlerLogger.info("Setting up WiFi");
 	WiFi.persistent(true);
 	WiFi.mode(WIFI_STA);
-#if ESP8266
-#if USE_ATTENUATION
-	WiFi.setOutputPower(20.0 - ATTENUATION_N);
-#endif
-	WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-#endif
 	WiFi.hostname("SlimeVR FBT Tracker");
 	wifiHandlerLogger.info(
-		"Loaded credentials for SSID %s and pass length %d",
-		WiFi.SSID().c_str(),
-		WiFi.psk().length()
+		"Loaded credentials for SSID '%s' and pass length %d",
+		getSSID().c_str(),
+		getPassword().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();
+
+	trySavedCredentials();

 #if ESP8266
 #if POWERSAVING_MODE == POWER_SAVING_NONE
@@ -121,156 +105,283 @@ void WiFiNetwork::setUp() {
 #endif
 }

-void onConnected() {
-	WiFiNetwork::stopProvisioning();
+void WiFiNetwork::onConnected() {
+	wifiState = WiFiReconnectionStatus::Success;
+	wifiProvisioning.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(),
+		"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;
+	wifiProvisioning.upkeepProvisioning();
+
+	if (WiFi.status() == WL_CONNECTED) {
+		if (!isConnected()) {
+			onConnected();
+			return;
 		}
-		statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true);
-		reportWifiError();
-		if (wifiConnectionTimeout + 11000 < millis()) {
-			switch (wifiState) {
-				case SLIME_WIFI_NOT_SETUP:  // Wasn't set up
-					return;
-				case SLIME_WIFI_SAVED_ATTEMPT:  // Couldn't connect with first set of
-												// credentials
-#if ESP8266
-					// Try again but with 11G but only if there are credentials,
-					// otherwise we just waste time before switching to hardcoded
-					// credentials.
-					if (WiFi.SSID().length() > 0) {
-#if USE_ATTENUATION
-						WiFi.setOutputPower(20.0 - ATTENUATION_G);
-#endif
-						WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-						setStaticIPIfDefined();
-						WiFi.begin();
-						wifiConnectionTimeout = millis();
-						wifiHandlerLogger.error(
-							"Can't connect from saved credentials, status: %d.",
-							WiFi.status()
-						);
-						wifiHandlerLogger.debug(
-							"Trying saved credentials with PHY Mode G..."
-						);
-					} else {
-						wifiHandlerLogger.debug(
-							"Skipping PHY Mode G attempt on 0-length SSID..."
-						);
-					}
-#endif
-					wifiState = SLIME_WIFI_SAVED_G_ATTEMPT;
-					return;
-				case SLIME_WIFI_SAVED_G_ATTEMPT:  // Couldn't connect with first set of
-												  // credentials with PHY Mode G
-#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
-												  // Try hardcoded credentials now
-#if ESP8266
-#if USE_ATTENUATION
-					WiFi.setOutputPower(20.0 - ATTENUATION_N);
-#endif
-					WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-#endif
-					setStaticIPIfDefined();
-					WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
-					wifiConnectionTimeout = millis();
-					wifiHandlerLogger.error(
-						"Can't connect from saved credentials, status: %d.",
-						WiFi.status()
-					);
-					wifiHandlerLogger.debug("Trying hardcoded credentials...");
-#endif
-					wifiState = SLIME_WIFI_HARDCODE_ATTEMPT;
-					return;
-				case SLIME_WIFI_HARDCODE_ATTEMPT:  // Couldn't connect with second set
-												   // of credentials
-#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266
-												   // Try hardcoded credentials again,
-												   // but with PHY Mode G
-#if USE_ATTENUATION
-					WiFi.setOutputPower(20.0 - ATTENUATION_G);
-#endif
-					WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-					setStaticIPIfDefined();
-					WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
-					wifiConnectionTimeout = millis();
-					wifiHandlerLogger.error(
-						"Can't connect from saved credentials, status: %d.",
-						WiFi.status()
-					);
-					wifiHandlerLogger.debug(
-						"Trying hardcoded credentials with WiFi PHY Mode G..."
-					);
-#endif
-					wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT;
-					return;
-				case SLIME_WIFI_SERVER_CRED_ATTEMPT:  // Couldn't connect with
-													  // server-sent credentials.
-#if ESP8266
-													  // Try again silently but with 11G
-#if USE_ATTENUATION
-					WiFi.setOutputPower(20.0 - ATTENUATION_G);
-#endif
-					WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-					setStaticIPIfDefined();
-					WiFi.begin();
-					wifiConnectionTimeout = millis();
-					wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT;
-#endif
-					return;
-				case SLIME_WIFI_HARDCODE_G_ATTEMPT:  // Couldn't connect with second set
-													 // of credentials with PHY Mode G.
-				case SLIME_WIFI_SERVER_CRED_G_ATTEMPT:  // Or if couldn't connect with
-														// server-sent credentials
-// Return to the default PHY Mode N.
-#if ESP8266
-#if USE_ATTENUATION
-					WiFi.setOutputPower(20.0 - ATTENUATION_N);
-#endif
-					WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-#endif
-					// Start smart config
-					if (!hadWifi && !WiFi.smartConfigDone()
-						&& wifiConnectionTimeout + 11000 < millis()) {
-						if (WiFi.status() != WL_IDLE_STATUS) {
-							wifiHandlerLogger.error(
-								"Can't connect from any credentials, status: %d.",
-								WiFi.status()
-							);
-							wifiConnectionTimeout = millis();
-						}
-						startProvisioning();
-					}
-					return;
-			}
-		}
-		return;
-	}
-	if (!isWifiConnected) {
-		onConnected();
-		return;
-	} else {
-		if (millis() - last_rssi_sample >= 2000) {
-			last_rssi_sample = millis();
+
+		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;
 	}
-	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
--- a/src/network/wifihandler.h
+++ b/src/network/wifihandler.h
@@ -20,33 +20,78 @@
 	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>
 #else
 #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 WiFiNetwork
+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
--- a/src/network/wifiprovisioning.cpp
+++ b/src/network/wifiprovisioning.cpp
@@ -29,29 +29,31 @@
 // 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() {
+void WifiProvisioning::upkeepProvisioning() {
 	// Called even when not provisioning to do things like provide neighbours or other
 	// upkeep
 }

-void WiFiNetwork::startProvisioning() {
+void WifiProvisioning::startProvisioning() {
 	if (WiFi.beginSmartConfig()) {
 		provisioning = true;
 		wifiProvisioningLogger.info("SmartConfig started");
 	}
 }

-void WiFiNetwork::stopProvisioning() {
+void WifiProvisioning::stopProvisioning() {
 	WiFi.stopSmartConfig();
 	provisioning = false;
 }

-void WiFiNetwork::provideNeighbours() {
+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
--- a/src/network/wifiprovisioning.h
+++ b/src/network/wifiprovisioning.h
@@ -23,12 +23,23 @@
 #ifndef SLIMEVR_WIFIPROVISIONING_H_
 #define SLIMEVR_WIFIPROVISIONING_H_

-namespace WiFiNetwork {
-void upkeepProvisioning();
-void startProvisioning();
-void stopProvisioning();
-bool isProvisioning();
-void provideNeighbours();
-}  // namespace WiFiNetwork
+#include "logging/Logger.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_
--- a/src/sensorinterface/DirectPinInterface.cpp
+++ b/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); }
--- a/src/sensorinterface/DirectPinInterface.h
+++ b/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_
--- a/src/sensorinterface/DirectSPIInterface.cpp
+++ b/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
--- a/src/sensorinterface/DirectSPIInterface.h
+++ b/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
--- a/src/sensorinterface/I2CPCAInterface.cpp
+++ b/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
+}
--- a/src/sensorinterface/I2CPCAInterface.h
+++ b/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
--- a/src/sensorinterface/I2CWireSensorInterface.cpp
+++ b/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
--- a/src/sensorinterface/I2CWireSensorInterface.h
+++ b/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
--- a/src/sensorinterface/MCP23X17PinInterface.cpp
+++ b/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);
+}
--- a/src/sensorinterface/MCP23X17PinInterface.h
+++ b/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_
--- a/src/sensorinterface/RegisterInterface.cpp
+++ b/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
--- a/src/sensorinterface/RegisterInterface.h
+++ b/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
--- a/src/sensorinterface/SPIImpl.h
+++ b/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
--- a/src/sensorinterface/SensorInterface.cpp
+++ b/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;
+
+}
--- a/src/sensorinterface/SensorInterface.h
+++ b/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
--- a/src/sensorinterface/SensorInterfaceManager.cpp
+++ b/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);
+}
--- a/src/sensorinterface/SensorInterfaceManager.h
+++ b/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
--- a/src/sensors/softfusion/i2cimpl.h
+++ b/src/sensors/softfusion/i2cimpl.h
@@ -23,25 +23,26 @@

 #pragma once

+#include <i2cscan.h>
+
 #include <cstdint>

 #include "I2Cdev.h"
+#include "RegisterInterface.h"

-namespace SlimeVR::Sensors::SoftFusion {
-
-struct I2CImpl {
-	static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2;
+namespace SlimeVR::Sensors {

+struct I2CImpl : public RegisterInterface {
 	I2CImpl(uint8_t devAddr)
 		: m_devAddr(devAddr) {}

-	uint8_t readReg(uint8_t regAddr) const {
+	uint8_t readReg(uint8_t regAddr) const override {
 		uint8_t buffer = 0;
 		I2Cdev::readByte(m_devAddr, regAddr, &buffer);
 		return buffer;
 	}

-	uint16_t readReg16(uint8_t regAddr) const {
+	uint16_t readReg16(uint8_t regAddr) const override {
 		uint16_t buffer = 0;
 		I2Cdev::readBytes(
 			m_devAddr,
@@ -52,11 +53,11 @@ struct I2CImpl {
 		return buffer;
 	}

-	void writeReg(uint8_t regAddr, uint8_t value) const {
+	void writeReg(uint8_t regAddr, uint8_t value) const override {
 		I2Cdev::writeByte(m_devAddr, regAddr, value);
 	}

-	void writeReg16(uint8_t regAddr, uint16_t value) const {
+	void writeReg16(uint8_t regAddr, uint16_t value) const override {
 		I2Cdev::writeBytes(
 			m_devAddr,
 			regAddr,
@@ -65,16 +66,29 @@ struct I2CImpl {
 		);
 	}

-	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const {
+	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
 		I2Cdev::readBytes(m_devAddr, regAddr, size, buffer);
 	}

-	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const {
+	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
 		I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer);
 	}

+	bool hasSensorOnBus() {
+		// Ask twice, because we're nice like this
+		return I2CSCAN::hasDevOnBus(m_devAddr) || I2CSCAN::hasDevOnBus(m_devAddr);
+	}
+
+	uint8_t getAddress() const override { return m_devAddr; }
+
+	std::string toString() const {
+		char buf[16];
+		std::snprintf(buf, sizeof(buf), "I2C(0x%02x)", m_devAddr);
+		return std::string(buf);
+	}
+
 private:
 	uint8_t m_devAddr;
 };

-}  // namespace SlimeVR::Sensors::SoftFusion
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/ADCResistanceSensor.cpp
+++ b/src/sensors/ADCResistanceSensor.cpp
@@ -0,0 +1,44 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Eiren Rain & SlimeVR contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#include "ADCResistanceSensor.h"
+
+#include "GlobalVars.h"
+
+namespace SlimeVR::Sensors {
+void ADCResistanceSensor::motionLoop() {
+#if ESP8266
+	float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution;
+	m_Data = m_ResistanceDivider
+		   * (ADCVoltageMax / voltage - 1.0f);  // Convert voltage to resistance
+#elif defined(ESP32)
+	float voltage = ((float)analogReadMilliVolts(m_Pin)) / 1000;
+	m_Data = m_ResistanceDivider
+		   * (m_VCC / voltage - 1.0f);  // Convert voltage to resistance
+#endif
+}
+
+void ADCResistanceSensor::sendData() {
+	networkConnection.sendFlexData(sensorId, m_Data);
+}
+
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/ADCResistanceSensor.h
+++ b/src/sensors/ADCResistanceSensor.h
@@ -0,0 +1,71 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Eiren Rain & SlimeVR contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#pragma once
+
+#include "../sensorinterface/RegisterInterface.h"
+#include "sensor.h"
+
+namespace SlimeVR::Sensors {
+class ADCResistanceSensor : Sensor {
+public:
+	static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE;
+
+	ADCResistanceSensor(
+		uint8_t id,
+		uint8_t pin,
+		float VCC,
+		float resistanceDivider,
+		float smoothFactor = 0.1f
+	)
+		: Sensor(
+			"ADCResistanceSensor",
+			SensorTypeID::ADC_RESISTANCE,
+			id,
+			EmptyRegisterInterface::instance,
+			0.0f,
+			new SlimeVR::EmptySensorInterface
+		)
+		, m_Pin(pin)
+		, m_VCC(VCC)
+		, m_ResistanceDivider(resistanceDivider)
+		, m_SmoothFactor(smoothFactor){};
+	~ADCResistanceSensor();
+
+	void motionLoop() override final;
+	void sendData() override final;
+
+	SensorStatus getSensorState() override final { return SensorStatus::SENSOR_OK; }
+
+	SensorDataType getDataType() override final {
+		return SensorDataType::SENSOR_DATATYPE_FLEX_RESISTANCE;
+	};
+
+private:
+	uint8_t m_Pin;
+	float m_VCC;
+	float m_ResistanceDivider;
+	float m_SmoothFactor;
+
+	float m_Data = 0.0f;
+};
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/EmptySensor.h
+++ b/src/sensors/EmptySensor.h
@@ -24,14 +24,20 @@
 #ifndef SENSORS_EMPTYSENSOR_H
 #define SENSORS_EMPTYSENSOR_H

+#include "../sensorinterface/RegisterInterface.h"
 #include "sensor.h"

-namespace SlimeVR {
-namespace Sensors {
+namespace SlimeVR::Sensors {
 class EmptySensor : public Sensor {
 public:
 	EmptySensor(uint8_t id)
-		: Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){};
+		: Sensor(
+			"EmptySensor",
+			SensorTypeID::Empty,
+			id,
+			EmptyRegisterInterface::instance,
+			0.0
+		){};
 	~EmptySensor(){};

 	void motionSetup() override final{};
@@ -42,7 +48,6 @@ public:
 		return SensorStatus::SENSOR_OFFLINE;
 	};
 };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors

 #endif
--- a/src/sensors/ErroneousSensor.cpp
+++ b/src/sensors/ErroneousSensor.cpp
@@ -25,15 +25,15 @@

 #include "GlobalVars.h"

-namespace SlimeVR {
-namespace Sensors {
+namespace SlimeVR::Sensors {
 void ErroneousSensor::motionSetup() {
 	m_Logger.error(
 		"IMU of type %s failed to initialize",
 		getIMUNameByType(m_ExpectedType)
 	);
+	m_tpsCounter.reset();
+	m_dataCounter.reset();
 }

 SensorStatus ErroneousSensor::getSensorState() { return SensorStatus::SENSOR_ERROR; };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/ErroneousSensor.h
+++ b/src/sensors/ErroneousSensor.h
@@ -24,14 +24,14 @@
 #ifndef SENSORS_ERRONEOUSSENSOR_H
 #define SENSORS_ERRONEOUSSENSOR_H

+#include "../sensorinterface/RegisterInterface.h"
 #include "sensor.h"

-namespace SlimeVR {
-namespace Sensors {
+namespace SlimeVR::Sensors {
 class ErroneousSensor : public Sensor {
 public:
-	ErroneousSensor(uint8_t id, ImuID type)
-		: Sensor("ErroneousSensor", type, id, 0, 0.0)
+	ErroneousSensor(uint8_t id, SensorTypeID type)
+		: Sensor("ErroneousSensor", type, id, EmptyRegisterInterface::instance, 0.0)
 		, m_ExpectedType(type){};
 	~ErroneousSensor(){};

@@ -42,9 +42,8 @@ public:
 	SensorStatus getSensorState() override final;

 private:
-	ImuID m_ExpectedType;
+	SensorTypeID m_ExpectedType;
 };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors

 #endif
--- a/src/sensors/RestCalibrationDetector.cpp
+++ b/src/sensors/RestCalibrationDetector.cpp
@@ -0,0 +1,55 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "RestCalibrationDetector.h"
+
+#include "sensors/SensorFusion.h"
+
+namespace SlimeVR::Sensors {
+
+bool RestCalibrationDetector::update(SensorFusion& fusion) {
+	if (state == CalibrationState::Done) {
+		return false;
+	}
+
+	if (!fusion.getRestDetected()) {
+		state = CalibrationState::NoRest;
+		return false;
+	}
+
+	if (state == CalibrationState::NoRest) {
+		state = CalibrationState::Calibrating;
+		lastRestStartedMillis = millis();
+		return false;
+	}
+
+	uint32_t elapsed = millis() - lastRestStartedMillis;
+	if (elapsed < static_cast<uint32_t>(restCalibrationSeconds * 1e3)) {
+		return false;
+	}
+
+	state = CalibrationState::Done;
+	return true;
+}
+
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/RestCalibrationDetector.h
+++ b/src/sensors/RestCalibrationDetector.h
@@ -0,0 +1,49 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+
+#include "sensors/SensorFusion.h"
+
+namespace SlimeVR::Sensors {
+
+class RestCalibrationDetector {
+public:
+	bool update(SensorFusion& fusion);
+
+private:
+	static constexpr float restCalibrationSeconds = 3.0f;
+
+	enum class CalibrationState {
+		NoRest,
+		Calibrating,
+		Done,
+	};
+
+	CalibrationState state = CalibrationState::NoRest;
+	uint32_t lastRestStartedMillis = 0;
+};
+
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/SensorBuilder.cpp
+++ b/src/sensors/SensorBuilder.cpp
@@ -0,0 +1,126 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Eiren Rain, Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "SensorBuilder.h"
+
+namespace SlimeVR::Sensors {
+
+SensorBuilder::SensorBuilder(SensorManager* sensorManager)
+	: m_Manager(sensorManager) {}
+
+#define SENSOR_DESC_ENTRY(ImuType, ...)                                           \
+	activeSensorCount += sensorDescEntry<ImuType>(sensorID, __VA_ARGS__) ? 1 : 0; \
+	sensorID++;
+
+#define SENSOR_INFO_ENTRY(ImuID, SensorPosition) \
+	m_Manager->m_Sensors[ImuID]->setSensorInfo(SensorPosition);
+
+uint8_t SensorBuilder::buildAllSensors() {
+	uint8_t sensorID = 0;
+	uint8_t activeSensorCount = 0;
+
+	[[maybe_unused]] const auto NO_PIN = nullptr;
+	[[maybe_unused]] const auto NO_WIRE = &EmptySensorInterface::instance;
+	[[maybe_unused]] const auto DIRECT_PIN
+		= [&](uint8_t pin) { return interfaceManager.directPinInterface().get(pin); };
+	[[maybe_unused]] const auto DIRECT_WIRE = [&](uint8_t scl, uint8_t sda) {
+		return interfaceManager.i2cWireInterface().get(scl, sda);
+	};
+	[[maybe_unused]] const auto MCP_PIN = [&](uint8_t pin) {
+		return interfaceManager.mcpPinInterface().get(&m_Manager->m_MCP, pin);
+	};
+	[[maybe_unused]] const auto PCA_WIRE
+		= [&](uint8_t scl, uint8_t sda, uint8_t addr, uint8_t ch) {
+			  return interfaceManager.pcaWireInterface().get(scl, sda, addr, ch);
+		  };
+	[[maybe_unused]] const auto DIRECT_SPI
+		= [&](uint32_t clockFreq, uint8_t bitOrder, uint8_t dataMode) {
+			  return interfaceManager.directSPIInterface().get(
+				  SPI,
+				  SPISettings(clockFreq, bitOrder, dataMode)
+			  );
+		  };
+
+	// Apply descriptor list and expand to entries
+	SENSOR_DESC_LIST
+
+	// Apply sensor info list and expand to entries
+	SENSOR_INFO_LIST
+
+	return activeSensorCount;
+}
+
+std::unique_ptr<::Sensor>
+SensorBuilder::buildSensorDynamically(SensorTypeID type, SensorDefinition sensorDef) {
+	switch (type) {
+		// case SensorTypeID::MPU9250:
+		//	return buildSensor<MPU9250Sensor>(sensorDef);
+		// case SensorTypeID::BNO080:
+		//	return buildSensor<BNO080Sensor>(sensorDef);
+		case SensorTypeID::BNO085:
+			return buildSensor<BNO085Sensor>(sensorDef);
+		// case SensorTypeID::BNO055:
+		//	return buildSensor<BNO055Sensor>(sensorDef);
+		// case SensorTypeID::MPU6050:
+		//	return buildSensor<SoftFusionMPU6050>(
+		//		sensorDef
+		//	);
+		// case SensorTypeID::BNO086:
+		//	return buildSensor<BNO086Sensor>(sensorDef);
+		// case SensorTypeID::BMI160:
+		// 	return buildSensor<BMI160Sensor>(sensorDef);
+		// case SensorTypeID::ICM20948:
+		//	return buildSensor<ICM20948Sensor>(sensorDef);
+		// case SensorTypeID::ICM42688:
+		//	return buildSensor<SoftFusionICM42688>(
+		//		sensorDef
+		//	);
+		case SensorTypeID::BMI270:
+			return buildSensor<SoftFusionBMI270>(sensorDef);
+		// case SensorTypeID::LSM6DS3TRC:
+		//	return buildSensor<SoftFusionLSM6DS3TRC>(
+		//		sensorDef
+		//	);
+		case SensorTypeID::LSM6DSV:
+			return buildSensor<SoftFusionLSM6DSV>(sensorDef);
+		case SensorTypeID::LSM6DSO:
+			return buildSensor<SoftFusionLSM6DSO>(sensorDef);
+		case SensorTypeID::LSM6DSR:
+			return buildSensor<SoftFusionLSM6DSR>(sensorDef);
+		case SensorTypeID::ICM45686:
+			return buildSensor<SoftFusionICM45686>(sensorDef);
+		// case SensorTypeID::ICM45605:
+		//	return buildSensor<SoftFusionICM45605>(
+		//		sensorDef
+		//	);
+		default:
+			m_Manager->m_Logger.error(
+				"Unable to create sensor with type %s (%d)",
+				getIMUNameByType(type),
+				static_cast<int>(type)
+			);
+	}
+	return std::make_unique<EmptySensor>(sensorDef.sensorID);
+}
+
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/SensorBuilder.h
+++ b/src/sensors/SensorBuilder.h
@@ -0,0 +1,366 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Eiren Rain & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#pragma once
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <type_traits>
+
+#include "EmptySensor.h"
+#include "ErroneousSensor.h"
+#include "PinInterface.h"
+#include "SensorManager.h"
+#include "bno055sensor.h"
+#include "bno080sensor.h"
+#include "consts.h"
+#include "globals.h"
+#include "icm20948sensor.h"
+#include "logging/Logger.h"
+#include "mpu6050sensor.h"
+#include "mpu9250sensor.h"
+#include "sensor.h"
+#include "sensorinterface/DirectPinInterface.h"
+#include "sensorinterface/DirectSPIInterface.h"
+#include "sensorinterface/I2CPCAInterface.h"
+#include "sensorinterface/I2CWireSensorInterface.h"
+#include "sensorinterface/MCP23X17PinInterface.h"
+#include "sensorinterface/RegisterInterface.h"
+#include "sensorinterface/SPIImpl.h"
+#include "sensorinterface/SensorInterface.h"
+#include "sensorinterface/SensorInterfaceManager.h"
+#include "sensorinterface/i2cimpl.h"
+#include "softfusion/drivers/bmi160.h"
+#include "softfusion/drivers/bmi270.h"
+#include "softfusion/drivers/icm42688.h"
+#include "softfusion/drivers/icm45605.h"
+#include "softfusion/drivers/icm45686.h"
+#include "softfusion/drivers/lsm6ds3trc.h"
+#include "softfusion/drivers/lsm6dso.h"
+#include "softfusion/drivers/lsm6dsr.h"
+#include "softfusion/drivers/lsm6dsv.h"
+#include "softfusion/drivers/mpu6050.h"
+#include "softfusion/softfusionsensor.h"
+
+#ifndef PRIMARY_IMU_ADDRESS_ONE
+#define PRIMARY_IMU_ADDRESS_ONE false
+#endif
+
+#ifndef SECONDARY_IMU_ADDRESS_TWO
+#define SECONDARY_IMU_ADDRESS_TWO true
+#endif
+
+#if USE_RUNTIME_CALIBRATION
+#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
+#define SFCALIBRATOR RuntimeCalibration::RuntimeCalibrator
+#else
+#include "sensors/softfusion/SoftfusionCalibration.h"
+#define SFCALIBRATOR SoftfusionCalibrator
+#endif
+
+#ifdef ESP32
+#include "driver/i2c.h"
+#endif
+
+namespace SlimeVR::Sensors {
+using SoftFusionLSM6DS3TRC
+	= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SFCALIBRATOR>;
+using SoftFusionICM42688
+	= SoftFusionSensor<SoftFusion::Drivers::ICM42688, SFCALIBRATOR>;
+using SoftFusionBMI270 = SoftFusionSensor<SoftFusion::Drivers::BMI270, SFCALIBRATOR>;
+using SoftFusionLSM6DSV = SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SFCALIBRATOR>;
+using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SFCALIBRATOR>;
+using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SFCALIBRATOR>;
+using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SFCALIBRATOR>;
+using SoftFusionICM45686
+	= SoftFusionSensor<SoftFusion::Drivers::ICM45686, SFCALIBRATOR>;
+using SoftFusionICM45605
+	= SoftFusionSensor<SoftFusion::Drivers::ICM45605, SFCALIBRATOR>;
+using SoftFusionBMI160 = SoftFusionSensor<SoftFusion::Drivers::BMI160, SFCALIBRATOR>;
+class SensorAuto {};
+
+struct SensorBuilder {
+private:
+	struct SensorDefinition {
+		uint8_t sensorID;
+		RegisterInterface& imuInterface;
+		float rotation;
+		SensorInterface* sensorInterface;
+		bool optional;
+		PinInterface* intPin;
+		int extraParam;
+	};
+
+public:
+	SensorManager* m_Manager;
+	explicit SensorBuilder(SensorManager* sensorManager);
+
+	uint8_t buildAllSensors();
+
+	std::unique_ptr<::Sensor>
+	buildSensorDynamically(SensorTypeID type, SensorDefinition sensorDef);
+
+	template <typename Sensor, typename AccessInterface>
+	std::optional<std::pair<SensorTypeID, RegisterInterface*>> checkSensorPresent(
+		uint8_t sensorId,
+		SensorInterface* sensorInterface,
+		AccessInterface accessInterface
+	) {
+		auto* registerInterface
+			= getRegisterInterface<Sensor>(sensorId, sensorInterface, accessInterface);
+
+		if (!registerInterface->hasSensorOnBus()) {
+			return {};
+		}
+
+		if constexpr (requires {
+						  {
+							  Sensor::checkPresent(*registerInterface)
+						  } -> std::same_as<SensorTypeID>;
+					  }) {
+			auto type = Sensor::checkPresent(*registerInterface);
+
+			if (type == SensorTypeID::Unknown) {
+				return {};
+			}
+
+			return std::make_pair(type, registerInterface);
+		} else {
+			if (!Sensor::checkPresent(*registerInterface)) {
+				return {};
+			}
+		}
+
+		return std::make_pair(Sensor::TypeID, registerInterface);
+	}
+
+	template <typename AccessInterface>
+	inline std::optional<std::pair<SensorTypeID, RegisterInterface*>>
+	checkSensorsPresent(uint8_t, SensorInterface*, AccessInterface) {
+		return std::nullopt;
+	}
+
+	template <typename AccessInterface, typename Sensor, typename... Rest>
+	inline std::optional<std::pair<SensorTypeID, RegisterInterface*>>
+	checkSensorsPresent(
+		uint8_t sensorId,
+		SensorInterface* sensorInterface,
+		AccessInterface accessInterface
+	) {
+		auto result
+			= checkSensorPresent<Sensor>(sensorId, sensorInterface, accessInterface);
+		if (result) {
+			return result;
+		}
+
+		return checkSensorsPresent<AccessInterface, Rest...>(
+			sensorId,
+			sensorInterface,
+			accessInterface
+		);
+	}
+
+	template <typename Sensor, typename AccessInterface>
+	RegisterInterface* getRegisterInterface(
+		uint8_t sensorId,
+		SensorInterface* interface,
+		AccessInterface access
+	) {
+		if constexpr (std::is_base_of_v<
+						  PinInterface,
+						  std::remove_pointer_t<AccessInterface>>) {
+			return interfaceManager.spiImpl().get(
+				static_cast<DirectSPIInterface*>(interface),
+				access
+			);
+		} else if constexpr (std::is_same_v<AccessInterface, bool>) {
+			uint8_t addressIncrement = access ? 1 : 0;
+			return interfaceManager.i2cImpl().get(Sensor::Address + addressIncrement);
+		} else if constexpr (std::is_integral_v<AccessInterface>) {
+			return interfaceManager.i2cImpl().get(access);
+		}
+
+		return &EmptyRegisterInterface::instance;
+	}
+
+	template <typename AccessInterface>
+	std::optional<std::pair<SensorTypeID, RegisterInterface*>> findSensorType(
+		uint8_t sensorID,
+		SensorInterface* sensorInterface,
+		AccessInterface accessInterface
+	) {
+		sensorInterface->init();
+		sensorInterface->swapIn();
+
+		return checkSensorsPresent<
+			AccessInterface,
+			// SoftFusionLSM6DS3TRC,
+			// SoftFusionICM42688,
+			SoftFusionBMI270,
+			SoftFusionLSM6DSV,
+			SoftFusionLSM6DSO,
+			SoftFusionLSM6DSR,
+			// SoftFusionMPU6050,
+			SoftFusionICM45686,
+			// SoftFusionICM45605
+			BNO085Sensor>(sensorID, sensorInterface, accessInterface);
+	}
+
+	template <typename SensorType, typename AccessInterface>
+	bool sensorDescEntry(
+		uint8_t sensorID,
+		AccessInterface accessInterface,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional = false,
+		PinInterface* intPin = nullptr,
+		int extraParam = 0
+	) {
+		std::unique_ptr<::Sensor> sensor;
+		if constexpr (std::is_same<SensorType, SensorAuto>::value) {
+			auto result = findSensorType(sensorID, sensorInterface, accessInterface);
+
+			if (!result) {
+				m_Manager->m_Logger.error(
+					"Can't find sensor type for sensor %d",
+					sensorID
+				);
+				return false;
+			}
+
+			auto sensorType = result->first;
+			auto& regInterface = *(result->second);
+
+			m_Manager->m_Logger.info(
+				"Sensor %d automatically detected with %s",
+				sensorID,
+				getIMUNameByType(sensorType)
+			);
+			sensor = buildSensorDynamically(
+				sensorType,
+				{
+					sensorID,
+					regInterface,
+					rotation,
+					sensorInterface,
+					optional,
+					intPin,
+					extraParam,
+				}
+			);
+		} else {
+			auto& regInterface = *getRegisterInterface<SensorType>(
+				sensorID,
+				sensorInterface,
+				accessInterface
+			);
+
+			sensor = buildSensor<SensorType>({
+				sensorID,
+				regInterface,
+				rotation,
+				sensorInterface,
+				optional,
+				intPin,
+				extraParam,
+			});
+		}
+
+		bool working = sensor->isWorking();
+		m_Manager->m_Sensors.push_back(std::move(sensor));
+
+		if (!working) {
+			return false;
+		}
+
+		m_Manager->m_Logger.info("Sensor %d configured", sensorID);
+		return true;
+	}
+
+	template <typename ImuType>
+	std::unique_ptr<::Sensor> buildSensor(SensorDefinition sensorDef) {
+		m_Manager->m_Logger.trace(
+			"Building IMU with: id=%d,\n\
+						address=%s, rotation=%f,\n\
+						interface=%s, int=%s, extraParam=%d, optional=%d",
+			sensorDef.sensorID,
+			sensorDef.imuInterface.toString().c_str(),
+			sensorDef.rotation,
+			sensorDef.sensorInterface->toString().c_str(),
+			sensorDef.intPin ? sensorDef.intPin->toString().c_str() : "None",
+			sensorDef.extraParam,
+			sensorDef.optional
+		);
+
+		// Now start detecting and building the IMU
+		std::unique_ptr<::Sensor> sensor;
+
+		// Init I2C bus for each sensor upon startup
+		sensorDef.sensorInterface->init();
+		sensorDef.sensorInterface->swapIn();
+
+		if (!sensorDef.imuInterface.hasSensorOnBus()) {
+			if (!sensorDef.optional) {
+				m_Manager->m_Logger.error(
+					"Mandatory sensor %d not found at address %s",
+					sensorDef.sensorID + 1,
+					sensorDef.imuInterface.toString().c_str()
+				);
+				return std::make_unique<ErroneousSensor>(
+					sensorDef.sensorID,
+					ImuType::TypeID
+				);
+			} else {
+				m_Manager->m_Logger.debug(
+					"Optional sensor %d not found at address %s",
+					sensorDef.sensorID + 1,
+					sensorDef.imuInterface.toString().c_str()
+				);
+				return std::make_unique<EmptySensor>(sensorDef.sensorID);
+			}
+		}
+
+		m_Manager->m_Logger.trace(
+			"Sensor %d found at address %s",
+			sensorDef.sensorID + 1,
+			sensorDef.imuInterface.toString().c_str()
+		);
+
+		sensor = std::make_unique<ImuType>(
+			sensorDef.sensorID,
+			sensorDef.imuInterface,
+			sensorDef.rotation,
+			sensorDef.sensorInterface,
+			sensorDef.intPin,
+			sensorDef.extraParam
+		);
+
+		sensor->motionSetup();
+		return sensor;
+	}
+
+private:
+	SensorInterfaceManager interfaceManager;
+};
+
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/SensorFusion.cpp
+++ b/src/sensors/SensorFusion.cpp
@@ -1,7 +1,6 @@
 #include "SensorFusion.h"

-namespace SlimeVR {
-namespace Sensors {
+namespace SlimeVR::Sensors {

 void SensorFusion::update6D(
 	sensor_real_t Axyz[3],
@@ -29,13 +28,7 @@ void SensorFusion::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat)
 	}

 	std::copy(Axyz, Axyz + 3, bAxyz);
-#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-	accelUpdated = true;
-#elif SENSOR_USE_BASICVQF
-	basicvqf.updateAcc(Axyz);
-#elif SENSOR_USE_VQF
 	vqf.updateAcc(Axyz);
-#endif
 }

 void SensorFusion::updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat) {
@@ -51,13 +44,7 @@ void SensorFusion::updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat)
 		}
 	}

-#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-	std::copy(Mxyz, Mxyz + 3, bMxyz);
-#elif SENSOR_USE_BASICVQF
-	basicvqf.updateMag(Mxyz);
-#elif SENSOR_USE_VQF
 	vqf.updateMag(Mxyz);
-#endif
 }

 void SensorFusion::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat) {
@@ -65,73 +52,7 @@ void SensorFusion::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat)
 		deltat = gyrTs;
 	}

-#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-	sensor_real_t Axyz[3]{0.0f, 0.0f, 0.0f};
-	if (accelUpdated) {
-		std::copy(bAxyz, bAxyz + 3, Axyz);
-		accelUpdated = false;
-	}
-#endif
-
-#if SENSOR_USE_MAHONY
-	if (!magExist) {
-		mahony.update(
-			qwxyz,
-			Axyz[0],
-			Axyz[1],
-			Axyz[2],
-			Gxyz[0],
-			Gxyz[1],
-			Gxyz[2],
-			deltat
-		);
-	} else {
-		mahony.update(
-			qwxyz,
-			Axyz[0],
-			Axyz[1],
-			Axyz[2],
-			Gxyz[0],
-			Gxyz[1],
-			Gxyz[2],
-			bMxyz[0],
-			bMxyz[1],
-			bMxyz[2],
-			deltat
-		);
-	}
-#elif SENSOR_USE_MADGWICK
-	if (!magExist) {
-		madgwick.update(
-			qwxyz,
-			Axyz[0],
-			Axyz[1],
-			Axyz[2],
-			Gxyz[0],
-			Gxyz[1],
-			Gxyz[2],
-			deltat
-		);
-	} else {
-		madgwick.update(
-			qwxyz,
-			Axyz[0],
-			Axyz[1],
-			Axyz[2],
-			Gxyz[0],
-			Gxyz[1],
-			Gxyz[2],
-			bMxyz[0],
-			bMxyz[1],
-			bMxyz[2],
-			deltat
-		);
-	}
-#elif SENSOR_USE_BASICVQF
-	basicvqf.updateGyr(Gxyz, deltat);
-#elif SENSOR_USE_VQF
 	vqf.updateGyr(Gxyz, deltat);
-#endif

 	updated = true;
 	gravityReady = false;
@@ -143,19 +64,11 @@ bool SensorFusion::isUpdated() { return updated; }
 void SensorFusion::clearUpdated() { updated = false; }

 sensor_real_t const* SensorFusion::getQuaternion() {
-#if SENSOR_USE_BASICVQF
-	if (magExist) {
-		basicvqf.getQuat9D(qwxyz);
-	} else {
-		basicvqf.getQuat6D(qwxyz);
-	}
-#elif SENSOR_USE_VQF
 	if (magExist) {
 		vqf.getQuat9D(qwxyz);
 	} else {
 		vqf.getQuat6D(qwxyz);
 	}
-#endif

 	return qwxyz;
 }
@@ -211,5 +124,11 @@ void SensorFusion::calcLinearAcc(
 	accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY;
 	accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
 }
-}  // namespace Sensors
-}  // namespace SlimeVR
+
+void SensorFusion::updateBiasForgettingTime(float biasForgettingTime) {
+	vqf.updateBiasForgettingTime(biasForgettingTime);
+}
+
+bool SensorFusion::getRestDetected() const { return vqf.getRestDetected(); }
+
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/SensorFusion.h
+++ b/src/sensors/SensorFusion.h
@@ -6,55 +6,24 @@

 #define SENSOR_DOUBLE_PRECISION 0

-#define SENSOR_FUSION_TYPE SENSOR_FUSION_VQF
-
-#define SENSOR_FUSION_MAHONY 1
-#define SENSOR_FUSION_MADGWICK 2
-#define SENSOR_FUSION_BASICVQF 3
-#define SENSOR_FUSION_VQF 4
-
-#if SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY
-#define SENSOR_FUSION_TYPE_STRING "mahony"
-#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK
-#define SENSOR_FUSION_TYPE_STRING "madgwick"
-#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF
-#define SENSOR_FUSION_TYPE_STRING "bvqf"
-#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF
 #define SENSOR_FUSION_TYPE_STRING "vqf"
-#endif

-#define SENSOR_USE_MAHONY (SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY)
-#define SENSOR_USE_MADGWICK (SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK)
-#define SENSOR_USE_BASICVQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF)
-#define SENSOR_USE_VQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF)
-
-#include <basicvqf.h>
 #include <vqf.h>

 #include "../motionprocessing/types.h"
-#include "madgwick.h"
-#include "mahony.h"

-namespace SlimeVR {
-namespace Sensors {
-#if SENSOR_USE_VQF
-struct SensorVQFParams : VQFParams {
-	SensorVQFParams()
-		: VQFParams() {
-#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-		motionBiasEstEnabled = true;
-#endif
-		tauAcc = 2.0f;
-		restMinT = 2.0f;
-		restThGyr = 0.6f;  // 400 norm
-		restThAcc = 0.06f;  // 100 norm
-	}
+namespace SlimeVR::Sensors {
+constexpr VQFParams DefaultVQFParams = VQFParams{
+	.tauAcc = 2.0f,
+	.restMinT = 2.0f,
+	.restThGyr = 0.6f,
+	.restThAcc = 0.06f,
 };
-#endif

 class SensorFusion {
 public:
 	SensorFusion(
+		VQFParams vqfParams,
 		sensor_real_t gyrTs,
 		sensor_real_t accTs = -1.0,
 		sensor_real_t magTs = -1.0
@@ -62,18 +31,18 @@ public:
 		: gyrTs(gyrTs)
 		, accTs((accTs < 0) ? gyrTs : accTs)
 		, magTs((magTs < 0) ? gyrTs : magTs)
-#if SENSOR_USE_MAHONY
-#elif SENSOR_USE_MADGWICK
-#elif SENSOR_USE_BASICVQF
-		, basicvqf(gyrTs, ((accTs < 0) ? gyrTs : accTs), ((magTs < 0) ? gyrTs : magTs))
-#elif SENSOR_USE_VQF
-		, vqf(vqfParams,
+		, vqfParams(vqfParams)
+		, vqf(this->vqfParams,
 			  gyrTs,
 			  ((accTs < 0) ? gyrTs : accTs),
-			  ((magTs < 0) ? gyrTs : magTs))
-#endif
-	{
-	}
+			  ((magTs < 0) ? gyrTs : magTs)) {}
+
+	explicit SensorFusion(
+		sensor_real_t gyrTs,
+		sensor_real_t accTs = -1.0,
+		sensor_real_t magTs = -1.0
+	)
+		: SensorFusion(DefaultVQFParams, gyrTs, accTs, magTs) {}

 	void update6D(
 		sensor_real_t Axyz[3],
@@ -106,31 +75,21 @@ public:
 		sensor_real_t accout[3]
 	);

+	void updateBiasForgettingTime(float biasForgettingTime);
+
+	[[nodiscard]] bool getRestDetected() const;
+
 protected:
 	sensor_real_t gyrTs;
 	sensor_real_t accTs;
 	sensor_real_t magTs;

-#if SENSOR_USE_MAHONY
-	Mahony<sensor_real_t> mahony;
-#elif SENSOR_USE_MADGWICK
-	Madgwick<sensor_real_t> madgwick;
-#elif SENSOR_USE_BASICVQF
-	BasicVQF basicvqf;
-#elif SENSOR_USE_VQF
-	SensorVQFParams vqfParams{};
+	VQFParams vqfParams;
 	VQF vqf;
-#endif

 	// A also used for linear acceleration extraction
 	sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};

-#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-	// Buffer M here to keep the behavior of BMI160
-	sensor_real_t bMxyz[3]{0.0f, 0.0f, 0.0f};
-	bool accelUpdated = false;
-#endif
-
 	bool magExist = false;
 	sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
 	bool updated = false;
@@ -139,11 +98,10 @@ protected:
 	sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
 	bool linaccelReady = false;
 	sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
-#if ESP32
+#ifdef ESP32
 	sensor_real_t linAccel_guard;  // Temporary patch for some weird ESP32 bug
 #endif
 };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors

 #endif  // SLIMEVR_SENSORFUSION_H
--- a/src/sensors/SensorFusionDMP.cpp
+++ b/src/sensors/SensorFusionDMP.cpp
@@ -1,7 +1,6 @@
 #include "SensorFusionDMP.h"

-namespace SlimeVR {
-namespace Sensors {
+namespace SlimeVR::Sensors {
 void SensorFusionDMP::updateAcc(sensor_real_t Axyz[3]) {
 	std::copy(Axyz, Axyz + 3, bAxyz);
 }
@@ -96,5 +95,4 @@ Vector3 SensorFusionDMP::getLinearAccVec() {
 	getLinearAcc();
 	return Vector3(linAccel[0], linAccel[1], linAccel[2]);
 }
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors
--- a/src/sensors/SensorFusionDMP.h
+++ b/src/sensors/SensorFusionDMP.h
@@ -1,11 +1,12 @@
 #ifndef SLIMEVR_SENSORFUSIONDMP_H
 #define SLIMEVR_SENSORFUSIONDMP_H

+#include <helper_3dmath.h>
+
 #include "SensorFusion.h"
 #include "dmpmag.h"

-namespace SlimeVR {
-namespace Sensors {
+namespace SlimeVR::Sensors {
 class SensorFusionDMP {
 public:
 	void updateQuaternion(sensor_real_t nqwxyz[4]);
@@ -37,11 +38,10 @@ protected:
 	sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
 	bool linaccelReady = false;
 	sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
-#if ESP32
+#ifdef ESP32
 	sensor_real_t linAccel_guard;  // Temporary patch for some weird ESP32 bug
 #endif
 };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors

 #endif  // SLIMEVR_SENSORFUSIONDMP_H
--- a/src/sensors/SensorFusionRestDetect.cpp
+++ b/src/sensors/SensorFusionRestDetect.cpp
@@ -1,37 +0,0 @@
-#include "SensorFusionRestDetect.h"
-
-namespace SlimeVR {
-namespace Sensors {
-#if !SENSOR_FUSION_WITH_RESTDETECT
-void SensorFusionRestDetect::updateAcc(
-	const sensor_real_t Axyz[3],
-	sensor_real_t deltat
-) {
-	if (deltat < 0) {
-		deltat = accTs;
-	}
-	restDetection.updateAcc(deltat, Axyz);
-	SensorFusion::updateAcc(Axyz, deltat);
-}
-
-void SensorFusionRestDetect::updateGyro(
-	const sensor_real_t Gxyz[3],
-	sensor_real_t deltat
-) {
-	if (deltat < 0) {
-		deltat = gyrTs;
-	}
-	restDetection.updateGyr(Gxyz);
-	SensorFusion::updateGyro(Gxyz, deltat);
-}
-#endif
-
-bool SensorFusionRestDetect::getRestDetected() {
-#if !SENSOR_FUSION_WITH_RESTDETECT
-	return restDetection.getRestDetected();
-#elif SENSOR_USE_VQF
-	return vqf.getRestDetected();
-#endif
-}
-}  // namespace Sensors
-}  // namespace SlimeVR
--- a/src/sensors/SensorFusionRestDetect.h
+++ b/src/sensors/SensorFusionRestDetect.h
@@ -1,51 +0,0 @@
-#ifndef SLIMEVR_SENSORFUSIONRESTDETECT_H_
-#define SLIMEVR_SENSORFUSIONRESTDETECT_H_
-
-#include "../motionprocessing/RestDetection.h"
-#include "SensorFusion.h"
-
-#if SENSOR_USE_VQF
-#define SENSOR_FUSION_WITH_RESTDETECT 1
-#else
-#define SENSOR_FUSION_WITH_RESTDETECT 0
-#endif
-
-namespace SlimeVR {
-namespace Sensors {
-#if !SENSOR_FUSION_WITH_RESTDETECT
-struct SensorRestDetectionParams : RestDetectionParams {
-	SensorRestDetectionParams()
-		: RestDetectionParams() {
-		restMinTime = 2.0f;
-		restThGyr = 0.6f;  // 400 norm
-		restThAcc = 0.06f;  // 100 norm
-	}
-};
-#endif
-
-class SensorFusionRestDetect : public SensorFusion {
-public:
-	SensorFusionRestDetect(float gyrTs, float accTs = -1.0, float magTs = -1.0)
-		: SensorFusion(gyrTs, accTs, magTs)
-#if !SENSOR_FUSION_WITH_RESTDETECT
-		, restDetection(restDetectionParams, gyrTs, (accTs < 0) ? gyrTs : accTs)
-#endif
-	{
-	}
-
-	bool getRestDetected();
-
-#if !SENSOR_FUSION_WITH_RESTDETECT
-	void updateAcc(const sensor_real_t Axyz[3], const sensor_real_t deltat);
-	void updateGyro(const sensor_real_t Gxyz[3], const sensor_real_t deltat);
-#endif
-protected:
-#if !SENSOR_FUSION_WITH_RESTDETECT
-	SensorRestDetectionParams restDetectionParams{};
-	RestDetection restDetection;
-#endif
-};
-}  // namespace Sensors
-}  // namespace SlimeVR
-
-#endif  // SLIMEVR_SENSORFUSIONRESTDETECT_H_
--- a/src/sensors/SensorManager.cpp
+++ b/src/sensors/SensorManager.cpp
@@ -23,111 +23,35 @@

 #include "SensorManager.h"

-#include "bmi160sensor.h"
-#include "bno055sensor.h"
-#include "bno080sensor.h"
-#include "icm20948sensor.h"
-#include "mpu6050sensor.h"
-#include "mpu9250sensor.h"
-#include "sensoraddresses.h"
-#include "softfusion/drivers/bmi270.h"
-#include "softfusion/drivers/icm42688.h"
-#include "softfusion/drivers/lsm6ds3trc.h"
-#include "softfusion/drivers/lsm6dso.h"
-#include "softfusion/drivers/lsm6dsr.h"
-#include "softfusion/drivers/lsm6dsv.h"
-#include "softfusion/drivers/mpu6050.h"
-#include "softfusion/i2cimpl.h"
-#include "softfusion/softfusionsensor.h"
+#include "SensorBuilder.h"

-#if ESP32
-#include "driver/i2c.h"
-#endif
-
-namespace SlimeVR {
-namespace Sensors {
-using SoftFusionLSM6DS3TRC
-	= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl>;
-using SoftFusionICM42688
-	= SoftFusionSensor<SoftFusion::Drivers::ICM42688, SoftFusion::I2CImpl>;
-using SoftFusionBMI270
-	= SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl>;
-using SoftFusionLSM6DSV
-	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl>;
-using SoftFusionLSM6DSO
-	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
-using SoftFusionLSM6DSR
-	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
-using SoftFusionMPU6050
-	= SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
-
-// TODO Make it more generic in the future and move another place (abstract sensor
-// interface)
-void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) {
-	if (sclPin != activeSCL || sdaPin != activeSDA || !running) {
-		Wire.flush();
-#if ESP32
-		if (running) {
-		} else {
-			// Reset HWI2C to avoid being affected by I2CBUS reset
-			Wire.end();
-		}
-		// Disconnect pins from HWI2C
-		gpio_set_direction((gpio_num_t)activeSCL, GPIO_MODE_INPUT);
-		gpio_set_direction((gpio_num_t)activeSDA, GPIO_MODE_INPUT);
-
-		if (running) {
-			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
-
-		activeSCL = sclPin;
-		activeSDA = sdaPin;
-	}
-}
+namespace SlimeVR::Sensors {

 void SensorManager::setup() {
-	running = false;
-	activeSCL = PIN_IMU_SCL;
-	activeSDA = PIN_IMU_SDA;
-
-	uint8_t sensorID = 0;
-	uint8_t activeSensorCount = 0;
-#define IMU_DESC_ENTRY(ImuType, ...)                               \
-	{                                                              \
-		auto sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__); \
-		if (sensor->isWorking()) {                                 \
-			m_Logger.info("Sensor %d configured", sensorID + 1);   \
-			activeSensorCount++;                                   \
-		}                                                          \
-		m_Sensors.push_back(std::move(sensor));                    \
-		sensorID++;                                                \
+	if (m_MCP.begin_I2C()) {
+		m_Logger.info("MCP initialized");
 	}
-	// Apply descriptor list and expand to entrys
-	IMU_DESC_LIST;

-#undef IMU_DESC_ENTRY
+	SensorBuilder sensorBuilder = SensorBuilder(this);
+	uint8_t activeSensorCount = sensorBuilder.buildAllSensors();
+
 	m_Logger.info("%d sensor(s) configured", activeSensorCount);
 	// Check and scan i2c if no sensors active
 	if (activeSensorCount == 0) {
 		m_Logger.error(
 			"Can't find I2C device on provided addresses, scanning for all I2C devices "
-			"and returning"
+			"in the background"
 		);
 		I2CSCAN::scani2cports();
 	}
 }

 void SensorManager::postSetup() {
-	running = true;
 	for (auto& sensor : m_Sensors) {
 		if (sensor->isWorking()) {
-			swapI2C(sensor->sclPin, sensor->sdaPin);
+			if (sensor->m_hwInterface != nullptr) {
+				sensor->m_hwInterface->swapIn();
+			}
 			sensor->postSetup();
 		}
 	}
@@ -138,7 +62,9 @@ void SensorManager::update() {
 	bool allIMUGood = true;
 	for (auto& sensor : m_Sensors) {
 		if (sensor->isWorking()) {
-			swapI2C(sensor->sclPin, sensor->sdaPin);
+			if (sensor->m_hwInterface != nullptr) {
+				sensor->m_hwInterface->swapIn();
+			}
 			sensor->motionLoop();
 		}
 		if (sensor->getSensorState() == SensorStatus::SENSOR_ERROR) {
@@ -195,5 +121,4 @@ void SensorManager::update() {
 #endif
 }

-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors
--- a/Show More
+++ b/Show More