Add nrf const to consts.h (#505)

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

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

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

.github/workflows/actions.yml

@@ -14,7 +14,7 @@ jobs:
   format:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v6
       - uses: jidicula/clang-format-action@v4.14.0
         with:
           clang-format-version: "17"
@@ -33,8 +33,8 @@ jobs:
     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
@@ -45,7 +45,7 @@ jobs:
         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"
 
@@ -58,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
@@ -71,3 +71,4 @@ jobs:
           generate_release_notes: true
           files: |
             ./build/BOARD_SLIMEVR-firmware.bin
+            ./build/BOARD_SLIMEVR_V1_2-firmware.bin

.gitignore

@@ -4,3 +4,7 @@ build/
 venv/
 cache/
 .idea/
+compile_commands.json
+node_modules/
+dist/
+.nix-platformio

board-defaults.json

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

board-defaults.schema.json

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

boards/esp32s3_supermini.json

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

boards/variants/esp32s3_supermini/pins_arduino.h

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

ci/build.py

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

flake.lock

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

flake.nix

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

lib/bmi160/BMI160.h

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

lib/bno080/BNO080.cpp

@@ -12,7 +12,6 @@
 
   This library handles the initialization of the BNO080 and is able to query the sensor
   for different readings.
-
   https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library
 
   Development environment specifics:
@@ -688,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()
 {

lib/bno080/BNO080.h

@@ -214,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();

lib/bno080/PinInterface.h

@@ -23,6 +23,7 @@
 #pragma once
 
 #include <cstdint>
+#include <string>
 
 class PinInterface
 {
@@ -31,4 +32,6 @@ public:
 	virtual int digitalRead() = 0;
 	virtual void pinMode(uint8_t mode) = 0;
 	virtual void digitalWrite(uint8_t val) = 0;
+
+	[[nodiscard]] virtual std::string toString() const = 0;
 };

lib/i2cscan/i2cscan.cpp

@@ -1,116 +1,156 @@
 #include "i2cscan.h"
+
+#include <array>
+#include <cstdint>
+#include <string>
+
 #include "../../src/globals.h"
+#include "../../src/consts.h"
 
-#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"};
-#elif defined(ESP32C3)
-uint8_t portArray[] = {2, 3, 4, 5, 6, 7, 8, 9, 10};
-uint8_t portExclude[] = {18, 19, 20, 21, LED_PIN};
-String portMap[] = {"2", "3", "4", "5", "6", "7", "8", "9", "10"};
-#elif defined(ESP32C6)
-uint8_t portArray[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23};
-String portMap[] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"};
-uint8_t portExclude[] = {12, 13, 16, 17, LED_PIN};
-#elif defined(ESP32)
-uint8_t portArray[] = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
-String portMap[] = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
-uint8_t portExclude[] = {LED_PIN};
-#endif
-
-namespace I2CSCAN
-{
-    enum class ScanState {
+namespace I2CSCAN {
+    enum class ScanState : uint8_t {
         IDLE,
         SCANNING,
         DONE
     };
 
-    ScanState scanState = ScanState::IDLE;
-    uint8_t currentSDA = 0;
-    uint8_t currentSCL = 0;
-    uint8_t currentAddress = 1;
-    bool found = false;
-    std::vector<uint8_t> validPorts;
+	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;
 
-    void scani2cports()
-    {
+#ifdef ESP8266
+		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)
+		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)
+		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
+
+		bool selectNextPort() {
+			currentSCL++;
+
+			if(validPorts[currentSCL] == validPorts[currentSDA]) currentSCL++;
+
+			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) {
-            return;
+			if (scanState == ScanState::DONE) {
+				Serial.println("[DEBUG] I2C scan finished previously, resetting and scanning again..."); //NOLINT
+			} else {
+				return; // Already scanning, do not start again
+			}
         }
 
         // Filter out excluded ports
-        for (size_t i = 0; i < sizeof(portArray); i++) {
-            if (!inArray(portArray[i], portExclude, sizeof(portExclude))) {
-                validPorts.push_back(portArray[i]);
-            }
-        }
+		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;
-    }
+	}
 
-    bool selectNextPort() {
-        currentSCL++;
-        if(validPorts[currentSCL] == validPorts[currentSDA])
-            currentSCL++;
-        if (currentSCL < validPorts.size()) {
-            Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
-            return true;
-        }
-
-        currentSCL = 0;
-        currentSDA++;
-
-        if (currentSDA >= validPorts.size()) {
-            if (!found) {
-                Serial.println("[ERR] I2C: No I2C devices found");
-            }
-#if ESP32
-            Wire.end();
-#endif
-            Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
-            scanState = ScanState::DONE;
-            return false;
-        }
-
-        Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
-        return true;
-    }
-
-    void update()
-    {
+    void update() {
         if (scanState != ScanState::SCANNING) {
             return;
         }
 
-        if (currentAddress == 1) {
-#if ESP32
+#ifdef ESP32
+		if (currentAddress == 1) {
             Wire.end();
+		}
 #endif
-        }
 
         Wire.beginTransmission(currentAddress);
-        byte error = Wire.endTransmission();
+        const uint8_t error = Wire.endTransmission();
 
-        if (error == 0)
-        {
-            Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n",
+        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)
-        {
-            Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n",
+        } 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++;
         }
 
         currentAddress++;
+
         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
+			}
+
             return;
         }
 
@@ -118,19 +158,6 @@ namespace I2CSCAN
         selectNextPort();
     }
 
-    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;
-            }
-        }
-
-        return false;
-    }
-
     bool hasDevOnBus(uint8_t addr) {
         byte error;
 #if ESP32C3
@@ -165,9 +192,9 @@ namespace I2CSCAN
      */
 
     int clearBus(uint8_t SDA, uint8_t SCL) {
-        #if defined(TWCR) && defined(TWEN)
+#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
+#endif
 
         pinMode(SDA, INPUT_PULLUP);
         pinMode(SCL, INPUT_PULLUP);
@@ -214,4 +241,4 @@ namespace I2CSCAN
         pinMode(SCL, INPUT);
         return 0;
     }
-}
+}

lib/i2cscan/i2cscan.h

@@ -11,7 +11,7 @@ namespace I2CSCAN {
     bool hasDevOnBus(uint8_t addr);
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound);
     int clearBus(uint8_t SDA, uint8_t SCL);
-    boolean inArray(uint8_t value, uint8_t* arr, size_t arrSize);
+    bool inArray(uint8_t value, const uint8_t *array, size_t arraySize);
 }
 
-#endif // _I2CSCAN_H_
+#endif // _I2CSCAN_H_

lib/magneto/madgwick.h

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

lib/magneto/madgwick.hpp

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

lib/magneto/mahony.h

@@ -1,50 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
-
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
-
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
-
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-
-#ifndef _MAHONY_H_
-#define _MAHONY_H_
-
-#include "helper_3dmath.h"
-
-template<typename T>
-class Mahony {
-
-    // These are the free parameters in the Mahony filter and fusion scheme,
-    // Kp for proportional feedback, Ki for integral
-    // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
-    static constexpr float Kp = 10.0f;
-    static constexpr float Ki = 0.0f;
-
-public:
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat);
-    void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat);
-    
-private:
-    T ix = 0.0;
-    T iy = 0.0;
-    T iz = 0.0;
-};
-
-#include "mahony.hpp"
-
-#endif /* _MAHONY_H_ */

lib/magneto/mahony.hpp

@@ -1,206 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
-
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
-
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
-
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-
-#include "mahony.h"
-
-// Mahony orientation filter, assumed World Frame NWU (xNorth, yWest, zUp)
-// Modified from Madgwick version to remove Z component of magnetometer:
-// reference vectors are Up (Acc) and West (Acc cross Mag)
-// sjr 12/2020
-// gx, gy, gz must be in units of radians/second
-template<typename T>
-void Mahony<T>::update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat)
-{
-    // short name local variable for readability
-    T q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
-    T norm;
-    T hx, hy, hz;  //observed West vector W = AxM
-    T ux, uy, uz, wx, wy, wz; //calculated A (Up) and W in body frame
-    T ex, ey, ez;
-    T qa, qb, qc;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    T q1q1 = q1 * q1;
-    T q1q2 = q1 * q2;
-    T q1q3 = q1 * q3;
-    T q1q4 = q1 * q4;
-    T q2q2 = q2 * q2;
-    T q2q3 = q2 * q3;
-    T q2q4 = q2 * q4;
-    T q3q3 = q3 * q3;
-    T q3q4 = q3 * q4;
-    T q4q4 = q4 * q4;
-
-    // Compute feedback only if magnetometer measurement valid (avoids NaN in magnetometer normalisation)
-    T tmp = mx * mx + my * my + mz * mz;
-    if (tmp == 0.0f) {
-        update(q, ax, ay, az, gx, gy, gz, deltat);
-        return;
-    }
-    // Normalise magnetometer
-    norm = invSqrt(tmp);
-    mx *= norm;
-    my *= norm;
-    mz *= norm;
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    tmp = ax * ax + ay * ay + az * az;
-    if (tmp > 0.0f)
-    {
-        // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
-        norm = invSqrt(tmp);
-        ax *= norm;
-        ay *= norm;
-        az *= norm;
-
-        // Measured horizon vector = a x m (in body frame)
-        hx = ay * mz - az * my;
-        hy = az * mx - ax * mz;
-        hz = ax * my - ay * mx;
-
-        // Normalise horizon vector
-        norm = invSqrt(hx * hx + hy * hy + hz * hz);
-        hx *= norm;
-        hy *= norm;
-        hz *= norm;
-
-        // Estimated direction of Up reference vector
-        ux = 2.0f * (q2q4 - q1q3);
-        uy = 2.0f * (q1q2 + q3q4);
-        uz = q1q1 - q2q2 - q3q3 + q4q4;
-
-        // estimated direction of horizon (West) reference vector
-        wx = 2.0f * (q2q3 + q1q4);
-        wy = q1q1 - q2q2 + q3q3 - q4q4;
-        wz = 2.0f * (q3q4 - q1q2);
-
-        // Error is cross product between estimated direction and measured direction of the reference vectors
-        ex = (ay * uz - az * uy) + (hy * wz - hz * wy);
-        ey = (az * ux - ax * uz) + (hz * wx - hx * wz);
-        ez = (ax * uy - ay * ux) + (hx * wy - hy * wx);
-
-        // Compute and apply to gyro term the integral feedback, if enabled
-        if (Ki > 0.0f) {
-            ix += Ki * ex * deltat;  // integral error scaled by Ki
-            iy += Ki * ey * deltat;
-            iz += Ki * ez * deltat;
-            gx += ix;  // apply integral feedback
-            gy += iy;
-            gz += iz;
-        }
-
-        // Apply proportional feedback to gyro term
-        gx += Kp * ex;
-        gy += Kp * ey;
-        gz += Kp * ez;
-    }
-
-    // Integrate rate of change of quaternion
-    // small correction 1/11/2022, see https://github.com/kriswiner/MPU9250/issues/447
-    deltat *= 0.5f;
-    gx *= deltat;   // pre-multiply common factors
-    gy *= deltat;
-    gz *= deltat;
-    qa = q1;
-    qb = q2;
-    qc = q3;
-    q1 += (-qb * gx - qc * gy - q4 * gz);
-    q2 += (qa * gx + qc * gz - q4 * gy);
-    q3 += (qa * gy - qb * gz + q4 * gx);
-    q4 += (qa * gz + qb * gy - qc * gx);
-
-    // Normalise quaternion
-    norm = invSqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-    q[0] = q1 * norm;
-    q[1] = q2 * norm;
-    q[2] = q3 * norm;
-    q[3] = q4 * norm;
-}
-
-template<typename T>
-void Mahony<T>::update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat)
-{
-    // short name local variable for readability
-    T q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
-    T norm;
-    T vx, vy, vz;
-    T ex, ey, ez;  //error terms
-    T qa, qb, qc;
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    T tmp = ax * ax + ay * ay + az * az;
-    if (tmp > 0.0f)
-    {
-        // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
-        norm = invSqrt(tmp);
-        ax *= norm;
-        ay *= norm;
-        az *= norm;
-
-        // Estimated direction of gravity in the body frame (factor of two divided out)
-        vx = q2 * q4 - q1 * q3;
-        vy = q1 * q2 + q3 * q4;
-        vz = q1 * q1 - 0.5f + q4 * q4;
-
-        // Error is cross product between estimated and measured direction of gravity in body frame
-        // (half the actual magnitude)
-        ex = (ay * vz - az * vy);
-        ey = (az * vx - ax * vz);
-        ez = (ax * vy - ay * vx);
-
-        // Compute and apply to gyro term the integral feedback, if enabled
-        if (Ki > 0.0f) {
-            ix += Ki * ex * deltat;  // integral error scaled by Ki
-            iy += Ki * ey * deltat;
-            iz += Ki * ez * deltat;
-            gx += ix;  // apply integral feedback
-            gy += iy;
-            gz += iz;
-        }
-
-        // Apply proportional feedback to gyro term
-        gx += Kp * ex;
-        gy += Kp * ey;
-        gz += Kp * ez;
-    }
-
-    // Integrate rate of change of quaternion, q cross gyro term
-    deltat *= 0.5f;
-    gx *= deltat;   // pre-multiply common factors
-    gy *= deltat;
-    gz *= deltat;
-    qa = q1;
-    qb = q2;
-    qc = q3;
-    q1 += (-qb * gx - qc * gy - q4 * gz);
-    q2 += (qa * gx + qc * gz - q4 * gy);
-    q3 += (qa * gy - qb * gz + q4 * gx);
-    q4 += (qa * gz + qb * gy - qc * gx);
-
-    // normalise quaternion
-    norm = invSqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-    q[0] = q1 * norm;
-    q[1] = q2 * norm;
-    q[2] = q3 * norm;
-    q[3] = q4 * norm;
-}

lib/math/quat.cpp

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

lib/math/quat.h

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

lib/vqf/vqf.h

@@ -56,7 +56,7 @@ struct VQFParams {
 	 *
 	 * Default value: 3.0 s
 	 */
-	vqf_real_t tauAcc = 3.0;
+	vqf_real_t tauAcc = 4.337983;
 	/**
 	 * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds.
 	 *
@@ -106,7 +106,7 @@ struct VQFParams {
 	 *
 	 * Default value: 0.5 °/s
 	 */
-	vqf_real_t biasSigmaInit = 0.5;
+	vqf_real_t biasSigmaInit = 3.219453;
 	/**
 	 * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1
 	 * °/s (in seconds).
@@ -115,7 +115,7 @@ struct VQFParams {
 	 *
 	 * Default value: 100.0 s
 	 */
-	vqf_real_t biasForgettingTime = 100.0;
+	vqf_real_t biasForgettingTime = 136.579346;
 	/**
 	 * @brief Maximum expected gyroscope bias (in degrees per second).
 	 *
@@ -126,7 +126,7 @@ struct VQFParams {
 	 *
 	 * Default value: 2.0 °/s
 	 */
-	vqf_real_t biasClip = 2.0;
+	vqf_real_t biasClip = 5.0;
 #ifndef VQF_NO_MOTION_BIAS_ESTIMATION
 	/**
 	 * @brief Standard deviation of the converged bias estimation uncertainty during
@@ -137,7 +137,7 @@ struct VQFParams {
 	 *
 	 * Default value: 0.1 °/s
 	 */
-	vqf_real_t biasSigmaMotion = 0.1;
+	vqf_real_t biasSigmaMotion = 0.348501;
 	/**
 	 * @brief Forgetting factor for unobservable bias in vertical direction during
 	 * motion.
@@ -149,7 +149,7 @@ struct VQFParams {
 	 *
 	 * Default value: 0.0001
 	 */
-	vqf_real_t biasVerticalForgettingFactor = 0.0001;
+	vqf_real_t biasVerticalForgettingFactor = 0.007056;
 #endif
 	/**
 	 * @brief Standard deviation of the converged bias estimation uncertainty during
@@ -160,7 +160,7 @@ struct VQFParams {
 	 *
 	 * Default value: 0.03 °
 	 */
-	vqf_real_t biasSigmaRest = 0.03;
+	vqf_real_t biasSigmaRest = 0.063616;
 
 	/**
 	 * @brief Time threshold for rest detection (in seconds).
@@ -170,7 +170,7 @@ struct VQFParams {
 	 *
 	 * Default value: 1.5 s
 	 */
-	vqf_real_t restMinT = 1.5;
+	vqf_real_t restMinT = 2.586910;
 	/**
 	 * @brief Time constant for the low-pass filter used in rest detection (in seconds).
 	 *
@@ -179,7 +179,7 @@ struct VQFParams {
 	 *
 	 * Default value: 0.5 s
 	 */
-	vqf_real_t restFilterTau = 0.5;
+	vqf_real_t restFilterTau = 1.114532;
 	/**
 	 * @brief Angular velocity threshold for rest detection (in °/s).
 	 *
@@ -189,7 +189,7 @@ struct VQFParams {
 	 *
 	 * Default value: 2.0 °/s
 	 */
-	vqf_real_t restThGyr = 2.0;
+	vqf_real_t restThGyr = 1.399189;
 	/**
 	 * @brief Acceleration threshold for rest detection (in m/s²).
 	 *
@@ -198,7 +198,7 @@ struct VQFParams {
 	 *
 	 * Default value: 0.5 m/s²
 	 */
-	vqf_real_t restThAcc = 0.5;
+	vqf_real_t restThAcc = 1.418598;
 
 	/**
 	 * @brief Time constant for current norm/dip value in magnetic disturbance detection

platformio-tools.ini

@@ -1,100 +0,0 @@
-[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
-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_ES32C6DEVKITC1]
-platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
-build_flags =
-  ${env.build_flags}
-  -DESP32C6
-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
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-board = seeed_xiao_esp32c3

platformio.ini

@@ -13,6 +13,7 @@
 
 [platformio]
 build_cache_dir = cache
+default_envs = BOARD_WEMOSD1MINI
 
 [env]
 lib_deps=
@@ -26,7 +27,8 @@ 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
@@ -60,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:
@@ -102,23 +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
-;monitor_filters = colorize, esp32_exception_decoder
+[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
 
-; If you want to use a ESP32C6, you can use this (experimental)
-;[env:esp32c6]
-;platform =  https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
-;board = esp32-c6-devkitc-1
-;build_flags =
-;  ${env.build_flags}
-;  -DESP32C6
-;  -DARDUINO_USB_MODE=1
-;  -DARDUINO_USB_CDC_ON_BOOT=1
+[env:BOARD_ESP32C3DEVKITM1]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+custom_slime_board = BOARD_ESP32C3DEVKITM1
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = esp32-c3-devkitm-1
+monitor_filters = colorize, esp32_exception_decoder
+; If you want to use a ESP32C3, you can use this (experimental)
+; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards
+; There are 2 main Boardtypes:
+; 1. Boards that use a USB 2 Serial Chipset ( esp32-c3-devkitm-1, ttgo-t-oi-plus )
+; 2. Boards that relay on the USB interface of the ESP32C3 ( lolin_c3_mini , dfrobot_beetle_esp32c3)
+;    On this board there are 2 type some of them need to have set the build flag (menuconfig)
+;    -DARDUINO_USB_MODE=1
+;    -DARDUINO_USB_CDC_ON_BOOT=1
+
+[env:BOARD_ESP32C6DEVKITC1]
+platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
+custom_slime_board = BOARD_ESP32C6DEVKITC1
+build_flags =
+  ${env.build_flags}
+  -DESP32C6
+  -DARDUINO_USB_MODE=1
+  -DARDUINO_USB_CDC_ON_BOOT=1
+board = esp32-c6-devkitc-1
+
+[env:BOARD_XIAO_ESP32C3]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+custom_slime_board = BOARD_XIAO_ESP32C3
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = seeed_xiao_esp32c3
+monitor_filters = colorize, esp32_exception_decoder
+
+[env:BOARD_ESP32S3_SUPERMINI]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+custom_slime_board = BOARD_ESP32S3_SUPERMINI
+build_flags = 
+  ${env.build_flags}
+  -DARDUINO_USB_MODE=1
+  -DESP32S3
+board = esp32s3_supermini
+board_upload.use_1200bps_touch = 1
+board_upload.wait_for_upload_port = 1
+board_upload.require_upload_port = 1
+upload_speed = 921600
+
+[env:BOARD_SLIMEVR]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_SLIMEVR
+build_flags =
+  ${env.build_flags}
+  -D VENDOR_NAME='"SlimeVR"'
+  -D VENDOR_URL='"https://slimevr.dev"'
+  -D PRODUCT_NAME='"SlimeVR Tracker"'
+  -D UPDATE_ADDRESS='"SlimeVR/SlimeVR-Tracker-ESP"'
+  -D UPDATE_NAME='"BOARD_SLIMEVR-firmware"'
+
+[env:BOARD_SLIMEVR_V1_2]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_SLIMEVR_V1_2
+build_flags =
+  ${env.build_flags}
+  -D VENDOR_NAME='"SlimeVR"'
+  -D VENDOR_URL='"https://slimevr.dev"'
+  -D PRODUCT_NAME='"SlimeVR Tracker v1.2"'
+  -D UPDATE_ADDRESS='"SlimeVR/SlimeVR-Tracker-ESP"'
+  -D UPDATE_NAME='"BOARD_SLIMEVR_V1_2-firmware"'
+
+[env:BOARD_SLIMEVR_DEV]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_SLIMEVR_DEV
+build_flags =
+  ${env.build_flags}
+  -D VENDOR_NAME='"SlimeVR"'
+  -D PRODUCT_NAME='"SlimeVR Tracker (dev)"'
+
+[env:BOARD_GLOVE_IMU_SLIMEVR_DEV]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_GLOVE_IMU_SLIMEVR_DEV
+  -DESP32C3
+  -D PRODUCT_NAME='"SlimeVR Glove (dev)"'
+board = lolin_c3_mini
+monitor_filters = colorize, esp32_exception_decoder

scripts/get_git_commit.py

@@ -36,7 +36,10 @@ except Exception:
 
 output = f"-DGIT_REV='\"{revision}\"'"
 
-if 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}\"'"

scripts/preprocessor.py

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

src/FSHelper.cpp

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

src/FSHelper.h

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

src/GlobalVars.h

@@ -20,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;

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

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

src/boards/boards_default.h

@@ -0,0 +1,110 @@
+/*
+ * LED configuration:
+ * Configuration Priority 1 = Highest:
+ * 1. LED_PIN
+ * 2. LED_BUILTIN
+ *
+ *   LED_PIN
+ *     - Number or Symbol (D1,..) of the Output
+ *     - To turn off the LED, set LED_PIN to LED_OFF
+ *   LED_INVERTED
+ *     - false for output 3.3V on high
+ *     - true for pull down to GND on high
+ */
+
+/*
+ * D1 Mini boards with ESP8266 have internal resistors. For these boards you only have
+ * to adjust BATTERY_SHIELD_RESISTANCE. For other boards you can now adjust the other
+ * resistor values. The diagram looks like this:
+ *   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2]
+ *   ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
+ * BATTERY_SHIELD_R(180)
+ * 130k BatteryShield, 180k SlimeVR or fill in
+ * external resistor value in kOhm BATTERY_R1(100)
+ * Board voltage divider resistor Ain to GND in kOhm BATTERY_R2(220)
+ * Board voltage divider resistor Ain to INPUT_BOARD in kOhm
+ */
+
+#include "defines_helpers.h"
+
+// Default IMU pinouts and definitions for default tracker types
+
+#if BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV
+// Defaunlt definitions for normal 2-sensor trackers
+#ifndef MAX_SENSORS_COUNT
+#define MAX_SENSORS_COUNT 2
+#endif
+#ifndef TRACKER_TYPE
+#define TRACKER_TYPE TrackerType::TRACKER_TYPE_SVR_ROTATION
+#endif
+
+// Axis mapping example
+/*
+#include "sensors/axisremap.h"
+#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN,
+AXIS_REMAP_USE_Z, \ AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)
+
+SENSOR_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL,
+PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
+*/
+
+#ifndef SENSOR_DESC_LIST
+#if BOARD == BOARD_SLIMEVR_V1_2
+#define SENSOR_DESC_LIST                             \
+	SENSOR_DESC_ENTRY(                               \
+		IMU,                                         \
+		DIRECT_PIN(15),                              \
+		IMU_ROTATION,                                \
+		DIRECT_SPI(24'000'000, MSBFIRST, SPI_MODE3), \
+		PRIMARY_IMU_OPTIONAL,                        \
+		DIRECT_PIN(PIN_IMU_INT),                     \
+		0                                            \
+	)                                                \
+	SENSOR_DESC_ENTRY(                               \
+		SECOND_IMU,                                  \
+		SECONDARY_IMU_ADDRESS_TWO,                   \
+		SECOND_IMU_ROTATION,                         \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA),       \
+		SECONDARY_IMU_OPTIONAL,                      \
+		DIRECT_PIN(PIN_IMU_INT_2),                   \
+		0                                            \
+	)
+#else
+#define SENSOR_DESC_LIST                       \
+	SENSOR_DESC_ENTRY(                         \
+		IMU,                                   \
+		PRIMARY_IMU_ADDRESS_ONE,               \
+		IMU_ROTATION,                          \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \
+		PRIMARY_IMU_OPTIONAL,                  \
+		DIRECT_PIN(PIN_IMU_INT),               \
+		0                                      \
+	)                                          \
+	SENSOR_DESC_ENTRY(                         \
+		SECOND_IMU,                            \
+		SECONDARY_IMU_ADDRESS_TWO,             \
+		SECOND_IMU_ROTATION,                   \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \
+		SECONDARY_IMU_OPTIONAL,                \
+		DIRECT_PIN(PIN_IMU_INT_2),             \
+		0                                      \
+	)
+#endif
+#endif
+#else  // BOARD == BOARD_GLOVE_IMU_SLIMEVR_DEV
+
+SDA(1)
+SCL(0)
+#define PCA_ADDR 0x70
+INT(16)
+INT2(13)
+BATTERY(3)
+LED(2)
+INVERTED_LED(true)
+BATTERY_SHIELD_R(0)
+BATTERY_R1(10)
+BATTERY_R2(40.2)
+
+#include "glove_default.h"
+
+#endif  // BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV

src/boards/defines_helpers.cpp

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

src/boards/defines_helpers.h

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

src/boards/glove_default.h

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

src/configuration/Configuration.cpp

@@ -25,16 +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 "/toggles"
+#define DIR_TOGGLES_OLD "/toggles"
+#define DIR_TOGGLES "/sensortoggles"
 
-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 void Configuration::setup() {
 	if (m_Loaded) {
 		return;
@@ -119,13 +123,21 @@ void Configuration::save() {
 		file.write((uint8_t*)&config, sizeof(SensorConfig));
 		file.close();
 
-		sprintf(path, DIR_TOGGLES "/%zu", i);
+		if (i < m_SensorToggles.size()) {
+			sprintf(path, DIR_TOGGLES "/%zu", i);
 
-		m_Logger.trace("Saving sensor toggle state for %d", i);
+			m_Logger.trace("Saving sensor toggle state for %d", i);
 
-		file = LittleFS.open(path, "w");
-		file.write((uint8_t*)&m_SensorToggles[i], sizeof(SensorToggleState));
-		file.close();
+			file = 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
+			);
+		}
 	}
 
 	{
@@ -134,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");
 }
 
@@ -227,14 +251,34 @@ void Configuration::loadSensors() {
 		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) {
-		SensorToggleState sensorToggleState;
-		f.read((uint8_t*)&sensorToggleState, sizeof(SensorToggleState));
+		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);
+		setSensorToggles(sensorId, SensorToggleState{values});
 	});
 }
 
@@ -442,5 +486,4 @@ void Configuration::print() {
 		}
 	}
 }
-}  // namespace Configuration
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Configuration

src/configuration/Configuration.h

@@ -31,8 +31,7 @@
 #include "DeviceConfig.h"
 #include "logging/Logger.h"
 
-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 class Configuration {
 public:
 	void setup();
@@ -72,7 +71,6 @@ private:
 
 	Logging::Logger m_Logger = Logging::Logger("Configuration");
 };
-}  // namespace Configuration
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Configuration
 
 #endif

src/configuration/SensorConfig.cpp

@@ -23,8 +23,7 @@
 
 #include "SensorConfig.h"
 
-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 const char* calibrationConfigTypeToString(SensorConfigType type) {
 	switch (type) {
 		case SensorConfigType::NONE:
@@ -60,5 +59,4 @@ bool SensorConfigBits::operator!=(const SensorConfigBits& rhs) const {
 	return !(*this == rhs);
 }
 
-}  // namespace Configuration
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Configuration

src/configuration/SensorConfig.h

@@ -28,8 +28,7 @@
 
 #include "consts.h"
 
-namespace SlimeVR {
-namespace Configuration {
+namespace SlimeVR::Configuration {
 struct BMI160SensorConfig {
 	// accelerometer offsets and correction matrix
 	float A_B[3];
@@ -196,7 +195,6 @@ struct SensorConfigBits {
 // If this fails, you forgot to do the above
 static_assert(sizeof(SensorConfigBits) == 2);
 
-}  // namespace Configuration
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Configuration
 
 #endif

src/consts.h

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

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,7 +96,7 @@
 // Not recommended for production
 #define ENABLE_INSPECTION false
 
-#define PROTOCOL_VERSION 20
+#define PROTOCOL_VERSION 22
 
 #ifndef FIRMWARE_VERSION
 #define FIRMWARE_VERSION "UNKNOWN"

src/defines.h

@@ -26,362 +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
+#endif
 
+#ifndef PRIMARY_IMU_OPTIONAL
 #define PRIMARY_IMU_OPTIONAL false
+#endif
+#ifndef SECONDARY_IMU_OPTIONAL
 #define SECONDARY_IMU_OPTIONAL true
+#endif
 
-#if BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV
-#define MAX_SENSORS_COUNT 2
-#define TRACKER_TYPE TrackerType::TRACKER_TYPE_SVR_ROTATION
 // 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
 
-// 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
-#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
-
-#else
-
-// Predefines for the GLOVE
-#ifndef SENSOR_DESC_LIST
-#define MAX_SENSORS_COUNT 10
-#define TRACKER_TYPE TrackerType::TRACKER_TYPE_SVR_GLOVE_LEFT
-#define GLOVE_SIDE GloveSide::GLOVE_LEFT
-#define PRIMARY_IMU_ADDRESS_ONE 0x4a
-#define SECONDARY_IMU_ADDRESS_TWO 0x4b
-
-#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                                                \
-	)
-
-#if GLOVE_SIDE == GloveSide::GLOVE_LEFT
-#define SENSOR_INFO_LIST                                                               \
-	SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_LEFT_HAND)                           \
-	SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_LEFT_LITTLE_INTERMEDIATE)            \
-	SENSOR_INFO_ENTRY(2, SensorPosition::POSITION_LEFT_RING_INTERMEDIATE)              \
-	SENSOR_INFO_ENTRY(3, SensorPosition::POSITION_LEFT_RING_DISTAL)                    \
-	SENSOR_INFO_ENTRY(4, SensorPosition::POSITION_LEFT_MIDDLE_INTERMEDIATE)            \
-	SENSOR_INFO_ENTRY(5, SensorPosition::POSITION_LEFT_MIDDLE_DISTAL)                  \
-	SENSOR_INFO_ENTRY(6, SensorPosition::POSITION_LEFT_INDEX_INTERMEDIATE)             \
-	SENSOR_INFO_ENTRY(7, SensorPosition::POSITION_LEFT_INDEX_DISTAL)                   \
-	SENSOR_INFO_ENTRY(8, SensorPosition::SensorPosition::POSITION_LEFT_THUMB_PROXIMAL) \
-	SENSOR_INFO_ENTRY(9, SensorPosition::POSITION_LEFT_THUMB_DISTAL)
-#elif GLOVE_SDIE == GloveSide::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_SDIE
-#error "Glove side not defined"
-#endif  // GLOVE_SDIE
-
-#endif  // SENSOR_DESC_LIST
-#endif  // BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV
-
+#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 || BOARD == BOARD_ES32C6DEVKITC1
-#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
-#elif BOARD == BOARD_GLOVE_IMU_SLIMEVR_DEV
-#define PIN_IMU_SDA 1
-#define PIN_IMU_SCL 0
-#define PCA_ADDR 0x70
-#define PIN_IMU_INT 16
-#define PIN_IMU_INT_2 13
-#define PIN_BATTERY_LEVEL 3
-#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
-#endif
+// ------------------------------

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

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

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,39 +44,6 @@
 #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
-#endif
-
-#if !defined(LED_INVERTED)
-// default is inverted for SlimeVR / ESP-12E
-#define LED_INVERTED true
-#endif
-
-#if LED_INVERTED
-#define LED__ON LOW
-#define LED__OFF HIGH
-#else
-#define LED__ON HIGH
-#define LED__OFF LOW
-#endif
-
 #ifndef SENSOR_INFO_LIST
 #define SENSOR_INFO_LIST
 #endif
@@ -85,4 +53,26 @@
 #define EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION true
 #endif
 
-#endif  // SLIMEVR_GLOBALS_H_
+#ifndef IMU_USE_EXTERNAL_CLOCK
+#define IMU_USE_EXTERNAL_CLOCK true  // Use external clock for IMU (ICM-45686 only)
+#endif
+
+#ifndef VENDOR_NAME
+#define VENDOR_NAME "Unknown"
+#endif
+
+#ifndef VENDOR_URL
+#define VENDOR_URL ""
+#endif
+
+#ifndef PRODUCT_NAME
+#define PRODUCT_NAME "DIY SlimeVR Tracker"
+#endif
+
+#ifndef UPDATE_ADDRESS
+#define UPDATE_ADDRESS ""
+#endif
+
+#ifndef UPDATE_NAME
+#define UPDATE_NAME ""
+#endif

src/logging/Level.cpp

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

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

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

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

src/main.cpp

@@ -37,11 +37,13 @@
 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"};
@@ -66,6 +68,38 @@ void setup() {
 
 	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();
@@ -78,12 +112,12 @@ void setup() {
 	// this, check needs to be re-added.
 	auto clearResult = I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL);
 	if (clearResult != 0) {
-		logger.error("Can't clear I2C bus, error %d", clearResult);
+		logger.warn("Can't clear I2C bus, error %d", clearResult);
 	}
 
 	// join I2C bus
 
-#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();

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

src/network/connection.cpp

@@ -23,6 +23,8 @@
 
 #include "connection.h"
 
+#include <string_view>
+
 #include "GlobalVars.h"
 #include "logging/Logger.h"
 #include "packets.h"
@@ -163,10 +165,14 @@ 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;
 }
@@ -373,6 +379,16 @@ void Connection::sendTrackerDiscovery() {
 			// 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
@@ -604,6 +620,9 @@ void Connection::reset() {
 
 	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);
 }
 
@@ -634,6 +653,9 @@ void Connection::update() {
 		);
 		m_Logger.warn("Connection to server timed out");
 
+		// Reset server address to broadcast if disconnected
+		m_ServerHost = IPAddress(255, 255, 255, 255);
+
 		return;
 	}
 
@@ -642,7 +664,6 @@ void Connection::update() {
 		return;
 	}
 
-	m_LastPacketTimestamp = millis();
 	int len = m_UDP.read(m_Packet, sizeof(m_Packet));
 
 #ifdef DEBUG_NETWORK
@@ -657,6 +678,12 @@ void Connection::update() {
 	(void)packetSize;
 #endif
 
+	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();
@@ -666,8 +693,7 @@ void Connection::update() {
 			break;
 
 		case ReceivePacketType::Handshake:
-			// Assume handshake successful
-			m_Logger.warn("Handshake received again, ignoring");
+			// handled above
 			break;
 
 		case ReceivePacketType::Command:

src/network/connection.h

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

src/network/manager.cpp

@@ -24,17 +24,16 @@
 
 #include "GlobalVars.h"
 
-namespace SlimeVR {
-namespace Network {
+namespace SlimeVR::Network {
 
-void Manager::setup() { ::WiFiNetwork::setUp(); }
+void Manager::setup() { wifiNetwork.setUp(); }
 
 void Manager::update() {
-	WiFiNetwork::upkeep();
+	wifiNetwork.upkeep();
 
 	auto wasConnected = m_IsConnected;
 
-	m_IsConnected = ::WiFiNetwork::isConnected();
+	m_IsConnected = wifiNetwork.isConnected();
 
 	if (!m_IsConnected) {
 		return;
@@ -48,5 +47,4 @@ void Manager::update() {
 	networkConnection.update();
 }
 
-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network

src/network/manager.h

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

src/network/packets.h

@@ -54,6 +54,7 @@ enum class SendPacketType : uint8_t {
 	// RotationAcceleration = 23,
 	AcknowledgeConfigChange = 24,
 	FlexData = 26,
+	// PositionData = 27,
 	Bundle = 100,
 	Inspection = 105,
 };

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()
+		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(),
+		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

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

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

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_

src/sensorinterface/DirectPinInterface.h

@@ -39,6 +39,11 @@ public:
 	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;
 };

src/sensorinterface/DirectSPIInterface.cpp

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

src/sensorinterface/DirectSPIInterface.h

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

src/sensorinterface/I2CPCAInterface.cpp

@@ -32,7 +32,7 @@ void SlimeVR::I2CPCASensorInterface::swapIn() {
 	Wire.beginTransmission(m_Address);
 	Wire.write(1 << m_Channel);
 	Wire.endTransmission();
-#if ESP32
+#ifdef ESP32
 	// On ESP32 we need to reconnect to I2C bus for some reason
 	m_Wire.disconnect();
 	m_Wire.swapIn();

src/sensorinterface/I2CPCAInterface.h

@@ -46,6 +46,11 @@ public:
 	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;

src/sensorinterface/I2CWireSensorInterface.cpp

@@ -25,7 +25,7 @@
 
 #include <optional>
 
-#if ESP32
+#ifdef ESP32
 #include "driver/i2c.h"
 #endif
 
@@ -37,7 +37,7 @@ namespace SlimeVR {
 void swapI2C(uint8_t sclPin, uint8_t sdaPin) {
 	if (sclPin != activeSCLPin || sdaPin != activeSDAPin || !isI2CActive) {
 		Wire.flush();
-#if ESP32
+#ifdef ESP32
 		if (!isI2CActive) {
 			// Reset HWI2C to avoid being affected by I2CBUS reset
 			Wire.end();
@@ -68,7 +68,7 @@ void swapI2C(uint8_t sclPin, uint8_t sdaPin) {
 void disconnectI2C() {
 	Wire.flush();
 	isI2CActive = false;
-#if ESP32
+#ifdef ESP32
 	Wire.end();
 #endif
 }

src/sensorinterface/I2CWireSensorInterface.h

@@ -49,6 +49,12 @@ public:
 	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;

src/sensorinterface/MCP23X17PinInterface.h

@@ -56,6 +56,11 @@ public:
 	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;

src/sensorinterface/RegisterInterface.cpp

@@ -0,0 +1,45 @@
+/* SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "RegisterInterface.h"
+
+namespace SlimeVR::Sensors {
+
+[[nodiscard]] uint8_t EmptyRegisterInterface::readReg(uint8_t regAddr) const {
+	return 0;
+};
+[[nodiscard]] uint16_t EmptyRegisterInterface::readReg16(uint8_t regAddr) const {
+	return 0;
+};
+void EmptyRegisterInterface::writeReg(uint8_t regAddr, uint8_t value) const {};
+void EmptyRegisterInterface::writeReg16(uint8_t regAddr, uint16_t value) const {};
+void EmptyRegisterInterface::readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer)
+	const {};
+void EmptyRegisterInterface::writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer)
+	const {};
+[[nodiscard]] uint8_t EmptyRegisterInterface::getAddress() const { return 0; };
+bool EmptyRegisterInterface::hasSensorOnBus() { return true; };
+[[nodiscard]] std::string EmptyRegisterInterface::toString() const { return "Empty"; };
+
+EmptyRegisterInterface EmptyRegisterInterface::instance;
+
+}  // namespace SlimeVR::Sensors

src/sensorinterface/RegisterInterface.h

@@ -0,0 +1,59 @@
+/* SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Eiren Rain & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+#include "I2Cdev.h"
+
+namespace SlimeVR::Sensors {
+
+struct RegisterInterface {
+	static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2;
+
+	[[nodiscard]] virtual uint8_t readReg(uint8_t regAddr) const = 0;
+	[[nodiscard]] virtual uint16_t readReg16(uint8_t regAddr) const = 0;
+	virtual void writeReg(uint8_t regAddr, uint8_t value) const = 0;
+	virtual void writeReg16(uint8_t regAddr, uint16_t value) const = 0;
+	virtual void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0;
+	virtual void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0;
+	[[nodiscard]] virtual uint8_t getAddress() const = 0;
+	virtual bool hasSensorOnBus() = 0;
+	[[nodiscard]] virtual std::string toString() const = 0;
+};
+
+struct EmptyRegisterInterface : public RegisterInterface {
+	[[nodiscard]] uint8_t readReg(uint8_t regAddr) const final;
+	[[nodiscard]] uint16_t readReg16(uint8_t regAddr) const final;
+	void writeReg(uint8_t regAddr, uint8_t value) const final;
+	void writeReg16(uint8_t regAddr, uint16_t value) const final;
+	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const final;
+	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const final;
+	[[nodiscard]] uint8_t getAddress() const final;
+	bool hasSensorOnBus() final;
+	[[nodiscard]] std::string toString() const final;
+
+	static EmptyRegisterInterface instance;
+};
+
+}  // namespace SlimeVR::Sensors

src/sensorinterface/SPIImpl.h

@@ -0,0 +1,130 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Eiren Rain & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#pragma once
+
+#include <PinInterface.h>
+#include <SPI.h>
+
+#include <cstdint>
+
+#include "../logging/Logger.h"
+#include "DirectSPIInterface.h"
+#include "RegisterInterface.h"
+
+#define ICM_READ_FLAG 0x80
+
+namespace SlimeVR::Sensors {
+
+struct SPIImpl : public RegisterInterface {
+	SPIImpl(DirectSPIInterface* spi, PinInterface* csPin)
+		: m_spi(spi)
+		, m_csPin(csPin) {
+		auto& spiSettings = spi->getSpiSettings();
+		m_Logger.info(
+			"SPI settings: clock: %d, bit order: 0x%02X, data mode: 0x%02X",
+			spiSettings._clock,
+			spiSettings._bitOrder,
+			spiSettings._dataMode
+		);
+		csPin->pinMode(OUTPUT);
+		csPin->digitalWrite(HIGH);
+	}
+
+	uint8_t readReg(uint8_t regAddr) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr | ICM_READ_FLAG);
+		uint8_t buffer = m_spi->transfer(0);
+
+		m_spi->endTransaction(m_csPin);
+
+		return buffer;
+	}
+
+	uint16_t readReg16(uint8_t regAddr) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr | ICM_READ_FLAG);
+		uint8_t b1 = m_spi->transfer(0);
+		uint8_t b2 = m_spi->transfer(0);
+
+		m_spi->endTransaction(m_csPin);
+		return b2 << 8 | b1;
+	}
+
+	void writeReg(uint8_t regAddr, uint8_t value) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr);
+		m_spi->transfer(value);
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	void writeReg16(uint8_t regAddr, uint16_t value) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr);
+		m_spi->transfer(value & 0xFF);
+		m_spi->transfer(value >> 8);
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr | ICM_READ_FLAG);
+		for (uint8_t i = 0; i < size; ++i) {
+			buffer[i] = m_spi->transfer(0);
+		}
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
+		m_spi->beginTransaction(m_csPin);
+
+		m_spi->transfer(regAddr);
+		for (uint8_t i = 0; i < size; ++i) {
+			m_spi->transfer(buffer[i]);
+		}
+
+		m_spi->endTransaction(m_csPin);
+	}
+
+	bool hasSensorOnBus() override {
+		return true;  // TODO
+	}
+
+	uint8_t getAddress() const override { return 0; }
+
+	std::string toString() const override { return std::string("SPI"); }
+
+private:
+	DirectSPIInterface* m_spi;
+	PinInterface* m_csPin;
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("SPI");
+};
+
+}  // namespace SlimeVR::Sensors

src/sensorinterface/SensorInterface.cpp

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

src/sensorinterface/SensorInterface.h

@@ -24,11 +24,14 @@
 #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 {
@@ -36,6 +39,9 @@ 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
 

src/sensorinterface/SensorInterfaceManager.cpp

@@ -0,0 +1,46 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "SensorInterfaceManager.h"
+
+template <typename T>
+bool byteCompare(const T& lhs, const T& rhs) {
+	const auto* lhsBytes = reinterpret_cast<const uint8_t*>(&lhs);
+	const auto* rhsBytes = reinterpret_cast<const uint8_t*>(&rhs);
+
+	for (size_t i = 0; i < sizeof(T); i++) {
+		if (lhsBytes[i] < rhsBytes[i]) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool operator<(const SPISettings& lhs, const SPISettings& rhs) {
+	return byteCompare(lhs, rhs);
+}
+
+bool operator<(const SPIClass& lhs, const SPIClass& rhs) {
+	return byteCompare(lhs, rhs);
+}

src/sensorinterface/SensorInterfaceManager.h

@@ -23,6 +23,9 @@
 
 #pragma once
 
+#include <PinInterface.h>
+#include <SPI.h>
+
 #include <functional>
 #include <map>
 #include <optional>
@@ -31,6 +34,17 @@
 #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 {
 
@@ -52,7 +66,15 @@ private:
 
 			if (!cache.contains(key)) {
 				auto ptr = new InterfaceClass(args...);
-				if (!ptr->init()) {
+
+				bool success;
+				if constexpr (requires { ptr->init(); }) {
+					success = ptr->init();
+				} else {
+					success = true;
+				}
+
+				if (!success) {
 					cache[key] = nullptr;
 					return nullptr;
 				}
@@ -72,14 +94,20 @@ public:
 	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, int> mcpPinInterfaces;
+	SensorInterface<MCP23X17PinInterface, Adafruit_MCP23X17*, int> mcpPinInterfaces;
 	SensorInterface<I2CWireSensorInterface, int, int> i2cWireInterfaces;
 	SensorInterface<I2CPCASensorInterface, int, int, int, int> pcaWireInterfaces;
+	SensorInterface<Sensors::I2CImpl, uint8_t> i2cImpls;
+	SensorInterface<DirectSPIInterface, SPIClass, SPISettings> directSPIInterfaces;
+	SensorInterface<Sensors::SPIImpl, DirectSPIInterface*, PinInterface*> spiImpls;
 };
 
 }  // namespace SlimeVR

src/sensorinterface/i2cimpl.h

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

src/sensors/ADCResistanceSensor.cpp

@@ -24,12 +24,13 @@
 
 #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 ESP32
+#elif defined(ESP32)
 	float voltage = ((float)analogReadMilliVolts(m_Pin)) / 1000;
 	m_Data = m_ResistanceDivider
 		   * (m_VCC / voltage - 1.0f);  // Convert voltage to resistance
@@ -39,3 +40,5 @@ void ADCResistanceSensor::motionLoop() {
 void ADCResistanceSensor::sendData() {
 	networkConnection.sendFlexData(sensorId, m_Data);
 }
+
+}  // namespace SlimeVR::Sensors

src/sensors/ADCResistanceSensor.h

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

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", SensorTypeID::Empty, id, 0, 0.0){};
+		: Sensor(
+			"EmptySensor",
+			SensorTypeID::Empty,
+			id,
+			EmptyRegisterInterface::instance,
+			0.0
+		){};
 	~EmptySensor(){};
 
 	void motionSetup() override final{};
@@ -42,7 +48,6 @@ public:
 		return SensorStatus::SENSOR_OFFLINE;
 	};
 };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors
 
 #endif

src/sensors/ErroneousSensor.cpp

@@ -25,8 +25,7 @@
 
 #include "GlobalVars.h"
 
-namespace SlimeVR {
-namespace Sensors {
+namespace SlimeVR::Sensors {
 void ErroneousSensor::motionSetup() {
 	m_Logger.error(
 		"IMU of type %s failed to initialize",
@@ -37,5 +36,4 @@ void ErroneousSensor::motionSetup() {
 }
 
 SensorStatus ErroneousSensor::getSensorState() { return SensorStatus::SENSOR_ERROR; };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors

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, SensorTypeID type)
-		: Sensor("ErroneousSensor", type, id, 0, 0.0)
+		: Sensor("ErroneousSensor", type, id, EmptyRegisterInterface::instance, 0.0)
 		, m_ExpectedType(type){};
 	~ErroneousSensor(){};
 
@@ -44,7 +44,6 @@ public:
 private:
 	SensorTypeID m_ExpectedType;
 };
-}  // namespace Sensors
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Sensors
 
 #endif

src/sensors/RestCalibrationDetector.cpp

@@ -23,9 +23,11 @@
 
 #include "RestCalibrationDetector.h"
 
+#include "sensors/SensorFusion.h"
+
 namespace SlimeVR::Sensors {
 
-bool RestCalibrationDetector::update(SensorFusionRestDetect& fusion) {
+bool RestCalibrationDetector::update(SensorFusion& fusion) {
 	if (state == CalibrationState::Done) {
 		return false;
 	}

src/sensors/RestCalibrationDetector.h

@@ -25,13 +25,13 @@
 
 #include <cstdint>
 
-#include "SensorFusionRestDetect.h"
+#include "sensors/SensorFusion.h"
 
 namespace SlimeVR::Sensors {
 
 class RestCalibrationDetector {
 public:
-	bool update(SensorFusionRestDetect& fusion);
+	bool update(SensorFusion& fusion);
 
 private:
 	static constexpr float restCalibrationSeconds = 3.0f;

src/sensors/SensorBuilder.cpp

@@ -0,0 +1,126 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Eiren Rain, Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "SensorBuilder.h"
+
+namespace SlimeVR::Sensors {
+
+SensorBuilder::SensorBuilder(SensorManager* sensorManager)
+	: m_Manager(sensorManager) {}
+
+#define SENSOR_DESC_ENTRY(ImuType, ...)                                           \
+	activeSensorCount += sensorDescEntry<ImuType>(sensorID, __VA_ARGS__) ? 1 : 0; \
+	sensorID++;
+
+#define SENSOR_INFO_ENTRY(ImuID, SensorPosition) \
+	m_Manager->m_Sensors[ImuID]->setSensorInfo(SensorPosition);
+
+uint8_t SensorBuilder::buildAllSensors() {
+	uint8_t sensorID = 0;
+	uint8_t activeSensorCount = 0;
+
+	[[maybe_unused]] const auto NO_PIN = nullptr;
+	[[maybe_unused]] const auto NO_WIRE = &EmptySensorInterface::instance;
+	[[maybe_unused]] const auto DIRECT_PIN
+		= [&](uint8_t pin) { return interfaceManager.directPinInterface().get(pin); };
+	[[maybe_unused]] const auto DIRECT_WIRE = [&](uint8_t scl, uint8_t sda) {
+		return interfaceManager.i2cWireInterface().get(scl, sda);
+	};
+	[[maybe_unused]] const auto MCP_PIN = [&](uint8_t pin) {
+		return interfaceManager.mcpPinInterface().get(&m_Manager->m_MCP, pin);
+	};
+	[[maybe_unused]] const auto PCA_WIRE
+		= [&](uint8_t scl, uint8_t sda, uint8_t addr, uint8_t ch) {
+			  return interfaceManager.pcaWireInterface().get(scl, sda, addr, ch);
+		  };
+	[[maybe_unused]] const auto DIRECT_SPI
+		= [&](uint32_t clockFreq, uint8_t bitOrder, uint8_t dataMode) {
+			  return interfaceManager.directSPIInterface().get(
+				  SPI,
+				  SPISettings(clockFreq, bitOrder, dataMode)
+			  );
+		  };
+
+	// Apply descriptor list and expand to entries
+	SENSOR_DESC_LIST
+
+	// Apply sensor info list and expand to entries
+	SENSOR_INFO_LIST
+
+	return activeSensorCount;
+}
+
+std::unique_ptr<::Sensor>
+SensorBuilder::buildSensorDynamically(SensorTypeID type, SensorDefinition sensorDef) {
+	switch (type) {
+		// case SensorTypeID::MPU9250:
+		//	return buildSensor<MPU9250Sensor>(sensorDef);
+		// case SensorTypeID::BNO080:
+		//	return buildSensor<BNO080Sensor>(sensorDef);
+		case SensorTypeID::BNO085:
+			return buildSensor<BNO085Sensor>(sensorDef);
+		// case SensorTypeID::BNO055:
+		//	return buildSensor<BNO055Sensor>(sensorDef);
+		// case SensorTypeID::MPU6050:
+		//	return buildSensor<SoftFusionMPU6050>(
+		//		sensorDef
+		//	);
+		// case SensorTypeID::BNO086:
+		//	return buildSensor<BNO086Sensor>(sensorDef);
+		// case SensorTypeID::BMI160:
+		// 	return buildSensor<BMI160Sensor>(sensorDef);
+		// case SensorTypeID::ICM20948:
+		//	return buildSensor<ICM20948Sensor>(sensorDef);
+		// case SensorTypeID::ICM42688:
+		//	return buildSensor<SoftFusionICM42688>(
+		//		sensorDef
+		//	);
+		case SensorTypeID::BMI270:
+			return buildSensor<SoftFusionBMI270>(sensorDef);
+		// case SensorTypeID::LSM6DS3TRC:
+		//	return buildSensor<SoftFusionLSM6DS3TRC>(
+		//		sensorDef
+		//	);
+		case SensorTypeID::LSM6DSV:
+			return buildSensor<SoftFusionLSM6DSV>(sensorDef);
+		case SensorTypeID::LSM6DSO:
+			return buildSensor<SoftFusionLSM6DSO>(sensorDef);
+		case SensorTypeID::LSM6DSR:
+			return buildSensor<SoftFusionLSM6DSR>(sensorDef);
+		case SensorTypeID::ICM45686:
+			return buildSensor<SoftFusionICM45686>(sensorDef);
+		// case SensorTypeID::ICM45605:
+		//	return buildSensor<SoftFusionICM45605>(
+		//		sensorDef
+		//	);
+		default:
+			m_Manager->m_Logger.error(
+				"Unable to create sensor with type %s (%d)",
+				getIMUNameByType(type),
+				static_cast<int>(type)
+			);
+	}
+	return std::make_unique<EmptySensor>(sensorDef.sensorID);
+}
+
+}  // namespace SlimeVR::Sensors

src/sensors/SensorBuilder.h

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

src/sensors/SensorFusion.cpp

@@ -1,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;
 }
@@ -212,11 +125,10 @@ void SensorFusion::calcLinearAcc(
 	accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
 }
 
-#if SENSOR_USE_VQF
 void SensorFusion::updateBiasForgettingTime(float biasForgettingTime) {
 	vqf.updateBiasForgettingTime(biasForgettingTime);
 }
-#endif
 
-}  // namespace Sensors
-}  // namespace SlimeVR
+bool SensorFusion::getRestDetected() const { return vqf.getRestDetected(); }
+
+}  // namespace SlimeVR::Sensors

src/sensors/SensorFusion.h

@@ -6,45 +6,19 @@
 
 #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
+namespace SlimeVR::Sensors {
 constexpr VQFParams DefaultVQFParams = VQFParams{
 	.tauAcc = 2.0f,
 	.restMinT = 2.0f,
 	.restThGyr = 0.6f,
 	.restThAcc = 0.06f,
 };
-#endif
 
 class SensorFusion {
 public:
@@ -58,18 +32,10 @@ public:
 		, accTs((accTs < 0) ? gyrTs : accTs)
 		, magTs((magTs < 0) ? gyrTs : magTs)
 		, vqfParams(vqfParams)
-#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(this->vqfParams,
 			  gyrTs,
 			  ((accTs < 0) ? gyrTs : accTs),
-			  ((magTs < 0) ? gyrTs : magTs))
-#endif
-	{
-	}
+			  ((magTs < 0) ? gyrTs : magTs)) {}
 
 	explicit SensorFusion(
 		sensor_real_t gyrTs,
@@ -109,35 +75,21 @@ public:
 		sensor_real_t accout[3]
 	);
 
-#if SENSOR_USE_VQF
 	void updateBiasForgettingTime(float biasForgettingTime);
-#endif
+
+	[[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
 	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;
@@ -146,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

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

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

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

src/sensors/SensorFusionRestDetect.h

@@ -1,66 +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
-	{
-	}
-
-#if SENSOR_USE_VQF
-	SensorFusionRestDetect(
-		VQFParams vqfParams,
-		float gyrTs,
-		float accTs = -1.0,
-		float magTs = -1.0
-	)
-		: SensorFusion(vqfParams, gyrTs, accTs, magTs)
-#if !SENSOR_FUSION_WITH_RESTDETECT
-		, restDetection(restDetectionParams, gyrTs, (accTs < 0) ? gyrTs : accTs)
-#endif
-	{
-	}
-#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_

src/sensors/SensorManager.cpp

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

src/sensors/SensorManager.h

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

src/sensors/SensorToggles.cpp

@@ -1,15 +1,18 @@
 #include "SensorToggles.h"
 
+SensorToggleState::SensorToggleState(SensorToggleValues values)
+	: values{values} {}
+
 void SensorToggleState::setToggle(SensorToggles toggle, bool state) {
 	switch (toggle) {
 		case SensorToggles::MagEnabled:
-			magEnabled = state;
+			values.magEnabled = state;
 			break;
 		case SensorToggles::CalibrationEnabled:
-			calibrationEnabled = state;
+			values.calibrationEnabled = state;
 			break;
 		case SensorToggles::TempGradientCalibrationEnabled:
-			tempGradientCalibrationEnabled = state;
+			values.tempGradientCalibrationEnabled = state;
 			break;
 	}
 }
@@ -17,11 +20,37 @@ void SensorToggleState::setToggle(SensorToggles toggle, bool state) {
 bool SensorToggleState::getToggle(SensorToggles toggle) const {
 	switch (toggle) {
 		case SensorToggles::MagEnabled:
-			return magEnabled;
+			return values.magEnabled;
 		case SensorToggles::CalibrationEnabled:
-			return calibrationEnabled;
+			return values.calibrationEnabled;
 		case SensorToggles::TempGradientCalibrationEnabled:
-			return tempGradientCalibrationEnabled;
+			return values.tempGradientCalibrationEnabled;
 	}
 	return false;
 }
+
+void SensorToggleState::onToggleChange(
+	std::function<void(SensorToggles, bool)>&& callback
+) {
+	this->callback = callback;
+}
+
+SensorToggleValues SensorToggleState::getValues() const { return values; }
+
+void SensorToggleState::emitToggleChange(SensorToggles toggle, bool state) const {
+	if (callback) {
+		(*callback)(toggle, state);
+	}
+}
+
+const char* SensorToggleState::toggleToString(SensorToggles toggle) {
+	switch (toggle) {
+		case SensorToggles::MagEnabled:
+			return "MagEnabled";
+		case SensorToggles::CalibrationEnabled:
+			return "CalibrationEnabled";
+		case SensorToggles::TempGradientCalibrationEnabled:
+			return "TempGradientCalibrationEnabled";
+	}
+	return "Unknown";
+}

src/sensors/SensorToggles.h

@@ -24,6 +24,8 @@
 #pragma once
 
 #include <cstdint>
+#include <functional>
+#include <optional>
 
 #include "../debug.h"
 
@@ -33,13 +35,30 @@ enum class SensorToggles : uint16_t {
 	TempGradientCalibrationEnabled = 3,
 };
 
+struct SensorToggleValues {
+	bool magEnabled = !USE_6_AXIS;
+	bool calibrationEnabled = true;
+	bool tempGradientCalibrationEnabled
+		= false;  // disable by default, it is not clear that it really helps
+};
+
 class SensorToggleState {
 public:
+	SensorToggleState() = default;
+	explicit SensorToggleState(SensorToggleValues values);
 	void setToggle(SensorToggles toggle, bool state);
 	[[nodiscard]] bool getToggle(SensorToggles toggle) const;
 
+	void onToggleChange(std::function<void(SensorToggles, bool)>&& callback);
+
+	static const char* toggleToString(SensorToggles toggle);
+
+	[[nodiscard]] SensorToggleValues getValues() const;
+
 private:
-	bool magEnabled = !USE_6_AXIS;
-	bool calibrationEnabled = true;
-	bool tempGradientCalibrationEnabled = true;
+	std::optional<std::function<void(SensorToggles, bool)>> callback;
+
+	void emitToggleChange(SensorToggles toggle, bool state) const;
+
+	SensorToggleValues values;
 };

src/sensors/bmi160sensor.h

@@ -1,272 +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 SENSORS_BMI160SENSOR_H
-#define SENSORS_BMI160SENSOR_H
-
-#include <BMI160.h>
-
-#include "../motionprocessing/GyroTemperatureCalibrator.h"
-#include "../motionprocessing/RestDetection.h"
-#include "../motionprocessing/types.h"
-#include "RestCalibrationDetector.h"
-#include "SensorFusionRestDetect.h"
-#include "magneto1.4.h"
-#include "sensor.h"
-#include "sensors/axisremap.h"
-
-#if BMI160_USE_VQF
-#if USE_6_AXIS
-#define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ
-#else
-#define BMI160_GYRO_RATE BMI160_GYRO_RATE_200HZ
-#endif
-#else
-#if USE_6_AXIS
-#define BMI160_GYRO_RATE BMI160_GYRO_RATE_800HZ
-#else
-#define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ
-#endif
-#endif
-#define BMI160_GYRO_RANGE BMI160_GYRO_RANGE_1000
-#define BMI160_GYRO_FILTER_MODE BMI160_DLPF_MODE_NORM
-
-#define BMI160_ACCEL_RATE BMI160_ACCEL_RATE_100HZ
-#define BMI160_ACCEL_RANGE BMI160_ACCEL_RANGE_4G
-#define BMI160_ACCEL_FILTER_MODE BMI160_DLPF_MODE_NORM
-
-// note: if changing ODR or filter modes - adjust rest detection params and buffer size
-
-#define BMI160_TIMESTAMP_RESOLUTION_MICROS 39.0625f
-// #define BMI160_TIMESTAMP_RESOLUTION_MICROS 39.0f
-#define BMI160_MAP_ODR_MICROS(micros)                          \
-	((uint16_t)((micros) / BMI160_TIMESTAMP_RESOLUTION_MICROS) \
-	 * BMI160_TIMESTAMP_RESOLUTION_MICROS)
-constexpr float BMI160_ODR_GYR_HZ = 25.0f * (1 << (BMI160_GYRO_RATE - 6));
-constexpr float BMI160_ODR_ACC_HZ = 12.5f * (1 << (BMI160_ACCEL_RATE - 5));
-constexpr float BMI160_ODR_GYR_MICROS
-	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_GYR_HZ * 1e6f);
-constexpr float BMI160_ODR_ACC_MICROS
-	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_ACC_HZ * 1e6f);
-#if !USE_6_AXIS
-// note: this value only sets polling and fusion update rate - HMC is internally sampled
-// at 75hz, QMC at 200hz
-#define BMI160_MAG_RATE BMI160_MAG_RATE_50HZ
-constexpr float BMI160_ODR_MAG_HZ = (25.0f / 32.0f) * (1 << (BMI160_MAG_RATE - 1));
-constexpr float BMI160_ODR_MAG_MICROS
-	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_MAG_HZ * 1e6f);
-#else
-constexpr float BMI160_ODR_MAG_HZ = 0;
-constexpr float BMI160_ODR_MAG_MICROS = 0;
-#endif
-
-constexpr uint16_t BMI160_SETTINGS_MAX_ODR_HZ
-	= max(max(BMI160_ODR_GYR_HZ, BMI160_ODR_ACC_HZ), BMI160_ODR_MAG_HZ);
-constexpr uint16_t BMI160_SETTINGS_MAX_ODR_MICROS
-	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_SETTINGS_MAX_ODR_HZ * 1e6f);
-
-constexpr float BMI160_FIFO_AVG_DATA_FRAME_LENGTH
-	= (BMI160_SETTINGS_MAX_ODR_HZ * 1 + BMI160_ODR_GYR_HZ * BMI160_FIFO_G_LEN
-	   + BMI160_ODR_ACC_HZ * BMI160_FIFO_A_LEN + BMI160_ODR_MAG_HZ * BMI160_FIFO_M_LEN)
-	/ BMI160_SETTINGS_MAX_ODR_HZ;
-constexpr float BMI160_FIFO_READ_BUFFER_SIZE_MICROS = 30000;
-constexpr float BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES
-	= BMI160_SETTINGS_MAX_ODR_HZ * BMI160_FIFO_READ_BUFFER_SIZE_MICROS / 1e6f;
-constexpr uint16_t BMI160_FIFO_MAX_LENGTH = 1024;
-constexpr uint16_t BMI160_FIFO_READ_BUFFER_SIZE_BYTES = min(
-	(float)BMI160_FIFO_MAX_LENGTH - 64,
-	BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES* BMI160_FIFO_AVG_DATA_FRAME_LENGTH * 1.25f
-);
-
-// Typical sensitivity at 25C
-// See p. 9 of https://www.mouser.com/datasheet/2/783/BST-BMI160-DS000-1509569.pdf
-// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 16.4f  // 2000 deg  0
-// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 32.8f  // 1000 deg  1
-// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 65.6f  // 500 deg   2
-// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 131.2f // 250 deg   3
-// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 262.4f // 125 deg   4
-constexpr double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB
-	= (16.4f * (1 << BMI160_GYRO_RANGE));
-
-constexpr std::pair<uint8_t, float> BMI160_ACCEL_SENSITIVITY_LSB_MAP[]
-	= {{BMI160_ACCEL_RANGE_2G, 16384.0f},
-	   {BMI160_ACCEL_RANGE_4G, 8192.0f},
-	   {BMI160_ACCEL_RANGE_8G, 4096.0f},
-	   {BMI160_ACCEL_RANGE_16G, 2048.0f}};
-constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB
-	= BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second;
-constexpr double BMI160_ASCALE
-	= CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB;
-
-// Scale conversion steps: LSB/°/s -> °/s -> step/°/s -> step/rad/s
-constexpr double BMI160_GSCALE
-	= ((32768. / BMI160_GYRO_TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0);
-
-constexpr float targetSampleRateMs = 10.0f;
-constexpr uint32_t targetSampleRateMicros = (uint32_t)targetSampleRateMs * 1e3;
-
-constexpr uint32_t BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP
-	= TEMP_CALIBRATION_SECONDS_PER_STEP / (BMI160_ODR_GYR_MICROS / 1e6);
-static_assert(
-	0x7FFF * BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP < 0x7FFFFFFF,
-	"Temperature calibration sum overflow"
-);
-
-class BMI160Sensor : public Sensor {
-public:
-	static constexpr uint8_t Address = 0x68;
-	static constexpr auto TypeID = SensorTypeID::BMI160;
-
-	BMI160Sensor(
-		uint8_t id,
-		uint8_t i2cAddress,
-		float rotation,
-		SlimeVR::SensorInterface* sensorInterface,
-		PinInterface*,
-		int axisRemapParam
-	)
-		: Sensor(
-			"BMI160Sensor",
-			SensorTypeID::BMI160,
-			id,
-			i2cAddress,
-			rotation,
-			sensorInterface
-		)
-		, sfusion(
-			  BMI160_ODR_GYR_MICROS / 1e6f,
-			  BMI160_ODR_ACC_MICROS / 1e6f,
-			  BMI160_ODR_MAG_MICROS / 1e6f
-		  ) {
-		if (axisRemapParam < 256) {
-			axisRemap = AXIS_REMAP_DEFAULT;
-		} else {
-			axisRemap = axisRemapParam;
-		}
-	};
-	~BMI160Sensor(){};
-	void initHMC(BMI160MagRate magRate);
-	void initQMC(BMI160MagRate magRate);
-
-	void motionSetup() override final;
-	void motionLoop() override final;
-	void startCalibration(int calibrationType) override final;
-	void maybeCalibrateGyro();
-	void maybeCalibrateAccel();
-	void maybeCalibrateMag();
-
-	void printTemperatureCalibrationState() override final;
-	void printDebugTemperatureCalibrationState() override final;
-	void resetTemperatureCalibrationState() override final {
-		gyroTempCalibrator->reset();
-		m_Logger.info(
-			"Temperature calibration state has been reset for sensorId:%i",
-			sensorId
-		);
-	};
-	void saveTemperatureCalibration() override final;
-
-	void applyAccelCalibrationAndScale(sensor_real_t Axyz[3]);
-	void applyMagCalibrationAndScale(sensor_real_t Mxyz[3]);
-
-	bool hasGyroCalibration();
-	bool hasAccelCalibration();
-	bool hasMagCalibration();
-
-	void onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
-	void onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
-	void onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
-	void readFIFO();
-
-	void
-	getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z);
-
-	void remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z);
-	void remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z);
-	void getRemappedRotation(int16_t* x, int16_t* y, int16_t* z);
-	void getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z);
-
-	bool getTemperature(float* out);
-
-private:
-	BMI160 imu{};
-	int axisRemap;
-
-	SlimeVR::Sensors::SensorFusionRestDetect sfusion;
-
-	// clock sync and sample timestamping
-	uint32_t sensorTime0 = 0;
-	uint32_t sensorTime1 = 0;
-	uint32_t localTime0 = 0;
-	uint32_t localTime1 = 0;
-	double sensorTimeRatio = 1;
-	double sensorTimeRatioEma = 1;
-	double sampleDtMicros = BMI160_ODR_GYR_MICROS;
-	uint32_t syncLatencyMicros = 0;
-	uint32_t samplesSinceClockSync = 0;
-	uint32_t timestamp0 = 0;
-	uint32_t timestamp1 = 0;
-
-	// scheduling
-	uint32_t lastPollTime = micros();
-	uint32_t lastClockPollTime = micros();
-#if BMI160_DEBUG
-	uint32_t cpuUsageMicros = 0;
-	uint32_t lastCpuUsagePrinted = 0;
-	uint32_t gyrReads = 0;
-	uint32_t accReads = 0;
-	uint32_t magReads = 0;
-	uint16_t numFIFODropped = 0;
-	uint16_t numFIFOFailedReads = 0;
-#endif
-
-	uint32_t lastRotationPacketSent = 0;
-	uint32_t lastTemperaturePacketSent = 0;
-
-	struct BMI160FIFO {
-		uint8_t data[BMI160_FIFO_READ_BUFFER_SIZE_BYTES];
-		uint16_t length;
-	} fifo{};
-	float temperature = 0;
-	GyroTemperatureCalibrator* gyroTempCalibrator = nullptr;
-	sensor_real_t Gxyz[3] = {0};
-	sensor_real_t Axyz[3] = {0};
-	sensor_real_t Mxyz[3] = {0};
-	sensor_real_t lastAxyz[3] = {0};
-
-	double gscaleX = BMI160_GSCALE;
-	double gscaleY = BMI160_GSCALE;
-	double gscaleZ = BMI160_GSCALE;
-
-	double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0};
-
-	bool isGyroCalibrated = false;
-	bool isAccelCalibrated = false;
-	bool isMagCalibrated = false;
-
-	SlimeVR::Configuration::BMI160SensorConfig m_Config = {};
-
-	SlimeVR::Sensors::RestCalibrationDetector calibrationDetector;
-};
-
-#endif

src/sensors/bno055sensor.h

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

src/sensors/bno080sensor.cpp

@@ -135,6 +135,13 @@ void BNO080Sensor::motionSetup() {
 	configured = true;
 	m_tpsCounter.reset();
 	m_dataCounter.reset();
+
+	toggles.onToggleChange([&](SensorToggles toggle, bool) {
+		if (toggle == SensorToggles::MagEnabled) {
+			// TODO: maybe handle this more gracefully, I'm sure it's possible
+			motionSetup();
+		}
+	});
 }
 
 void BNO080Sensor::motionLoop() {