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

.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}")
 
@@ -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/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/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,147 +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
-build_flags =
-  ${env.build_flags}
-  -D BOARD=BOARD_SLIMEVR
-
-[env:BOARD_SLIMEVR_V1_2]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-build_flags =
-  ${env.build_flags}
-  -D BOARD=BOARD_SLIMEVR_V1_2
-
-[env:BOARD_SLIMEVR_DEV]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-build_flags =
-  ${env.build_flags}
-  -D BOARD=BOARD_SLIMEVR_DEV
-
-[env:BOARD_GLOVE_IMU_SLIMEVR_DEV]
-platform = espressif32 @ 6.7.0
-platform_packages =
-  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
-  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-  -D BOARD=BOARD_GLOVE_IMU_SLIMEVR_DEV
-board = lolin_c3_mini
-
-[env:BOARD_NODEMCU]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-build_flags = 
-  ${env.build_flags}
-  -D BOARD=BOARD_NODEMCU
-
-[env:BOARD_WEMOSD1MINI]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-build_flags = 
-  ${env.build_flags}
-  -D BOARD=BOARD_WEMOSD1MINI
-
-[env:BOARD_TTGO_TBASE]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-build_flags = 
-  ${env.build_flags}
-  -D BOARD=BOARD_TTGO_TBASE
-
-[env:BOARD_WEMOSWROOM02]
-platform = espressif8266 @ 4.2.1
-board = esp12e
-build_flags = 
-  ${env.build_flags}
-  -D BOARD=BOARD_NODEMCU
-
-[env:BOARD_WROOM32]
-platform = espressif32 @ 6.7.0
-platform_packages =
-  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
-  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
-board = esp32dev
-build_flags = 
-  ${env.build_flags}
-  -D BOARD=BOARD_WROOM32
-
-[env:BOARD_ESP01]
-platform = espressif32 @ 6.7.0
-platform_packages =
-  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
-  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
-board = esp32dev
-build_flags = 
-  ${env.build_flags}
-  -D BOARD=BOARD_ESP01
-
-[env:BOARD_LOLIN_C3_MINI]
-platform = espressif32 @ 6.7.0
-platform_packages =
-  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
-  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-  -D BOARD=BOARD_LOLIN_C3_MINI
-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
-  -D BOARD=BOARD_BEETLE32C3
-board = dfrobot_beetle_esp32c3
-
-[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
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-  -D BOARD=BOARD_ESP32C3DEVKITM1
-board = esp32-c3-devkitm-1
-
-[env:BOARD_ESP32C6DEVKITC1]
-platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
-build_flags =
-  ${env.build_flags}
-  -DESP32C6
-  -D BOARD=BOARD_ESP32C6DEVKITC1
-board = esp32-c6-devkitc-1
-
-[env:BOARD_XIAO_ESP32C3]
-platform = espressif32 @ 6.7.0
-platform_packages =
-  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
-  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
-build_flags = 
-  ${env.build_flags}
-  -DESP32C3
-  -D BOARD=BOARD_XIAO_ESP32C3
-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/GlobalVars.h

@@ -28,6 +28,8 @@
 #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"
@@ -40,3 +42,5 @@ extern SlimeVR::Sensors::SensorManager sensorManager;
 extern SlimeVR::Network::Manager networkManager;
 extern SlimeVR::Network::Connection networkConnection;
 extern BatteryMonitor battery;
+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/boards/boards_default.h

@@ -27,154 +27,6 @@
 
 #include "defines_helpers.h"
 
-// Board-specific configurations
-#if BOARD == BOARD_SLIMEVR
-
-SDA(14)
-SCL(12)
-INT(16)
-INT2(13)
-BATTERY(17)
-LED(2)
-INVERTED_LED(true)
-BATTERY_SHIELD_R(0)
-BATTERY_R1(10)
-BATTERY_R2(40.2)
-
-#elif BOARD == BOARD_SLIMEVR_V1_2
-
-SDA(4)
-SCL(5)
-INT(2)
-INT2(16)
-BATTERY(17)
-LED(2)
-INVERTED_LED(true)
-BATTERY_SHIELD_R(0)
-BATTERY_R1(10)
-BATTERY_R2(40.2)
-
-#elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV
-
-SDA(4)
-SCL(5)
-INT(10)
-INT2(13)
-BATTERY(17)
-LED(2)
-INVERTED_LED(true)
-BATTERY_SHIELD_R(0)
-BATTERY_R1(10)
-BATTERY_R2(40.2)
-
-#elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI
-
-SDA(D2)
-SCL(D1)
-INT(D5)
-INT2(D6)
-BATTERY(A0)
-BATTERY_SHIELD_R(180)
-BATTERY_R1(100)
-BATTERY_R2(220)
-
-#elif BOARD == BOARD_ESP01
-
-SDA(2)
-SCL(0)
-INT(255)
-INT2(255)
-BATTERY(255)
-LED(LED_OFF)
-INVERTED_LED(false)
-
-#elif BOARD == BOARD_TTGO_TBASE
-
-SDA(5)
-SCL(4)
-INT(14)
-INT2(13)
-BATTERY(A0)
-
-#elif BOARD == BOARD_CUSTOM
-
-// Define pins by the examples above
-
-#elif BOARD == BOARD_WROOM32
-
-SDA(21)
-SCL(22)
-INT(23)
-INT2(25)
-BATTERY(36)
-
-#elif BOARD == BOARD_LOLIN_C3_MINI
-
-SDA(5)
-SCL(4)
-INT(6)
-INT2(8)
-BATTERY(3)
-LED(7)
-
-#elif BOARD == BOARD_BEETLE32C3
-
-SDA(8)
-SCL(9)
-INT(6)
-INT2(7)
-BATTERY(3)
-LED(10)
-INVERTED_LED(false)
-
-#elif BOARD == BOARD_ESP32C3DEVKITM1 || BOARD == BOARD_ESP32C6DEVKITC1
-
-SDA(5)
-SCL(4)
-INT(6)
-INT2(7)
-BATTERY(3)
-LED(LED_OFF)
-
-#elif BOARD == BOARD_WEMOSWROOM02
-
-SDA(2)
-SCL(14)
-INT(0)
-INT2(4)
-BATTERY(A0)
-LED(16)
-INVERTED_LED(true)
-
-#elif BOARD == BOARD_XIAO_ESP32C3
-
-SDA(6)
-SCL(7)  // D5
-INT(5)  // D3
-INT2(10)  // D10
-LED(4)  // D2
-INVERTED_LED(false)
-BATTERY(2)  // D0 / A0
-BATTERY_SHIELD_R(0)
-BATTERY_R1(100)
-BATTERY_R2(100)
-
-#elif BOARD == BOARD_GLOVE_IMU_SLIMEVR_DEV
-
-SDA(1)
-SCL(0)
-#define PCA_ADDR 0x70
-INT(16)
-INT2(13)
-BATTERY(3)
-LED(2)
-INVERTED_LED(true)
-BATTERY_SHIELD_R(0)
-BATTERY_R1(10)
-BATTERY_R2(40.2)
-
-#endif
-
 // Default IMU pinouts and definitions for default tracker types
 
 #if BOARD != BOARD_GLOVE_IMU_SLIMEVR_DEV
@@ -241,6 +93,18 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
 #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

@@ -29,5 +29,10 @@
 #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/configuration/Configuration.cpp

@@ -25,13 +25,18 @@
 
 #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::Configuration {
 void Configuration::setup() {
@@ -118,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
+			);
+		}
 	}
 
 	{
@@ -133,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");
 }
 
@@ -226,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});
 	});
 }
 

src/consts.h

@@ -95,6 +95,10 @@ enum class SensorTypeID : uint8_t {
 #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
@@ -151,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

@@ -71,7 +71,7 @@
 // #define PIN_BATTERY_LEVEL 17
 // #define LED_PIN 2
 // #define LED_INVERTED true
-// #define BATTERY_SHILED_RESISTANCE 0
+// #define BATTERY_SHIELD_RESISTANCE 0
 // #define BATTERY_SHIELD_R1 10
 // #define BATTERY_SHIELD_R2 40.2
 

src/globals.h

@@ -52,3 +52,27 @@
 #ifndef EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION
 #define EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION true
 #endif
+
+#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/main.cpp

@@ -42,6 +42,8 @@ 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();
@@ -83,7 +117,7 @@ void setup() {
 
 	// 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/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/manager.cpp

@@ -26,14 +26,14 @@
 
 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;

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

@@ -31,7 +31,7 @@ class SensorInterface {
 public:
 	virtual bool init() = 0;
 	virtual void swapIn() = 0;
-	[[nodiscard]] virtual std::string toString() const;
+	[[nodiscard]] virtual std::string toString() const = 0;
 };
 
 class EmptySensorInterface : public SensorInterface {

src/sensors/ADCResistanceSensor.cpp

@@ -30,7 +30,7 @@ void ADCResistanceSensor::motionLoop() {
 	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
@@ -41,4 +41,4 @@ void ADCResistanceSensor::sendData() {
 	networkConnection.sendFlexData(sensorId, m_Data);
 }
 
-}  // namespace SlimeVR::Sensors
+}  // namespace SlimeVR::Sensors

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

@@ -72,13 +72,13 @@
 
 #if USE_RUNTIME_CALIBRATION
 #include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
-#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator
+#define SFCALIBRATOR RuntimeCalibration::RuntimeCalibrator
 #else
 #include "sensors/softfusion/SoftfusionCalibration.h"
-#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator
+#define SFCALIBRATOR SoftfusionCalibrator
 #endif
 
-#if ESP32
+#ifdef ESP32
 #include "driver/i2c.h"
 #endif
 
@@ -285,11 +285,15 @@ public:
 				extraParam,
 			});
 		}
-		if (sensor->isWorking()) {
-			m_Manager->m_Logger.info("Sensor %d configured", sensorID);
-		}
+
+		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;
 	}
 

src/sensors/SensorFusion.cpp

@@ -28,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) {
@@ -50,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) {
@@ -64,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;
@@ -142,19 +64,11 @@ bool SensorFusion::isUpdated() { return updated; }
 void SensorFusion::clearUpdated() { updated = false; }
 
 sensor_real_t const* SensorFusion::getQuaternion() {
-#if SENSOR_USE_BASICVQF
-	if (magExist) {
-		basicvqf.getQuat9D(qwxyz);
-	} else {
-		basicvqf.getQuat6D(qwxyz);
-	}
-#elif SENSOR_USE_VQF
 	if (magExist) {
 		vqf.getQuat9D(qwxyz);
 	} else {
 		vqf.getQuat6D(qwxyz);
 	}
-#endif
 
 	return qwxyz;
 }
@@ -211,10 +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
+
+bool SensorFusion::getRestDetected() const { return vqf.getRestDetected(); }
 
 }  // namespace SlimeVR::Sensors

src/sensors/SensorFusion.h

@@ -6,44 +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::Sensors {
-#if SENSOR_USE_VQF
 constexpr VQFParams DefaultVQFParams = VQFParams{
 	.tauAcc = 2.0f,
 	.restMinT = 2.0f,
 	.restThGyr = 0.6f,
 	.restThAcc = 0.06f,
 };
-#endif
 
 class SensorFusion {
 public:
@@ -57,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,
@@ -108,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;
@@ -145,7 +98,7 @@ 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
 };

src/sensors/SensorFusionDMP.h

@@ -1,6 +1,8 @@
 #ifndef SLIMEVR_SENSORFUSIONDMP_H
 #define SLIMEVR_SENSORFUSIONDMP_H
 
+#include <helper_3dmath.h>
+
 #include "SensorFusion.h"
 #include "dmpmag.h"
 
@@ -36,7 +38,7 @@ 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
 };

src/sensors/SensorFusionRestDetect.cpp

@@ -1,35 +0,0 @@
-#include "SensorFusionRestDetect.h"
-
-namespace SlimeVR::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 SlimeVR::Sensors

src/sensors/SensorFusionRestDetect.h

@@ -1,64 +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::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 SlimeVR::Sensors
-
-#endif  // SLIMEVR_SENSORFUSIONRESTDETECT_H_

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/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() {

src/sensors/sensor.cpp

@@ -91,6 +91,8 @@ void Sensor::resetTemperatureCalibrationState() {
 	printTemperatureCalibrationUnsupported();
 };
 
+const char* Sensor::getAttachedMagnetometer() const { return nullptr; }
+
 SlimeVR::Configuration::SensorConfigBits Sensor::getSensorConfigData() {
 	return SlimeVR::Configuration::SensorConfigBits{
 		.magEnabled = toggles.getToggle(SensorToggles::MagEnabled),
@@ -157,12 +159,16 @@ void Sensor::markRestCalibrationComplete(bool completed) {
 }
 
 void Sensor::setFlag(SensorToggles toggle, bool state) {
-	assert(isFlagSupported(toggle));
+	if (!isFlagSupported(toggle)) {
+		m_Logger.error(
+			"Toggle %s isn't supported by this sensor!",
+			SensorToggleState::toggleToString(toggle)
+		);
+		return;
+	}
 
 	toggles.setToggle(toggle, state);
 
 	configuration.setSensorToggles(sensorId, toggles);
 	configuration.save();
-
-	motionSetup();
 }

src/sensors/sensor.h

@@ -85,6 +85,11 @@ public:
 	virtual void printDebugTemperatureCalibrationState();
 	virtual void resetTemperatureCalibrationState();
 	virtual void saveTemperatureCalibration();
+	// TODO: currently only for softfusionsensor, bmi160 and others should get
+	// an overload too
+	virtual const char* getAttachedMagnetometer() const;
+	// TODO: realistically each sensor should print its own state instead of
+	// having 15 getters for things only the serial commands use
 	bool isWorking() { return working; };
 	bool getHadData() const { return hadData; };
 	bool isValid() { return m_hwInterface != nullptr; };

src/sensors/softfusion/CalibrationBase.h

@@ -28,33 +28,31 @@
 #include <functional>
 
 #include "configuration/SensorConfig.h"
+#include "imuconsts.h"
 #include "motionprocessing/types.h"
-#include "sensors/SensorFusionRestDetect.h"
+#include "sensors/SensorFusion.h"
 
-namespace SlimeVR::Sensor {
+namespace SlimeVR::Sensors {
 
-template <typename IMU, typename RawSensorT, typename RawVectorT>
+template <typename IMU>
 class CalibrationBase {
 public:
 	CalibrationBase(
-		SlimeVR::Sensors::SensorFusionRestDetect& fusion,
+		SlimeVR::Sensors::SensorFusion& fusion,
 		IMU& sensor,
 		uint8_t sensorId,
 		SlimeVR::Logging::Logger& logger,
-		float TempTs,
-		float AScale,
-		float GScale,
 		SensorToggleState& toggles
 	)
 		: fusion{fusion}
 		, sensor{sensor}
 		, sensorId{sensorId}
 		, logger{logger}
-		, TempTs{TempTs}
-		, AScale{AScale}
-		, GScale{GScale}
 		, toggles{toggles} {}
 
+	using Consts = IMUConsts<IMU>;
+	using RawSensorT = typename Consts::RawSensorT;
+
 	static constexpr bool HasMotionlessCalib
 		= requires(IMU& i) { typename IMU::MotionlessCalibrationData; };
 	static constexpr size_t MotionlessCalibDataSize() {
@@ -65,15 +63,8 @@ public:
 		}
 	}
 
-	using EatSamplesFn = std::function<void(const uint32_t)>;
-	using ReturnLastFn
-		= std::function<std::tuple<RawVectorT, RawVectorT, int16_t>(const uint32_t)>;
-
-	virtual void startCalibration(
-		int calibrationType,
-		const EatSamplesFn& eatSamplesForSeconds,
-		const ReturnLastFn& eatSamplesReturnLast
-	){};
+	virtual void checkStartupCalibration() {}
+	virtual void startCalibration(int calibrationType){};
 
 	virtual bool calibrationMatches(
 		const SlimeVR::Configuration::SensorConfig& sensorCalibration
@@ -96,6 +87,7 @@ public:
 
 	virtual const uint8_t* getMotionlessCalibrationData() = 0;
 
+	virtual void signalOverwhelmed() {}
 	virtual void provideAccelSample(const RawSensorT accelSample[3]) {}
 	virtual void provideGyroSample(const RawSensorT gyroSample[3]) {}
 	virtual void provideTempSample(float tempSample) {}
@@ -104,7 +96,7 @@ public:
 
 protected:
 	void recalcFusion() {
-		fusion = Sensors::SensorFusionRestDetect(
+		fusion = Sensors::SensorFusion(
 			IMU::SensorVQFParams,
 			getGyroTimestep(),
 			getAccelTimestep(),
@@ -112,14 +104,11 @@ protected:
 		);
 	}
 
-	Sensors::SensorFusionRestDetect& fusion;
+	Sensors::SensorFusion& fusion;
 	IMU& sensor;
 	uint8_t sensorId;
 	SlimeVR::Logging::Logger& logger;
-	float TempTs;
-	float AScale;
-	float GScale;
 	SensorToggleState& toggles;
 };
 
-}  // namespace SlimeVR::Sensor
+}  // namespace SlimeVR::Sensors

src/sensors/softfusion/SoftfusionCalibration.h

@@ -23,48 +23,120 @@
 
 #pragma once
 
+#include <cstdint>
 #include <cstring>
 #include <vector>
 
+#include "CalibrationBase.h"
 #include "GlobalVars.h"
 #include "configuration/SensorConfig.h"
 #include "logging/Logger.h"
 #include "motionprocessing/RestDetection.h"
 #include "motionprocessing/types.h"
-#include "sensors/SensorFusionRestDetect.h"
-#include "sensors/softfusion/CalibrationBase.h"
+#include "sensors/SensorFusion.h"
 
-namespace SlimeVR::Sensor {
+namespace SlimeVR::Sensors {
 
-template <typename IMU, typename RawSensorT, typename RawVectorT>
-class SoftfusionCalibrator : public CalibrationBase<IMU, RawSensorT, RawVectorT> {
+template <typename IMU>
+class SoftfusionCalibrator : public CalibrationBase<IMU> {
 public:
 	static constexpr bool HasUpsideDownCalibration = true;
 
-	using Base = CalibrationBase<IMU, RawSensorT, RawVectorT>;
+	using Base = CalibrationBase<IMU>;
+	using Consts = typename Base::Consts;
+	using RawSensorT = typename Consts::RawSensorT;
+	using RawVectorT = typename Consts::RawVectorT;
 
 	SoftfusionCalibrator(
-		Sensors::SensorFusionRestDetect& fusion,
+		Sensors::SensorFusion& fusion,
 		IMU& sensor,
 		uint8_t sensorId,
 		SlimeVR::Logging::Logger& logger,
-		float TempTs,
-		float AScale,
-		float GScale,
 		SensorToggleState& toggles
 	)
-		: Base{fusion, sensor, sensorId, logger, TempTs, AScale, GScale, toggles} {
-		calibration.T_Ts = TempTs;
+		: Base{fusion, sensor, sensorId, logger, toggles} {
+		calibration.T_Ts = Consts::getDefaultTempTs();
 	}
 
-	void startCalibration(
-		int calibrationType,
-		const Base::EatSamplesFn& eatSamplesForSeconds,
-		const Base::ReturnLastFn& eatSamplesReturnLast
-	) final {
+	void eatSamplesForSeconds(const uint32_t seconds) {
+		const auto targetDelay = millis() + 1000 * seconds;
+		auto lastSecondsRemaining = seconds;
+		while (millis() < targetDelay) {
+#ifdef ESP8266
+			ESP.wdtFeed();
+#endif
+			auto currentSecondsRemaining = (targetDelay - millis()) / 1000;
+			if (currentSecondsRemaining != lastSecondsRemaining) {
+				logger.info("%d...", currentSecondsRemaining + 1);
+				lastSecondsRemaining = currentSecondsRemaining;
+			}
+
+			sensor.bulkRead({
+				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
+				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
+				[](const int16_t xyz, const sensor_real_t timeDelta) {},
+			});
+		}
+	}
+
+	std::tuple<RawVectorT, RawVectorT, int16_t> eatSamplesReturnLast(
+		const uint32_t milliseconds
+	) {
+		RawVectorT accel = {0};
+		RawVectorT gyro = {0};
+		int16_t temp = 0;
+		const auto targetDelay = millis() + milliseconds;
+		while (millis() < targetDelay) {
+			sensor.bulkRead({
+				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
+					accel[0] = xyz[0];
+					accel[1] = xyz[1];
+					accel[2] = xyz[2];
+				},
+				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
+					gyro[0] = xyz[0];
+					gyro[1] = xyz[1];
+					gyro[2] = xyz[2];
+				},
+				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
+					temp = rawTemp;
+				},
+			});
+			yield();
+		}
+		return std::make_tuple(accel, gyro, temp);
+	}
+
+	void checkStartupCalibration() final {
+		auto lastRawSample = eatSamplesReturnLast(1000);
+		auto gravity = static_cast<sensor_real_t>(
+			Consts::AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
+		);
+		logger.info("Gravity read: %.1f (need < -7.5 to start calibration)", gravity);
+		if (gravity > -7.5f) {
+			return;
+		}
+
+		ledManager.on();
+		logger.info("Flip front in 5 seconds to start calibration");
+		lastRawSample = eatSamplesReturnLast(5000);
+		gravity = static_cast<sensor_real_t>(
+			Consts::AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
+		);
+		if (gravity > 7.5f) {
+			logger.debug("Starting calibration...");
+			startCalibration(0);
+		} else {
+			logger.info("Flip not detected. Skipping calibration.");
+		}
+
+		ledManager.off();
+	}
+
+	void startCalibration(int calibrationType) final {
 		if (calibrationType == 0) {
 			// ALL
-			calibrateSampleRate(eatSamplesForSeconds);
+			calibrateSampleRate();
 			if constexpr (Base::HasMotionlessCalib) {
 				typename IMU::MotionlessCalibrationData calibData;
 				sensor.motionlessCalibration(calibData);
@@ -73,14 +145,14 @@ public:
 			// Gryoscope offset calibration can only happen after any motionless
 			// gyroscope calibration, otherwise we are calculating the offset based
 			// on an incorrect starting point
-			calibrateGyroOffset(eatSamplesReturnLast);
-			calibrateAccel(eatSamplesForSeconds);
+			calibrateGyroOffset();
+			calibrateAccel();
 		} else if (calibrationType == 1) {
-			calibrateSampleRate(eatSamplesForSeconds);
+			calibrateSampleRate();
 		} else if (calibrationType == 2) {
-			calibrateGyroOffset(eatSamplesReturnLast);
+			calibrateGyroOffset();
 		} else if (calibrationType == 3) {
-			calibrateAccel(eatSamplesForSeconds);
+			calibrateAccel();
 		} else if (calibrationType == 4) {
 			if constexpr (Base::HasMotionlessCalib) {
 				typename IMU::MotionlessCalibrationData calibData;
@@ -133,28 +205,28 @@ public:
 		accelSample[0]
 			= (calibration.A_Ainv[0][0] * tmp[0] + calibration.A_Ainv[0][1] * tmp[1]
 			   + calibration.A_Ainv[0][2] * tmp[2])
-			* AScale;
+			* Consts::AScale;
 		accelSample[1]
 			= (calibration.A_Ainv[1][0] * tmp[0] + calibration.A_Ainv[1][1] * tmp[1]
 			   + calibration.A_Ainv[1][2] * tmp[2])
-			* AScale;
+			* Consts::AScale;
 		accelSample[2]
 			= (calibration.A_Ainv[2][0] * tmp[0] + calibration.A_Ainv[2][1] * tmp[1]
 			   + calibration.A_Ainv[2][2] * tmp[2])
-			* AScale;
+			* Consts::AScale;
 	}
 
 	float getAccelTimestep() final { return calibration.A_Ts; }
 
 	void scaleGyroSample(sensor_real_t gyroSample[3]) final {
 		gyroSample[0] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[0] - calibration.G_off[0])
+			Consts::GScale * (gyroSample[0] - calibration.G_off[0])
 		);
 		gyroSample[1] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[1] - calibration.G_off[1])
+			Consts::GScale * (gyroSample[1] - calibration.G_off[1])
 		);
 		gyroSample[2] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[2] - calibration.G_off[2])
+			Consts::GScale * (gyroSample[2] - calibration.G_off[2])
 		);
 	}
 
@@ -185,15 +257,15 @@ private:
 		configuration.save();
 	}
 
-	void calibrateGyroOffset(const Base::ReturnLastFn& eatSamplesReturnLast) {
+	void calibrateGyroOffset() {
 		if (!toggles.getToggle(SensorToggles::CalibrationEnabled)) {
 			return;
 		}
 
 		// Wait for sensor to calm down before calibration
 		logger.info(
-			"Put down the device and wait for baseline gyro reading calibration (%d "
-			"seconds)",
+			"Put down the device and wait for baseline gyro reading calibration "
+			"(%d seconds)",
 			GyroCalibDelaySeconds
 		);
 		ledManager.on();
@@ -216,7 +288,7 @@ private:
 #ifdef ESP8266
 			ESP.wdtFeed();
 #endif
-			sensor.bulkRead(
+			sensor.bulkRead({
 				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
 				[&sumXYZ,
 				 &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
@@ -225,8 +297,8 @@ private:
 					sumXYZ[2] += xyz[2];
 					++sampleCount;
 				},
-				[](const int16_t rawTemp, const sensor_real_t timeDelta) {}
-			);
+				[](const int16_t rawTemp, const sensor_real_t timeDelta) {},
+			});
 		}
 
 		ledManager.off();
@@ -244,15 +316,15 @@ private:
 		);
 	}
 
-	void calibrateAccel(const Base::EatSamplesFn& eatSamplesForSeconds) {
+	void calibrateAccel() {
 		if (!toggles.getToggle(SensorToggles::CalibrationEnabled)) {
 			return;
 		}
 
 		auto magneto = std::make_unique<MagnetoCalibration>();
 		logger.info(
-			"Put the device into 6 unique orientations (all sides), leave it still and "
-			"do not hold/touch for %d seconds each",
+			"Put the device into 6 unique orientations (all sides), leave it still "
+			"and do not hold/touch for %d seconds each",
 			AccelCalibRestSeconds
 		);
 		ledManager.on();
@@ -290,17 +362,17 @@ private:
 #ifdef ESP8266
 			ESP.wdtFeed();
 #endif
-			sensor.bulkRead(
+			sensor.bulkRead({
 				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
 					const sensor_real_t scaledData[]
 						= {static_cast<sensor_real_t>(
-							   AScale * static_cast<sensor_real_t>(xyz[0])
+							   Consts::AScale * static_cast<sensor_real_t>(xyz[0])
 						   ),
 						   static_cast<sensor_real_t>(
-							   AScale * static_cast<sensor_real_t>(xyz[1])
+							   Consts::AScale * static_cast<sensor_real_t>(xyz[1])
 						   ),
 						   static_cast<sensor_real_t>(
-							   AScale * static_cast<sensor_real_t>(xyz[2])
+							   Consts::AScale * static_cast<sensor_real_t>(xyz[2])
 						   )};
 
 					calibrationRestDetection.updateAcc(IMU::AccTs, scaledData);
@@ -347,8 +419,8 @@ private:
 					}
 				},
 				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
-				[](const int16_t rawTemp, const sensor_real_t timeDelta) {}
-			);
+				[](const int16_t rawTemp, const sensor_real_t timeDelta) {},
+			});
 		}
 		ledManager.off();
 		logger.debug("Calculating accelerometer calibration data...");
@@ -376,7 +448,7 @@ private:
 		logger.debug("}");
 	}
 
-	void calibrateSampleRate(const Base::EatSamplesFn& eatSamplesForSeconds) {
+	void calibrateSampleRate() {
 		logger.debug(
 			"Calibrating IMU sample rate in %d second(s)...",
 			SampleRateCalibDelaySeconds
@@ -392,7 +464,7 @@ private:
 		logger.debug("Counting samples now...");
 		uint32_t currentTime;
 		while ((currentTime = millis()) < calibTarget) {
-			sensor.bulkRead(
+			sensor.bulkRead({
 				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
 					accelSamples++;
 				},
@@ -401,8 +473,8 @@ private:
 				},
 				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
 					tempSamples++;
-				}
-			);
+				},
+			});
 			yield();
 		}
 
@@ -423,7 +495,8 @@ private:
 			= millisFromStart / (static_cast<float>(tempSamples) * 1000.0f);
 
 		logger.debug(
-			"Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: %fHz",
+			"Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: "
+			"%fHz",
 			1.0 / calibration.G_Ts,
 			1.0 / calibration.A_Ts,
 			1.0 / calibration.T_Ts
@@ -453,12 +526,10 @@ private:
 	};
 
 private:
-	using Base::AScale;
-	using Base::GScale;
 	using Base::logger;
 	using Base::sensor;
 	using Base::sensorId;
 	using Base::toggles;
 };
 
-}  // namespace SlimeVR::Sensor
+}  // namespace SlimeVR::Sensors

src/sensors/softfusion/drivers/bmi160.h

@@ -25,18 +25,20 @@
 
 #include <algorithm>
 #include <array>
+#include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <limits>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
 // Driver uses acceleration range at 4G
 // and gyroscope range at 1000DPS
-// Gyroscope ODR = 400Hz, accel ODR = 100Hz
+// Gyroscope ODR = 200Hz, accel ODR = 100Hz
 // Timestamps reading are not used
 
 // Sensorhub to be implemented
@@ -46,7 +48,7 @@ struct BMI160 {
 	static constexpr auto Name = "BMI160";
 	static constexpr auto Type = SensorTypeID::BMI160;
 
-	static constexpr float GyrTs = 1.0 / 400.0;
+	static constexpr float GyrTs = 1.0 / 200.0;
 	static constexpr float AccTs = 1.0 / 100.0;
 
 	static constexpr float MagTs = 1.0 / 100;
@@ -57,14 +59,7 @@ struct BMI160 {
 	static constexpr float TemperatureZROChange
 		= 2.0f;  // wow maybe BMI270 isn't that bad actually
 
-	static constexpr VQFParams SensorVQFParams{
-		// need to be refined, this IMU sucks
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 0.5f,
-		.biasClip = 2.0f,
-		.restThGyr = 0.5f,
-		.restThAcc = 0.196f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	RegisterInterface& m_RegisterInterface;
 	SlimeVR::Logging::Logger& m_Logger;
@@ -101,7 +96,7 @@ struct BMI160 {
 
 		struct GyrConf {
 			static constexpr uint8_t reg = 0x42;
-			static constexpr uint8_t value = 0b0101010;  // 400Hz, filter mode normal
+			static constexpr uint8_t value = 0b0101001;  // 200Hz, filter mode normal
 		};
 
 		struct GyrRange {
@@ -182,12 +177,7 @@ struct BMI160 {
 		return to_ret;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoLength) & 0x7FF;
 
 		const auto bytes_to_read = std::min(
@@ -218,7 +208,7 @@ struct BMI160 {
 					gyro[0] = getFromFifo<uint16_t>(i, read_buffer);
 					gyro[1] = getFromFifo<uint16_t>(i, read_buffer);
 					gyro[2] = getFromFifo<uint16_t>(i, read_buffer);
-					processGyroSample(gyro, GyrTs);
+					callbacks.processGyroSample(gyro, GyrTs);
 				}
 
 				if (header & Fifo::AccelDataBit) {
@@ -226,10 +216,11 @@ struct BMI160 {
 					accel[0] = getFromFifo<uint16_t>(i, read_buffer);
 					accel[1] = getFromFifo<uint16_t>(i, read_buffer);
 					accel[2] = getFromFifo<uint16_t>(i, read_buffer);
-					processAccelSample(accel, AccTs);
+					callbacks.processAccelSample(accel, AccTs);
 				}
 			}
 		}
+		return static_cast<size_t>(fifo_bytes) > static_cast<size_t>(bytes_to_read);
 	}
 };
 

src/sensors/softfusion/drivers/bmi270.h

@@ -31,13 +31,14 @@
 
 #include "../../../sensorinterface/RegisterInterface.h"
 #include "bmi270fw.h"
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
 // Driver uses acceleration range at 16g
 // and gyroscope range at 1000dps
-// Gyroscope ODR = 400Hz, accel ODR = 100Hz
+// Gyroscope ODR = 200Hz, accel ODR = 100Hz
 // Timestamps reading are not used
 
 struct BMI270 {
@@ -45,7 +46,7 @@ struct BMI270 {
 	static constexpr auto Name = "BMI270";
 	static constexpr auto Type = SensorTypeID::BMI270;
 
-	static constexpr float GyrTs = 1.0 / 400.0;
+	static constexpr float GyrTs = 1.0 / 200.0;
 	static constexpr float AccTs = 1.0 / 100.0;
 
 	static constexpr float MagTs = 1.0 / 100;
@@ -55,13 +56,7 @@ struct BMI270 {
 
 	static constexpr float TemperatureZROChange = 6.667f;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 0.5f,
-		.biasClip = 1.0f,
-		.restThGyr = 0.5f,
-		.restThAcc = 0.196f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	struct MotionlessCalibrationData {
 		bool valid;
@@ -137,7 +132,7 @@ struct BMI270 {
 			static constexpr uint8_t filterHighPerfMode = 1 << 7;
 
 			static constexpr uint8_t value
-				= rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode;
+				= rate200Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode;
 		};
 
 		struct GyrRange {
@@ -258,6 +253,7 @@ struct BMI270 {
 			Regs::InitCtrl::reg,
 			Regs::InitCtrl::valueStartInit
 		);
+		auto* firmware_buffer = new uint8_t[RegisterInterface::MaxTransactionLength];
 		for (uint16_t pos = 0; pos < sizeof(bmi270_firmware);) {
 			// tell the device current position
 
@@ -273,13 +269,11 @@ struct BMI270 {
 				static_cast<size_t>(sizeof(bmi270_firmware) - pos),
 				RegisterInterface::MaxTransactionLength
 			);
-			m_RegisterInterface.writeBytes(
-				Regs::InitData,
-				burstWrite,
-				const_cast<uint8_t*>(bmi270_firmware + pos)
-			);
+			memcpy_P(firmware_buffer, bmi270_firmware + pos, burstWrite);
+			m_RegisterInterface.writeBytes(Regs::InitData, burstWrite, firmware_buffer);
 			pos += burstWrite;
 		}
+		delete[] firmware_buffer;
 		m_RegisterInterface.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit);
 		delay(140);
 
@@ -434,12 +428,7 @@ struct BMI270 {
 		return to_ret;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
 		const auto bytes_to_read = std::min(
@@ -481,7 +470,7 @@ struct BMI270 {
 						static_cast<int32_t>(ShortLimit::min()),
 						static_cast<int32_t>(ShortLimit::max())
 					);
-					processGyroSample(gyro, GyrTs);
+					callbacks.processGyroSample(gyro, GyrTs);
 				}
 
 				if (header & Fifo::AccelDataBit) {
@@ -489,10 +478,12 @@ struct BMI270 {
 					accel[0] = getFromFifo<uint16_t>(i, read_buffer);
 					accel[1] = getFromFifo<uint16_t>(i, read_buffer);
 					accel[2] = getFromFifo<uint16_t>(i, read_buffer);
-					processAccelSample(accel, AccTs);
+					callbacks.processAccelSample(accel, AccTs);
 				}
 			}
 		}
+
+		return fifo_bytes > bytes_to_read;
 	}
 };
 

src/sensors/softfusion/drivers/callbacks.h

@@ -0,0 +1,34 @@
+/*
+	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 <functional>
+
+template <typename SampleType>
+struct DriverCallbacks {
+	std::function<void(const SampleType sample[3], float AccTs)> processAccelSample;
+	std::function<void(const SampleType sample[3], float GyrTs)> processGyroSample;
+	std::function<void(int16_t sample, float TempTs)> processTempSample;
+};

src/sensors/softfusion/drivers/icm42688.h

@@ -27,13 +27,16 @@
 #include <array>
 #include <cstdint>
 
+#include "callbacks.h"
 #include "vqf.h"
 
+constexpr static bool DEBUG_ICM42688_HIRES = false;
+
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
 // Driver uses acceleration range at 8g
 // and gyroscope range at 1000dps
-// Gyroscope ODR = 500Hz, accel ODR = 100Hz
+// Gyroscope ODR = 200Hz, accel ODR = 100Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
 struct ICM42688 {
@@ -41,29 +44,27 @@ struct ICM42688 {
 	static constexpr auto Name = "ICM-42688";
 	static constexpr auto Type = SensorTypeID::ICM42688;
 
-	static constexpr float GyrTs = 1.0 / 500.0;
+	static constexpr float GyrTs = 1.0 / 200.0;
 	static constexpr float AccTs = 1.0 / 100.0;
-	static constexpr float TempTs = 1.0 / 500.0;
+	static constexpr float TempTs = 1.0 / 200.0;
 
 	static constexpr float MagTs = 1.0 / 100;
 
-	static constexpr float GyroSensitivity = 32.8f;
-	static constexpr float AccelSensitivity = 4096.0f;
-
-	static constexpr bool Uses32BitSensorData = true;
+	// When 20-bits data format is used, the only FSR settings that are
+	// operational are ±2000dps for gyroscope and ±16g for accelerometer, even if the
+	// FSR selection register settings are configured for other FSR values. The
+	// corresponding sensitivity scale factor values are 131 LSB/dps for gyroscope and
+	// 8192 LSB/g for accelerometer.
+	static constexpr float GyroSensitivity = (DEBUG_ICM42688_HIRES ? 131.0f : 32.8f);
+	static constexpr float AccelSensitivity
+		= (DEBUG_ICM42688_HIRES ? 8192.0f : 4096.0f);
 
 	static constexpr float TemperatureBias = 25.0f;
-	static constexpr float TemperatureSensitivity = 2.07f;
-
+	static constexpr float TemperatureSensitivity
+		= (DEBUG_ICM42688_HIRES ? 132.48f : 2.07f);
 	static constexpr float TemperatureZROChange = 20.0f;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 0.5f,
-		.biasClip = 1.0f,
-		.restThGyr = 0.5f,
-		.restThAcc = 0.196f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	RegisterInterface& m_RegisterInterface;
 	SlimeVR::Logging::Logger& m_Logger;
@@ -95,12 +96,14 @@ struct ICM42688 {
 			static constexpr uint8_t reg = 0x5f;
 			static constexpr uint8_t value
 				= 0b1 | (0b1 << 1) | (0b1 << 2)
-				| (0b0 << 4);  // fifo accel en=1, gyro=1, temp=1, hires=1
+				| (DEBUG_ICM42688_HIRES ? (0b1 << 4) : (0b0 << 4));
+			// fifo accel en=1, gyro=1, temp=1,
+			// hires=DEBUG_ICM42688_HIRES
 		};
 		struct GyroConfig {
 			static constexpr uint8_t reg = 0x4f;
 			static constexpr uint8_t value
-				= (0b001 << 5) | 0b1111;  // 1000dps, odr=500Hz
+				= (0b001 << 5) | 0b0111;  // 1000dps, odr=200Hz
 		};
 		struct AccelConfig {
 			static constexpr uint8_t reg = 0x50;
@@ -122,7 +125,7 @@ struct ICM42688 {
 	};
 
 #pragma pack(push, 1)
-	struct FifoEntryAligned {
+	struct FifoEntryAlignedHires {
 		union {
 			struct {
 				int16_t accel[3];
@@ -135,10 +138,65 @@ struct ICM42688 {
 			} part;
 			uint8_t raw[19];
 		};
+
+		void getGyro(int32_t out[3]) {
+			// 6.1 Packet Structure for high resolution mode
+			// https://invensense.tdk.com/wp-content/uploads/2020/04/ds-000347_icm-42688-p-datasheet.pdf
+			// When 20-bits data format is used, gyroscope data consists of 19-bits
+			// of actual data and the LSB is always set to 0
+			out[0] = static_cast<int32_t>(part.gyro[0]) << 3 | ((part.xlsb & 0xe) >> 1);
+			out[1] = static_cast<int32_t>(part.gyro[1]) << 3 | ((part.ylsb & 0xe) >> 1);
+			out[2] = static_cast<int32_t>(part.gyro[2]) << 3 | ((part.zlsb & 0xe) >> 1);
+		}
+		void getAccel(int32_t out[3]) {
+			// accelerometer data consists of 18-bits of actual data and the two
+			// lowest order bits are always set to 0
+			out[0] = static_cast<int32_t>(part.accel[0]) << 2
+				   | (static_cast<int32_t>((part.xlsb) & 0xf0) >> 6);
+			out[1] = static_cast<int32_t>(part.accel[1]) << 2
+				   | (static_cast<int32_t>((part.ylsb) & 0xf0) >> 6);
+			out[2] = static_cast<int32_t>(part.accel[2]) << 2
+				   | (static_cast<int32_t>((part.zlsb) & 0xf0) >> 6);
+		}
+	};
+
+	struct FifoEntryAlignedDefault {
+		union {
+			struct {
+				int16_t accel[3];
+				int16_t gyro[3];
+				int8_t temp;
+				uint16_t timestamp;
+			} part;
+			uint8_t raw[15];
+		};
+
+		void getGyro(int32_t out[3]) {
+			out[0] = static_cast<int32_t>(part.gyro[0]);
+			out[1] = static_cast<int32_t>(part.gyro[1]);
+			out[2] = static_cast<int32_t>(part.gyro[2]);
+		}
+
+		void getAccel(int32_t out[3]) {
+			out[0] = static_cast<int32_t>(part.accel[0]);
+			out[1] = static_cast<int32_t>(part.accel[1]);
+			out[2] = static_cast<int32_t>(part.accel[2]);
+		}
 	};
 #pragma pack(pop)
 
+	static_assert(sizeof(FifoEntryAlignedHires) == 19);
+	static_assert(sizeof(FifoEntryAlignedDefault) == 15);
+
+	using FifoEntryAligned = std::conditional<
+		DEBUG_ICM42688_HIRES,
+		FifoEntryAlignedHires,
+		FifoEntryAlignedDefault>::type;
+
 	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
+	// max 4 readings in highres mode 8 readings delay too high 6 seems to be the
+	// edge to work reliably. Tested on ESP8266 with 2 IMU
+	static constexpr size_t MaxReadings = DEBUG_ICM42688_HIRES ? 4 : 8;
 
 	bool initialize() {
 		// perform initialization step
@@ -159,15 +217,11 @@ struct ICM42688 {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTemperatureSample
-	) {
+	bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
-		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
+		std::array<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
+
 		const auto bytes_to_read = std::min(
 									   static_cast<size_t>(read_buffer.size()),
 									   static_cast<size_t>(fifo_bytes)
@@ -183,29 +237,24 @@ struct ICM42688 {
 				sizeof(FifoEntryAligned)
 			);  // skip fifo header
 
-			const int32_t gyroData[3]{
-				static_cast<int32_t>(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf),
-				static_cast<int32_t>(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf),
-				static_cast<int32_t>(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf),
-			};
-			processGyroSample(gyroData, GyrTs);
+			int32_t gyroData[3];
+			entry.getGyro(gyroData);
+			callbacks.processGyroSample(gyroData, GyrTs);
 
 			if (entry.part.accel[0] != -32768) {
-				const int32_t accelData[3]{
-					static_cast<int32_t>(entry.part.accel[0]) << 4
-						| (static_cast<int32_t>(entry.part.xlsb) & 0xf0 >> 4),
-					static_cast<int32_t>(entry.part.accel[1]) << 4
-						| (static_cast<int32_t>(entry.part.ylsb) & 0xf0 >> 4),
-					static_cast<int32_t>(entry.part.accel[2]) << 4
-						| (static_cast<int32_t>(entry.part.zlsb) & 0xf0 >> 4),
-				};
-				processAccelSample(accelData, AccTs);
+				int32_t accelData[3];
+				entry.getAccel(accelData);
+				callbacks.processAccelSample(accelData, AccTs);
 			}
 
 			if (entry.part.temp != 0x8000) {
-				processTemperatureSample(static_cast<int16_t>(entry.part.temp), TempTs);
+				callbacks.processTempSample(
+					static_cast<int16_t>(entry.part.temp),
+					TempTs
+				);
 			}
 		}
+		return fifo_bytes > bytes_to_read;
 	}
 };
 

src/sensors/softfusion/drivers/icm45605.h

@@ -32,20 +32,14 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 4000dps
 // using high resolution mode
 // Uses 32.768kHz clock
-// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
+// Gyroscope ODR = 204.8Hz, accel ODR = 102.4Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
 struct ICM45605 : public ICM45Base {
 	static constexpr auto Name = "ICM-45605";
 	static constexpr auto Type = SensorTypeID::ICM45605;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 0.3f,
-		.biasClip = 0.6f,
-		.restThGyr = 0.3f,
-		.restThAcc = 0.0098f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	ICM45605(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
 		: ICM45Base{registerInterface, logger} {}

src/sensors/softfusion/drivers/icm45686.h

@@ -32,20 +32,14 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 4000dps
 // using high resolution mode
 // Uses 32.768kHz clock
-// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
+// Gyroscope ODR = 204.8Hz, accel ODR = 102.4Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
 struct ICM45686 : public ICM45Base {
 	static constexpr auto Name = "ICM-45686";
 	static constexpr auto Type = SensorTypeID::ICM45686;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 0.5f,
-		.biasClip = 1.0f,
-		.restThGyr = 0.5f,
-		.restThAcc = 0.196f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	ICM45686(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
 		: ICM45Base{registerInterface, logger} {}
@@ -69,8 +63,10 @@ struct ICM45686 : public ICM45Base {
 
 	bool initialize() {
 		ICM45Base::softResetIMU();
+#if IMU_USE_EXTERNAL_CLOCK
 		m_RegisterInterface.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value);
 		m_RegisterInterface.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value);
+#endif
 		return ICM45Base::initializeBase();
 	}
 };

src/sensors/softfusion/drivers/icm45base.h

@@ -25,6 +25,8 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
+#include "sensors/softfusion/magdriver.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
@@ -32,13 +34,13 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 4000dps
 // using high resolution mode
 // Uses 32.768kHz clock
-// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz
+// Gyroscope ODR = 204.8Hz, accel ODR = 102.4Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
 struct ICM45Base {
 	static constexpr uint8_t Address = 0x68;
 
-	static constexpr float GyrTs = 1.0 / 409.6;
+	static constexpr float GyrTs = 1.0 / 204.8;
 	static constexpr float AccTs = 1.0 / 102.4;
 	static constexpr float TempTs = 1.0 / 409.6;
 
@@ -52,8 +54,6 @@ struct ICM45Base {
 
 	static constexpr float TemperatureZROChange = 20.0f;
 
-	static constexpr bool Uses32BitSensorData = true;
-
 	RegisterInterface& m_RegisterInterface;
 	SlimeVR::Logging::Logger& m_Logger;
 	ICM45Base(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
@@ -71,7 +71,7 @@ struct ICM45Base {
 		struct GyroConfig {
 			static constexpr uint8_t reg = 0x1c;
 			static constexpr uint8_t value
-				= (0b0000 << 4) | 0b0111;  // 4000dps, odr=409.6Hz
+				= (0b0000 << 4) | 0b1000;  // 4000dps, odr=204.8Hz
 		};
 
 		struct AccelConfig {
@@ -105,6 +105,77 @@ struct ICM45Base {
 
 		static constexpr uint8_t FifoCount = 0x12;
 		static constexpr uint8_t FifoData = 0x14;
+
+		// Indirect Register Access
+
+		static constexpr uint32_t IRegWaitTimeMicros = 4;
+
+		enum class Bank : uint8_t {
+			IMemSram = 0x00,
+			IPregBar = 0xa0,
+			IPregSys1 = 0xa4,
+			IPregSys2 = 0xa5,
+			IPregTop1 = 0xa2,
+		};
+
+		static constexpr uint8_t IRegAddr = 0x7c;
+		static constexpr uint8_t IRegData = 0x7e;
+
+		// Mag Support
+
+		struct IOCPadScenarioAuxOvrd {
+			static constexpr uint8_t reg = 0x30;
+			static constexpr uint8_t value = (0b1 << 4)  // Enable AUX1 override
+										   | (0b01 << 2)  // Enable I2CM master
+										   | (0b1 << 1)  // Enable AUX1 enable override
+										   | (0b1 << 0);  // Enable AUX1
+		};
+
+		struct I2CMCommand0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x06;
+		};
+
+		struct I2CMDevProfile0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x0e;
+		};
+
+		struct I2CMDevProfile1 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x0f;
+		};
+
+		struct I2CMWrData0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x33;
+		};
+
+		struct I2CMRdData0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x1b;
+		};
+
+		struct DmpExtSenOdrCfg {
+			// TODO: todo
+		};
+
+		struct I2CMControl {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x16;
+		};
+
+		struct I2CMStatus {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x18;
+
+			static constexpr uint8_t SDAErr = 0b1 << 5;
+			static constexpr uint8_t SCLErr = 0b1 << 4;
+			static constexpr uint8_t SRSTErr = 0b1 << 3;
+			static constexpr uint8_t TimeoutErr = 0b1 << 2;
+			static constexpr uint8_t Done = 0b1 << 1;
+			static constexpr uint8_t Busy = 0b1 << 0;
+		};
 	};
 
 #pragma pack(push, 1)
@@ -149,54 +220,53 @@ struct ICM45Base {
 			BaseRegs::PwrMgmt0::reg,
 			BaseRegs::PwrMgmt0::value
 		);
+
+		m_RegisterInterface.writeReg(
+			BaseRegs::IOCPadScenarioAuxOvrd::reg,
+			BaseRegs::IOCPadScenarioAuxOvrd::value
+		);
+
+		read_buffer.resize(FullFifoEntrySize * MaxReadings);
+
 		delay(1);
 
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTemperatureSample
-	) {
-		// Allocate statically so that it does not take up stack space, which
-		// can result in stack overflow and panic
-		constexpr size_t MaxReadings = 8;
-		static std::array<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
+	static constexpr size_t MaxReadings = 8;
+	// Allocate on heap so that it does not take up stack space, which can result in
+	// stack overflow and panic
+	std::vector<uint8_t> read_buffer;
 
+	bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
 		constexpr int16_t InvalidReading = -32768;
 
 		size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount);
 
-		if (fifo_packets >= 1) {
-			//
-			// AN-000364
-			// 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA
-			//
-			// Description: When in FIFO streaming mode, a FIFO empty event
-			// (caused by host reading the last byte of the last FIFO frame) can
-			// cause FIFO data corruption in the first FIFO frame that arrives
-			// after the FIFO empty condition. Once the issue is triggered, the
-			// FIFO state is compromised and cannot recover. FIFO must be set in
-			// bypass mode to flush out the wrong state
-			//
-			// When operating in FIFO streaming mode, if FIFO threshold
-			// interrupt is triggered with M number of FIFO frames accumulated
-			// in the FIFO buffer, the host should only read the first M-1
-			// number of FIFO frames. This prevents the FIFO empty event, that
-			// can cause FIFO data corruption, from happening.
-			//
-			--fifo_packets;
+		if (fifo_packets <= 1) {
+			return false;
 		}
 
-		if (fifo_packets == 0) {
-			return;
-		}
+		// AN-000364
+		// 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA
+		//
+		// Description: When in FIFO streaming mode, a FIFO empty event
+		// (caused by host reading the last byte of the last FIFO frame) can
+		// cause FIFO data corruption in the first FIFO frame that arrives
+		// after the FIFO empty condition. Once the issue is triggered, the
+		// FIFO state is compromised and cannot recover. FIFO must be set in
+		// bypass mode to flush out the wrong state
+		//
+		// When operating in FIFO streaming mode, if FIFO threshold
+		// interrupt is triggered with M number of FIFO frames accumulated
+		// in the FIFO buffer, the host should only read the first M-1
+		// number of FIFO frames. This prevents the FIFO empty event, that
+		// can cause FIFO data corruption, from happening.
+		--fifo_packets;
 
-		fifo_packets = std::min(fifo_packets, MaxReadings);
+		auto packets_to_read = std::min(fifo_packets, MaxReadings);
 
-		size_t bytes_to_read = fifo_packets * FullFifoEntrySize;
+		size_t bytes_to_read = packets_to_read * FullFifoEntrySize;
 		m_RegisterInterface
 			.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
 
@@ -218,7 +288,7 @@ struct ICM45Base {
 					static_cast<int32_t>(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf),
 					static_cast<int32_t>(entry.gyro[2]) << 4 | (entry.lsb[2] & 0xf),
 				};
-				processGyroSample(gyroData, GyrTs);
+				callbacks.processGyroSample(gyroData, GyrTs);
 			}
 
 			if (has_accel && entry.accel[0] != InvalidReading) {
@@ -230,13 +300,139 @@ struct ICM45Base {
 					static_cast<int32_t>(entry.accel[2]) << 4
 						| (static_cast<int32_t>((entry.lsb[2]) & 0xf0) >> 4),
 				};
-				processAccelSample(accelData, AccTs);
+				callbacks.processAccelSample(accelData, AccTs);
 			}
 
 			if (entry.temp != 0x8000) {
-				processTemperatureSample(static_cast<int16_t>(entry.temp), TempTs);
+				callbacks.processTempSample(static_cast<int16_t>(entry.temp), TempTs);
 			}
 		}
+
+		return fifo_packets > MaxReadings;
+	}
+
+	template <typename Reg>
+	uint8_t readBankRegister() {
+		uint8_t buffer;
+		readBankRegister<Reg>(&buffer, sizeof(buffer));
+		return buffer;
+	}
+
+	template <typename Reg, typename T>
+	void readBankRegister(T* buffer, size_t length) {
+		uint8_t data[] = {
+			static_cast<uint8_t>(Reg::bank),
+			Reg::reg,
+		};
+
+		auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
+		m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
+		delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		for (size_t i = 0; i < length * sizeof(T); i++) {
+			bufferBytes[i] = m_RegisterInterface.readReg(BaseRegs::IRegData);
+			delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		}
+	}
+
+	template <typename Reg>
+	void writeBankRegister() {
+		writeBankRegister<Reg>(&Reg::value, sizeof(Reg::value));
+	}
+
+	template <typename Reg, typename T>
+	void writeBankRegister(T* buffer, size_t length) {
+		auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
+
+		uint8_t data[] = {
+			static_cast<uint8_t>(Reg::bank),
+			Reg::reg,
+			bufferBytes[0],
+		};
+
+		m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
+		delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		for (size_t i = 1; i < length * sizeof(T); i++) {
+			m_RegisterInterface.writeReg(BaseRegs::IRegData, bufferBytes[i]);
+			delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		}
+	}
+
+	template <typename Reg>
+	void writeBankRegister(uint8_t value) {
+		writeBankRegister<Reg>(&value, sizeof(value));
+	}
+
+	void setAuxId(uint8_t deviceId) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile1>(deviceId);
+	}
+
+	uint8_t readAux(uint8_t address) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
+
+		writeBankRegister<typename BaseRegs::I2CMCommand0>(
+			(0b1 << 7)  // Last transaction
+			| (0b0 << 6)  // Channel 0
+			| (0b01 << 4)  // Read with register
+			| (0b0001 << 0)  // Read 1 byte
+		);
+		writeBankRegister<typename BaseRegs::I2CMControl>(
+			(0b0 << 6)  // No restarts
+			| (0b0 << 3)  // Fast mode
+			| (0b1 << 0)  // Start transaction
+		);
+
+		uint8_t lastStatus;
+		while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
+			   & BaseRegs::I2CMStatus::Busy)
+			;
+
+		if (lastStatus != BaseRegs::I2CMStatus::Done) {
+			m_Logger.error(
+				"Aux read from address 0x%02x returned status 0x%02x",
+				address,
+				lastStatus
+			);
+		}
+
+		return readBankRegister<typename BaseRegs::I2CMRdData0>();
+	}
+
+	void writeAux(uint8_t address, uint8_t value) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
+		writeBankRegister<typename BaseRegs::I2CMWrData0>(value);
+		writeBankRegister<typename BaseRegs::I2CMCommand0>(
+			(0b1 << 7)  // Last transaction
+			| (0b0 << 6)  // Channel 0
+			| (0b01 << 4)  // Read with register
+			| (0b0001 << 0)  // Read 1 byte
+		);
+		writeBankRegister<typename BaseRegs::I2CMControl>(
+			(0b0 << 6)  // No restarts
+			| (0b0 << 3)  // Fast mode
+			| (0b1 << 0)  // Start transaction
+		);
+
+		uint8_t lastStatus;
+		while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
+			   & BaseRegs::I2CMStatus::Busy)
+			;
+
+		if (lastStatus != BaseRegs::I2CMStatus::Done) {
+			m_Logger.error(
+				"Aux write to address 0x%02x with value 0x%02x returned status 0x%02x",
+				address,
+				value,
+				lastStatus
+			);
+		}
+	}
+
+	void startAuxPolling(uint8_t dataReg, MagDataWidth dataWidth) {
+		// TODO:
+	}
+
+	void stopAuxPolling() {
+		// TODO:
 	}
 };
 

src/sensors/softfusion/drivers/lsm6ds-common.h

@@ -28,6 +28,7 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
@@ -53,11 +54,9 @@ struct LSM6DSOutputHandler {
 
 	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
 
-	template <typename AccelCall, typename GyroCall, typename TempCall, typename Regs>
-	void bulkRead(
-		AccelCall& processAccelSample,
-		GyroCall& processGyroSample,
-		TempCall& processTempSample,
+	template <typename Regs>
+	bool bulkRead(
+		DriverCallbacks<int16_t>&& callbacks,
 		float GyrTs,
 		float AccTs,
 		float TempTs
@@ -94,16 +93,17 @@ struct LSM6DSOutputHandler {
 
 			switch (tag) {
 				case 0x01:  // Gyro NC
-					processGyroSample(entry.xyz, GyrTs);
+					callbacks.processGyroSample(entry.xyz, GyrTs);
 					break;
 				case 0x02:  // Accel NC
-					processAccelSample(entry.xyz, AccTs);
+					callbacks.processAccelSample(entry.xyz, AccTs);
 					break;
 				case 0x03:  // Temperature
-					processTempSample(entry.xyz[0], TempTs);
+					callbacks.processTempSample(entry.xyz[0], TempTs);
 					break;
 			}
 		}
+		return fifo_bytes > bytes_to_read;
 	}
 };
 

src/sensors/softfusion/drivers/lsm6ds3trc.h

@@ -28,41 +28,40 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
-// Driver uses acceleration range at 8g
+// Driver uses acceleration range at 4g
 // and gyroscope range at 1000dps
-// Gyroscope ODR = 416Hz, accel ODR = 416Hz
+// Gyroscope ODR = 208Hz, accel ODR = 104Hz
 
 struct LSM6DS3TRC {
 	static constexpr uint8_t Address = 0x6a;
 	static constexpr auto Name = "LSM6DS3TR-C";
 	static constexpr auto Type = SensorTypeID::LSM6DS3TRC;
 
-	static constexpr float Freq = 416;
+	static constexpr float GyrFreq = 208.0f;
+	static constexpr float AccFreq = 104.0f;
+	static constexpr float MagFreq = 100.0f;
+	static constexpr float TempFreq
+		= 416.0f;  // I guess it's just output at the FIFO ODR?
 
-	static constexpr float GyrTs = 1.0 / Freq;
-	static constexpr float AccTs = 1.0 / Freq;
-	static constexpr float MagTs = 1.0 / Freq;
-	static constexpr float TempTs = 1.0 / Freq;
+	static constexpr float GyrTs = 1.0 / GyrFreq;
+	static constexpr float AccTs = 1.0 / AccFreq;
+	static constexpr float MagTs = 1.0 / MagFreq;
+	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 28.571428571f;
-	static constexpr float AccelSensitivity = 4098.360655738f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
 
 	static constexpr float TemperatureZROChange = 2.0f;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 3.0f,
-		.biasClip = 6.0f,
-		.restThGyr = 3.0f,
-		.restThAcc = 0.392f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	RegisterInterface& m_RegisterInterface;
 	SlimeVR::Logging::Logger m_Logger;
@@ -78,12 +77,12 @@ struct LSM6DS3TRC {
 		};
 		struct Ctrl1XL {
 			static constexpr uint8_t reg = 0x10;
-			static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4);  // 8g, 416Hz
+			static constexpr uint8_t value = (0b10 << 2) | (0b0100 << 4);  // 4g, 104Hz
 		};
 		struct Ctrl2G {
 			static constexpr uint8_t reg = 0x11;
 			static constexpr uint8_t value
-				= (0b10 << 2) | (0b0110 << 4);  // 1000dps, 416Hz
+				= (0b10 << 2) | (0b0101 << 4);  // 1000dps, 208Hz
 		};
 		struct Ctrl3C {
 			static constexpr uint8_t reg = 0x12;
@@ -103,7 +102,7 @@ struct LSM6DS3TRC {
 		struct FifoCtrl5 {
 			static constexpr uint8_t reg = 0x0a;
 			static constexpr uint8_t value
-				= 0b110 | (0b0111 << 3);  // continuous mode, odr = 833Hz
+				= 0b110 | (0b0110 << 3);  // continuous mode, odr = 416Hz
 		};
 
 		static constexpr uint8_t FifoStatus = 0x3a;
@@ -123,19 +122,14 @@ struct LSM6DS3TRC {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTemperatureSample
-	) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto read_result = m_RegisterInterface.readReg16(Regs::FifoStatus);
 		if (read_result & 0x4000) {  // overrun!
 			// disable and re-enable fifo to clear it
 			m_Logger.debug("Fifo overrun, resetting...");
 			m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, 0);
 			m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
-			return;
+			return true;
 		}
 		const auto unread_entries = read_result & 0x7ff;
 		constexpr auto single_measurement_words = 6;
@@ -158,14 +152,13 @@ struct LSM6DS3TRC {
 		);
 		for (uint16_t i = 0; i < bytes_to_read / sizeof(uint16_t);
 			 i += single_measurement_words) {
-			processGyroSample(reinterpret_cast<const int16_t*>(&read_buffer[i]), GyrTs);
-			processAccelSample(
-				reinterpret_cast<const int16_t*>(&read_buffer[i + 3]),
-				AccTs
-			);
-			processTemperatureSample(read_buffer[i + 9], TempTs);
+			callbacks.processGyroSample(&read_buffer[i], GyrTs);
+			callbacks.processAccelSample(&read_buffer[i + 3], AccTs);
+			callbacks.processTempSample(read_buffer[i + 9], TempTs);
 		}
+
+		return static_cast<size_t>(unread_entries) > read_buffer.size();
 	}
-};
+};  // namespace SlimeVR::Sensors::SoftFusion::Drivers
 
 }  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/lsm6dso.h

@@ -27,12 +27,13 @@
 #include <array>
 #include <cstdint>
 
+#include "callbacks.h"
 #include "lsm6ds-common.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
-// Driver uses acceleration range at 8g
+// Driver uses acceleration range at 4g
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 416Hz, accel ODR = 104Hz
 
@@ -41,7 +42,7 @@ struct LSM6DSO : LSM6DSOutputHandler {
 	static constexpr auto Name = "LSM6DSO";
 	static constexpr auto Type = SensorTypeID::LSM6DSO;
 
-	static constexpr float GyrFreq = 416;
+	static constexpr float GyrFreq = 208;
 	static constexpr float AccFreq = 104;
 	static constexpr float MagFreq = 120;
 	static constexpr float TempFreq = 52;
@@ -52,20 +53,14 @@ struct LSM6DSO : LSM6DSOutputHandler {
 	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 1000 / 35.0f;
-	static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
 
 	static constexpr float TemperatureZROChange = 10.0;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 1.0f,
-		.biasClip = 2.0f,
-		.restThGyr = 1.0f,
-		.restThAcc = 0.192f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	struct Regs {
 		struct WhoAmI {
@@ -74,11 +69,11 @@ struct LSM6DSO : LSM6DSOutputHandler {
 		};
 		struct Ctrl1XL {
 			static constexpr uint8_t reg = 0x10;
-			static constexpr uint8_t value = (0b01001100);  // XL at 104 Hz, 8g FS
+			static constexpr uint8_t value = (0b01001000);  // XL at 104 Hz, 4g FS
 		};
 		struct Ctrl2GY {
 			static constexpr uint8_t reg = 0x11;
-			static constexpr uint8_t value = (0b01101000);  // GY at 416 Hz, 1000dps FS
+			static constexpr uint8_t value = (0b01011000);  // GY at 208 Hz, 1000dps FS
 		};
 		struct Ctrl3C {
 			static constexpr uint8_t reg = 0x12;
@@ -89,7 +84,7 @@ struct LSM6DSO : LSM6DSOutputHandler {
 		struct FifoCtrl3BDR {
 			static constexpr uint8_t reg = 0x09;
 			static constexpr uint8_t value
-				= (0b0110) | (0b0110 << 4);  // gyro and accel batched at 417Hz
+				= 0b01010100;  // Gyroscope batched into FIFO at 208Hz, Accel at 104Hz
 		};
 		struct FifoCtrl4Mode {
 			static constexpr uint8_t reg = 0x0a;
@@ -122,16 +117,9 @@ struct LSM6DSO : LSM6DSOutputHandler {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
-		LSM6DSOutputHandler::template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-			processAccelSample,
-			processGyroSample,
-			processTempSample,
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		return LSM6DSOutputHandler::template bulkRead<Regs>(
+			std::move(callbacks),
 			GyrTs,
 			AccTs,
 			TempTs

src/sensors/softfusion/drivers/lsm6dsr.h

@@ -31,16 +31,16 @@
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
-// Driver uses acceleration range at 8g
+// Driver uses acceleration range at 4g
 // and gyroscope range at 1000dps
-// Gyroscope ODR = 416Hz, accel ODR = 104Hz
+// Gyroscope ODR = 208Hz, accel ODR = 104Hz
 
 struct LSM6DSR : LSM6DSOutputHandler {
 	static constexpr uint8_t Address = 0x6a;
 	static constexpr auto Name = "LSM6DSR";
 	static constexpr auto Type = SensorTypeID::LSM6DSR;
 
-	static constexpr float GyrFreq = 416;
+	static constexpr float GyrFreq = 208;
 	static constexpr float AccFreq = 104;
 	static constexpr float MagFreq = 120;
 	static constexpr float TempFreq = 52;
@@ -51,20 +51,14 @@ struct LSM6DSR : LSM6DSOutputHandler {
 	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 1000 / 35.0f;
-	static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
 
 	static constexpr float TemperatureZROChange = 20.0f;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 1.0f,
-		.biasClip = 2.0f,
-		.restThGyr = 1.0f,
-		.restThAcc = 0.192f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	struct Regs {
 		struct WhoAmI {
@@ -73,11 +67,11 @@ struct LSM6DSR : LSM6DSOutputHandler {
 		};
 		struct Ctrl1XL {
 			static constexpr uint8_t reg = 0x10;
-			static constexpr uint8_t value = (0b01001100);  // XL at 104 Hz, 8g FS
+			static constexpr uint8_t value = (0b01001000);  // XL at 104 Hz, 4g FS
 		};
 		struct Ctrl2GY {
 			static constexpr uint8_t reg = 0x11;
-			static constexpr uint8_t value = (0b01101000);  // GY at 416 Hz, 1000dps FS
+			static constexpr uint8_t value = (0b01011000);  // GY at 208 Hz, 1000dps FS
 		};
 		struct Ctrl3C {
 			static constexpr uint8_t reg = 0x12;
@@ -88,7 +82,7 @@ struct LSM6DSR : LSM6DSOutputHandler {
 		struct FifoCtrl3BDR {
 			static constexpr uint8_t reg = 0x09;
 			static constexpr uint8_t value
-				= (0b0110) | (0b0110 << 4);  // gyro and accel batched at 417Hz
+				= 0b01010100;  // Gyroscope batched into FIFO at 208Hz, Accel at 104Hz
 		};
 		struct FifoCtrl4Mode {
 			static constexpr uint8_t reg = 0x0a;
@@ -121,16 +115,9 @@ struct LSM6DSR : LSM6DSOutputHandler {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
-		LSM6DSOutputHandler::template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-			processAccelSample,
-			processGyroSample,
-			processTempSample,
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		return LSM6DSOutputHandler::template bulkRead<Regs>(
+			std::move(callbacks),
 			GyrTs,
 			AccTs,
 			TempTs

src/sensors/softfusion/drivers/lsm6dsv.h

@@ -32,16 +32,16 @@
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
-// Driver uses acceleration range at 8g
+// Driver uses acceleration range at 4g
 // and gyroscope range at 1000dps
-// Gyroscope ODR = 480Hz, accel ODR = 120Hz
+// Gyroscope ODR = 240Hz, accel ODR = 120Hz
 
 struct LSM6DSV : LSM6DSOutputHandler {
 	static constexpr uint8_t Address = 0x6a;
 	static constexpr auto Name = "LSM6DSV";
 	static constexpr auto Type = SensorTypeID::LSM6DSV;
 
-	static constexpr float GyrFreq = 480;
+	static constexpr float GyrFreq = 240;
 	static constexpr float AccFreq = 120;
 	static constexpr float MagFreq = 120;
 	static constexpr float TempFreq = 60;
@@ -52,20 +52,14 @@ struct LSM6DSV : LSM6DSOutputHandler {
 	static constexpr float TempTs = 1.0 / TempFreq;
 
 	static constexpr float GyroSensitivity = 1000 / 35.0f;
-	static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.122f;
 
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 256.0f;
 
 	static constexpr float TemperatureZROChange = 16.667f;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 1.0f,
-		.biasClip = 2.0f,
-		.restThGyr = 1.0f,
-		.restThAcc = 0.192f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	struct Regs {
 		struct WhoAmI {
@@ -82,7 +76,7 @@ struct LSM6DSV : LSM6DSOutputHandler {
 		};
 		struct Ctrl2GODR {
 			static constexpr uint8_t reg = 0x11;
-			static constexpr uint8_t value = (0b0011000);  // 480Hz, HAODR
+			static constexpr uint8_t value = (0b0010111);  // 240Hz, HAODR
 		};
 		struct Ctrl3C {
 			static constexpr uint8_t reg = 0x12;
@@ -96,12 +90,12 @@ struct LSM6DSV : LSM6DSOutputHandler {
 		};
 		struct Ctrl8XLFS {
 			static constexpr uint8_t reg = 0x17;
-			static constexpr uint8_t value = (0b10);  // 8g
+			static constexpr uint8_t value = (0b01);  // 4g
 		};
 		struct FifoCtrl3BDR {
 			static constexpr uint8_t reg = 0x09;
 			static constexpr uint8_t value
-				= (0b1000) | (0b1000 << 4);  // gyro and accel batched at 480Hz
+				= 0b01110110;  // Gyroscope batched into FIFO at 240Hz, Accel at 120Hz
 		};
 		struct FifoCtrl4Mode {
 			static constexpr uint8_t reg = 0x0a;
@@ -137,16 +131,9 @@ struct LSM6DSV : LSM6DSOutputHandler {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
-		LSM6DSOutputHandler::bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-			processAccelSample,
-			processGyroSample,
-			processTempSample,
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		return LSM6DSOutputHandler::template bulkRead<Regs>(
+			std::move(callbacks),
 			GyrTs,
 			AccTs,
 			TempTs

src/sensors/softfusion/drivers/mpu6050.h

@@ -30,6 +30,7 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -69,13 +70,7 @@ struct MPU6050 {
 
 	static constexpr float TemperatureZROChange = 1.6f;
 
-	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 20.0f,
-		.biasClip = 40.0f,
-		.restThGyr = 20.0f,
-		.restThAcc = 0.784f,
-	};
+	static constexpr VQFParams SensorVQFParams{};
 
 	RegisterInterface& m_RegisterInterface;
 	SlimeVR::Logging::Logger& m_Logger;
@@ -181,12 +176,7 @@ struct MPU6050 {
 		return result;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
+	bool bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto status = m_RegisterInterface.readReg(Regs::IntStatus);
 
 		if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) {
@@ -194,7 +184,7 @@ struct MPU6050 {
 			// This necessitates a reset
 			m_Logger.debug("Fifo overrun, resetting...");
 			resetFIFO();
-			return;
+			return true;
 		}
 
 		std::array<uint8_t, 12 * 10>
@@ -204,7 +194,7 @@ struct MPU6050 {
 		auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size())
 					   / sizeof(FifoSample) * sizeof(FifoSample);
 		if (!readBytes) {
-			return;
+			return false;
 		}
 
 		m_RegisterInterface.readBytes(Regs::FifoData, readBytes, readBuffer.data());
@@ -216,14 +206,16 @@ struct MPU6050 {
 			xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x);
 			xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y);
 			xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z);
-			processAccelSample(xyz, AccTs);
+			callbacks.processAccelSample(xyz, AccTs);
 
 			xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x);
 			xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y);
 			xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z);
-			processGyroSample(xyz, GyrTs);
+			callbacks.processGyroSample(xyz, GyrTs);
 		}
+
+		return byteCount > readBytes;
 	}
-};
+};  // namespace SlimeVR::Sensors::SoftFusion::Drivers
 
 }  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/imuconsts.h

@@ -0,0 +1,68 @@
+/*
+	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 <array>
+#include <cstdint>
+#include <type_traits>
+
+#include "../../motionprocessing/types.h"
+#include "drivers/callbacks.h"
+
+template <typename IMU>
+struct IMUConsts {
+	static constexpr bool Uses32BitSensorData
+		= requires(IMU& i, DriverCallbacks<int32_t> callbacks) {
+			  i.bulkRead(std::move(callbacks));
+		  };
+
+	static constexpr bool DirectTempReadOnly = requires(IMU& i) { i.getDirectTemp(); };
+
+	using RawSensorT =
+		typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
+	using RawVectorT = std::array<RawSensorT, 3>;
+
+	static constexpr float GScale
+		= ((32768. / IMU::GyroSensitivity) / 32768.) * (PI / 180.0);
+	static constexpr float AScale = CONST_EARTH_GRAVITY / IMU::AccelSensitivity;
+
+	static constexpr float DirectTempReadFreq = 15;
+	static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq;
+	static constexpr sensor_real_t getDefaultTempTs() {
+		if constexpr (DirectTempReadOnly) {
+			return DirectTempReadTs;
+		} else {
+			return IMU::TempTs;
+		}
+	}
+
+	static constexpr bool SupportsMags = requires(IMU& i) { i.readAux(0x00); };
+	static constexpr bool Supports9ByteMag = []() constexpr {
+		if constexpr (requires { IMU::Supports9ByteMag; }) {
+			return IMU::Supports9ByteMag;
+		} else {
+			return true;
+		}
+	}();
+};

src/sensors/softfusion/magdriver.cpp

@@ -0,0 +1,132 @@
+/*
+	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 "magdriver.h"
+
+namespace SlimeVR::Sensors::SoftFusion {
+
+std::vector<MagDefinition> MagDriver::supportedMags{
+	MagDefinition{
+		.name = "QMC6309",
+
+		.deviceId = 0x7c,
+
+		.whoAmIReg = 0x00,
+		.expectedWhoAmI = 0x90,
+
+		.dataWidth = MagDataWidth::SixByte,
+		.dataReg = 0x01,
+
+		.setup =
+			[](MagInterface& interface) {
+				interface.writeByte(0x0b, 0x80);
+				interface.writeByte(0x0b, 0x00);  // Soft reset
+				delay(10);
+				interface.writeByte(0x0b, 0x48);  // Set/reset on, 8g full range, 200Hz
+				interface.writeByte(
+					0x0a,
+					0x21
+				);  // LP filter 2, 8x Oversampling, normal mode
+				return true;
+			},
+	},
+	MagDefinition{
+		.name = "IST8306",
+
+		.deviceId = 0x19,
+
+		.whoAmIReg = 0x00,
+		.expectedWhoAmI = 0x06,
+
+		.dataWidth = MagDataWidth::SixByte,
+		.dataReg = 0x11,
+
+		.setup =
+			[](MagInterface& interface) {
+				interface.writeByte(0x32, 0x01);  // Soft reset
+				delay(50);
+				interface.writeByte(0x30, 0x20);  // Noise suppression: low
+				interface.writeByte(0x41, 0x2d);  // Oversampling: 32X
+				interface.writeByte(0x31, 0x02);  // Continuous measurement @ 10Hz
+				return true;
+			},
+	},
+};
+
+bool MagDriver::init(MagInterface&& interface, bool supports9ByteMags) {
+	for (auto& mag : supportedMags) {
+		interface.setDeviceId(mag.deviceId);
+
+		logger.info("Trying mag %s!", mag.name);
+
+		uint8_t whoAmI = interface.readByte(mag.whoAmIReg);
+		if (whoAmI != mag.expectedWhoAmI) {
+			continue;
+		}
+
+		if (!supports9ByteMags && mag.dataWidth == MagDataWidth::NineByte) {
+			logger.error("The sensor doesn't support this mag!");
+			return false;
+		}
+
+		logger.info("Found mag %s! Initializing", mag.name);
+
+		if (!mag.setup(interface)) {
+			logger.error("Mag %s failed to initialize!", mag.name);
+			return false;
+		}
+
+		detectedMag = mag;
+
+		break;
+	}
+
+	this->interface = interface;
+	return detectedMag.has_value();
+}
+
+void MagDriver::startPolling() const {
+	if (!detectedMag) {
+		return;
+	}
+
+	interface.startPolling(detectedMag->dataReg, detectedMag->dataWidth);
+}
+
+void MagDriver::stopPolling() const {
+	if (!detectedMag) {
+		return;
+	}
+
+	interface.stopPolling();
+}
+
+const char* MagDriver::getAttachedMagName() const {
+	if (!detectedMag) {
+		return nullptr;
+	}
+
+	return detectedMag->name;
+}
+
+}  // namespace SlimeVR::Sensors::SoftFusion

src/sensors/softfusion/magdriver.h

@@ -0,0 +1,77 @@
+/*
+	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 <functional>
+#include <optional>
+
+#include "logging/Logger.h"
+#include "sensorinterface/RegisterInterface.h"
+
+namespace SlimeVR::Sensors::SoftFusion {
+
+enum class MagDataWidth {
+	SixByte,
+	NineByte,
+};
+
+struct MagInterface {
+	std::function<uint8_t(uint8_t)> readByte;
+	std::function<void(uint8_t, uint8_t)> writeByte;
+	std::function<void(uint8_t)> setDeviceId;
+	std::function<void(uint8_t, MagDataWidth)> startPolling;
+	std::function<void()> stopPolling;
+};
+
+struct MagDefinition {
+	const char* name;
+
+	uint8_t deviceId;
+
+	uint8_t whoAmIReg;
+	uint8_t expectedWhoAmI;
+
+	MagDataWidth dataWidth;
+	uint8_t dataReg;
+
+	std::function<bool(MagInterface& interface)> setup;
+};
+
+class MagDriver {
+public:
+	bool init(MagInterface&& interface, bool supports9ByteMags);
+	void startPolling() const;
+	void stopPolling() const;
+	[[nodiscard]] const char* getAttachedMagName() const;
+
+private:
+	std::optional<MagDefinition> detectedMag;
+	MagInterface interface;
+
+	static std::vector<MagDefinition> supportedMags;
+
+	Logging::Logger logger{"MagDriver"};
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion

src/sensors/softfusion/runtimecalibration/CalibrationStep.h

@@ -46,6 +46,9 @@ public:
 	virtual void cancel() = 0;
 
 	virtual bool requiresRest() { return true; }
+	// Signals that the sensor had more packets than the MCU could read, which can
+	// compromise calibration
+	virtual void signalOverwhelmed() {}
 	virtual void processAccelSample(const SensorRawT accelSample[3]) {}
 	virtual void processGyroSample(const SensorRawT accelSample[3]) {}
 	virtual void processTempSample(float tempSample) {}

src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h

@@ -29,7 +29,6 @@
 
 #include "../../../GlobalVars.h"
 #include "../../../configuration/Configuration.h"
-#include "../../SensorFusionRestDetect.h"
 #include "AccelBiasCalibrationStep.h"
 #include "GyroBiasCalibrationStep.h"
 #include "MotionlessCalibrationStep.h"
@@ -37,31 +36,32 @@
 #include "SampleRateCalibrationStep.h"
 #include "configuration/SensorConfig.h"
 #include "logging/Logger.h"
+#include "sensors/SensorFusion.h"
 #include "sensors/softfusion/CalibrationBase.h"
 
 namespace SlimeVR::Sensors::RuntimeCalibration {
 
-template <typename IMU, typename RawSensorT, typename RawVectorT>
-class RuntimeCalibrator : public Sensor::CalibrationBase<IMU, RawSensorT, RawVectorT> {
+template <typename IMU>
+class RuntimeCalibrator : public Sensors::CalibrationBase<IMU> {
 public:
 	static constexpr bool HasUpsideDownCalibration = false;
 
-	using Base = Sensor::CalibrationBase<IMU, RawSensorT, RawVectorT>;
-	using Self = RuntimeCalibrator<IMU, RawSensorT, RawVectorT>;
+	using Base = Sensors::CalibrationBase<IMU>;
+	using Self = RuntimeCalibrator<IMU>;
+	using Consts = typename Base::Consts;
+	using RawSensorT = typename Consts::RawSensorT;
+	using RawVectorT = typename Consts::RawVectorT;
 
 	RuntimeCalibrator(
-		SensorFusionRestDetect& fusion,
+		SensorFusion& fusion,
 		IMU& imu,
 		uint8_t sensorId,
 		Logging::Logger& logger,
-		float TempTs,
-		float AScale,
-		float GScale,
 		SensorToggleState& toggles
 	)
-		: Base{fusion, imu, sensorId, logger, TempTs, AScale, GScale, toggles} {
-		calibration.T_Ts = TempTs;
-		activeCalibration.T_Ts = TempTs;
+		: Base{fusion, imu, sensorId, logger, toggles} {
+		calibration.T_Ts = Consts::getDefaultTempTs();
+		activeCalibration.T_Ts = Consts::getDefaultTempTs();
 	}
 
 	bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration
@@ -99,7 +99,10 @@ public:
 
 		gyroBiasCalibrationStep.swapCalibrationIfNecessary();
 
-		computeNextCalibrationStep();
+		currentStep = &sampleRateCalibrationStep;
+		currentStep->start();
+		nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE;
+
 		calculateZROChange();
 
 		printCalibration();
@@ -139,10 +142,14 @@ public:
 
 		switch (result) {
 			case CalibrationStep<RawSensorT>::TickResult::DONE:
+				if (nextCalibrationStep == CalibrationStepEnum::SAMPLING_RATE) {
+					stepCalibrationForward(true, false);
+					break;
+				}
 				stepCalibrationForward();
 				break;
 			case CalibrationStep<RawSensorT>::TickResult::SKIP:
-				stepCalibrationForward(false);
+				stepCalibrationForward(false, false);
 				break;
 			case CalibrationStep<RawSensorT>::TickResult::CONTINUE:
 				break;
@@ -152,22 +159,22 @@ public:
 	}
 
 	void scaleAccelSample(sensor_real_t accelSample[3]) final {
-		accelSample[0] = accelSample[0] * AScale - activeCalibration.A_off[0];
-		accelSample[1] = accelSample[1] * AScale - activeCalibration.A_off[1];
-		accelSample[2] = accelSample[2] * AScale - activeCalibration.A_off[2];
+		accelSample[0] = accelSample[0] * Consts::AScale - activeCalibration.A_off[0];
+		accelSample[1] = accelSample[1] * Consts::AScale - activeCalibration.A_off[1];
+		accelSample[2] = accelSample[2] * Consts::AScale - activeCalibration.A_off[2];
 	}
 
 	float getAccelTimestep() final { return activeCalibration.A_Ts; }
 
 	void scaleGyroSample(sensor_real_t gyroSample[3]) final {
 		gyroSample[0] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[0] - activeCalibration.G_off1[0])
+			Consts::GScale * (gyroSample[0] - activeCalibration.G_off1[0])
 		);
 		gyroSample[1] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[1] - activeCalibration.G_off1[1])
+			Consts::GScale * (gyroSample[1] - activeCalibration.G_off1[1])
 		);
 		gyroSample[2] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[2] - activeCalibration.G_off1[2])
+			Consts::GScale * (gyroSample[2] - activeCalibration.G_off1[2])
 		);
 	}
 
@@ -179,6 +186,12 @@ public:
 		return activeCalibration.MotionlessData;
 	}
 
+	void signalOverwhelmed() final {
+		if (isCalibrating) {
+			currentStep->signalOverwhelmed();
+		}
+	}
+
 	void provideAccelSample(const RawSensorT accelSample[3]) final {
 		if (isCalibrating) {
 			currentStep->processAccelSample(accelSample);
@@ -202,12 +215,12 @@ public:
 			activeZROChange = IMU::TemperatureZROChange;
 		}
 
-		float diffX
-			= (activeCalibration.G_off2[0] - activeCalibration.G_off1[0]) * GScale;
-		float diffY
-			= (activeCalibration.G_off2[1] - activeCalibration.G_off1[1]) * GScale;
-		float diffZ
-			= (activeCalibration.G_off2[2] - activeCalibration.G_off1[2]) * GScale;
+		float diffX = (activeCalibration.G_off2[0] - activeCalibration.G_off1[0])
+					* Consts::GScale;
+		float diffY = (activeCalibration.G_off2[1] - activeCalibration.G_off1[1])
+					* Consts::GScale;
+		float diffZ = (activeCalibration.G_off2[2] - activeCalibration.G_off1[2])
+					* Consts::GScale;
 
 		float maxDiff
 			= std::max(std::max(std::abs(diffX), std::abs(diffY)), std::abs(diffZ));
@@ -229,10 +242,7 @@ private:
 	};
 
 	void computeNextCalibrationStep() {
-		if (!calibration.sensorTimestepsCalibrated) {
-			nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE;
-			currentStep = &sampleRateCalibrationStep;
-		} else if (!calibration.motionlessCalibrated && Base::HasMotionlessCalib) {
+		if (!calibration.motionlessCalibrated && Base::HasMotionlessCalib) {
 			nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
 			currentStep = &motionlessCalibrationStep;
 		} else if (calibration.gyroPointsCalibrated == 0) {
@@ -247,7 +257,7 @@ private:
 		}
 	}
 
-	void stepCalibrationForward(bool save = true) {
+	void stepCalibrationForward(bool print = true, bool save = true) {
 		currentStep->cancel();
 		switch (nextCalibrationStep) {
 			case CalibrationStepEnum::NONE:
@@ -255,14 +265,14 @@ private:
 			case CalibrationStepEnum::SAMPLING_RATE:
 				nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
 				currentStep = &motionlessCalibrationStep;
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::TIMESTEPS);
 				}
 				break;
 			case CalibrationStepEnum::MOTIONLESS:
 				nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
 				currentStep = &gyroBiasCalibrationStep;
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::MOTIONLESS);
 				}
 				break;
@@ -274,7 +284,7 @@ private:
 					currentStep = &gyroBiasCalibrationStep;
 				}
 
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::GYRO_BIAS);
 				}
 				break;
@@ -282,7 +292,7 @@ private:
 				nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
 				currentStep = &gyroBiasCalibrationStep;
 
-				if (save) {
+				if (print) {
 					printCalibration(CalibrationPrintFlags::ACCEL_BIAS);
 				}
 
@@ -306,8 +316,6 @@ private:
 		calibration.data.runtimeCalibration = this->calibration;
 		configuration.setSensor(sensorId, calibration);
 		configuration.save();
-
-		ledManager.blink(100);
 	}
 
 	enum class CalibrationPrintFlags {
@@ -323,12 +331,12 @@ private:
 
 	void printCalibration(CalibrationPrintFlags toPrint = PrintAllCalibration) {
 		if (any(toPrint & CalibrationPrintFlags::TIMESTEPS)) {
-			if (calibration.sensorTimestepsCalibrated) {
+			if (activeCalibration.sensorTimestepsCalibrated) {
 				logger.info(
 					"Calibrated timesteps: Accel %f, Gyro %f, Temperature %f",
-					calibration.A_Ts,
-					calibration.G_Ts,
-					calibration.T_Ts
+					activeCalibration.A_Ts,
+					activeCalibration.G_Ts,
+					activeCalibration.T_Ts
 				);
 			} else {
 				logger.info("Sensor timesteps not calibrated");
@@ -389,12 +397,12 @@ private:
 		}
 	}
 
-	CalibrationStepEnum nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
+	CalibrationStepEnum nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE;
 
 	static constexpr float initialStartupDelaySeconds = 5;
 	uint64_t startupMillis = millis();
 
-	SampleRateCalibrationStep<RawSensorT> sampleRateCalibrationStep{calibration};
+	SampleRateCalibrationStep<RawSensorT> sampleRateCalibrationStep{activeCalibration};
 	MotionlessCalibrationStep<IMU, RawSensorT> motionlessCalibrationStep{
 		calibration,
 		sensor
@@ -402,7 +410,7 @@ private:
 	GyroBiasCalibrationStep<RawSensorT> gyroBiasCalibrationStep{calibration};
 	AccelBiasCalibrationStep<RawSensorT> accelBiasCalibrationStep{
 		calibration,
-		static_cast<float>(Base::AScale)
+		static_cast<float>(Consts::AScale)
 	};
 	NullCalibrationStep<RawSensorT> nullCalibrationStep{calibration};
 
@@ -440,9 +448,7 @@ private:
 
 	Configuration::RuntimeCalibrationSensorConfig activeCalibration = calibration;
 
-	using Base::AScale;
 	using Base::fusion;
-	using Base::GScale;
 	using Base::logger;
 	using Base::sensor;
 	using Base::sensorId;

src/sensors/softfusion/runtimecalibration/SampleRateCalibrationStep.h

@@ -67,6 +67,14 @@ public:
 	void cancel() override final { calibrationData.reset(); }
 	bool requiresRest() override final { return false; }
 
+	void signalOverwhelmed() override final {
+		// Not good, restart
+		calibrationData.value().accelSamples = 0;
+		calibrationData.value().gyroSamples = 0;
+		calibrationData.value().tempSamples = 0;
+		calibrationData.value().startMillis = millis();
+	}
+
 	void processAccelSample(const SensorRawT accelSample[3]) override final {
 		calibrationData.value().accelSamples++;
 	}

src/sensors/softfusion/softfusionsensor.h

@@ -23,54 +23,31 @@
 
 #pragma once
 
-#include <cstdint>
-#include <cstdlib>
-#include <tuple>
+#include <PinInterface.h>
 
-#include "../../sensorinterface/i2cimpl.h"
+#include <cstdint>
+#include <cstring>
+
+#include "../../GlobalVars.h"
+#include "../../sensorinterface/SensorInterface.h"
 #include "../RestCalibrationDetector.h"
-#include "../SensorFusionRestDetect.h"
 #include "../sensor.h"
-#include "GlobalVars.h"
-#include "PinInterface.h"
+#include "TempGradientCalculator.h"
+#include "imuconsts.h"
 #include "motionprocessing/types.h"
-#include "sensors/softfusion/TempGradientCalculator.h"
+#include "sensors/SensorFusion.h"
+#include "sensors/softfusion/magdriver.h"
 
 namespace SlimeVR::Sensors {
 
-template <
-	typename SensorType,
-	template <typename IMU, typename RawSensorT, typename RawVectorT>
-	typename Calibrator>
+template <typename SensorType, template <typename IMU> typename Calibrator>
 class SoftFusionSensor : public Sensor {
-	static constexpr sensor_real_t getDefaultTempTs() {
-		if constexpr (DirectTempReadOnly) {
-			return DirectTempReadTs;
-		} else {
-			return SensorType::TempTs;
-		}
-	}
-
-	static constexpr bool Uses32BitSensorData
-		= requires(SensorType& i) { i.Uses32BitSensorData; };
-
-	static constexpr bool DirectTempReadOnly
-		= requires(SensorType& i) { i.getDirectTemp(); };
-
-	using RawSensorT =
-		typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
-	using RawVectorT = std::array<RawSensorT, 3>;
-
-	static constexpr float GScale
-		= ((32768. / SensorType::GyroSensitivity) / 32768.) * (PI / 180.0);
-	static constexpr float AScale = CONST_EARTH_GRAVITY / SensorType::AccelSensitivity;
-
-	using Calib = Calibrator<SensorType, RawSensorT, RawVectorT>;
+	using Consts = IMUConsts<SensorType>;
+	using RawSensorT = typename Consts::RawSensorT;
 
+	using Calib = Calibrator<SensorType>;
 	static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration;
 
-	static constexpr float DirectTempReadFreq = 15;
-	static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq;
 	float lastReadTemperature = 0;
 	uint32_t lastTempPollTime = micros();
 
@@ -107,7 +84,7 @@ class SoftFusionSensor : public Sensor {
 		}
 	}
 
-	void sendData() {
+	void sendData() final {
 		Sensor::sendData();
 		sendTempIfNeeded();
 	}
@@ -155,7 +132,7 @@ class SoftFusionSensor : public Sensor {
 
 	void
 	processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) {
-		if constexpr (!DirectTempReadOnly) {
+		if constexpr (!Consts::DirectTempReadOnly) {
 			const float scaledTemperature
 				= SensorType::TemperatureBias
 				+ static_cast<float>(rawTemperature)
@@ -173,54 +150,6 @@ class SoftFusionSensor : public Sensor {
 		}
 	}
 
-	void eatSamplesForSeconds(const uint32_t seconds) {
-		const auto targetDelay = millis() + 1000 * seconds;
-		auto lastSecondsRemaining = seconds;
-		while (millis() < targetDelay) {
-#ifdef ESP8266
-			ESP.wdtFeed();
-#endif
-			auto currentSecondsRemaining = (targetDelay - millis()) / 1000;
-			if (currentSecondsRemaining != lastSecondsRemaining) {
-				m_Logger.info("%ld...", currentSecondsRemaining + 1);
-				lastSecondsRemaining = currentSecondsRemaining;
-			}
-			m_sensor.bulkRead(
-				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
-				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
-				[](const int16_t xyz, const sensor_real_t timeDelta) {}
-			);
-		}
-	}
-
-	std::tuple<RawVectorT, RawVectorT, int16_t> eatSamplesReturnLast(
-		const uint32_t milliseconds
-	) {
-		RawVectorT accel = {0};
-		RawVectorT gyro = {0};
-		int16_t temp = 0;
-		const auto targetDelay = millis() + milliseconds;
-		while (millis() < targetDelay) {
-			m_sensor.bulkRead(
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					accel[0] = xyz[0];
-					accel[1] = xyz[1];
-					accel[2] = xyz[2];
-				},
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					gyro[0] = xyz[0];
-					gyro[1] = xyz[1];
-					gyro[2] = xyz[2];
-				},
-				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
-					temp = rawTemp;
-				}
-			);
-			yield();
-		}
-		return std::make_tuple(accel, gyro, temp);
-	}
-
 public:
 	static constexpr auto TypeID = SensorType::Type;
 	static constexpr uint8_t Address = SensorType::Address;
@@ -276,12 +205,13 @@ public:
 		// read fifo updating fusion
 		uint32_t now = micros();
 
-		if constexpr (DirectTempReadOnly) {
+		if constexpr (Consts::DirectTempReadOnly) {
 			uint32_t tempElapsed = now - lastTempPollTime;
-			if (tempElapsed >= DirectTempReadTs * 1e6) {
+			if (tempElapsed >= Consts::DirectTempReadTs * 1e6) {
 				lastTempPollTime
 					= now
-					- (tempElapsed - static_cast<uint32_t>(DirectTempReadTs * 1e6));
+					- (tempElapsed
+					   - static_cast<uint32_t>(Consts::DirectTempReadTs * 1e6));
 				lastReadTemperature = m_sensor.getDirectTemp();
 
 				calibrator.provideTempSample(lastReadTemperature);
@@ -289,7 +219,7 @@ public:
 				if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) {
 					tempGradientCalculator.feedSample(
 						lastReadTemperature,
-						DirectTempReadTs
+						Consts::DirectTempReadTs
 					);
 				}
 			}
@@ -308,20 +238,23 @@ public:
 		// send new fusion values when time is up
 		now = micros();
 		constexpr float maxSendRateHz = 100.0f;
-		constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6;
+		constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6f;
 		elapsed = now - m_lastRotationPacketSent;
 		if (elapsed >= sendInterval) {
-			m_sensor.bulkRead(
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					processAccelSample(xyz, timeDelta);
+			auto overwhelmed = m_sensor.bulkRead({
+				[&](const auto sample[3], float AccTs) {
+					processAccelSample(sample, AccTs);
 				},
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					processGyroSample(xyz, timeDelta);
+				[&](const auto sample[3], float GyrTs) {
+					processGyroSample(sample, GyrTs);
 				},
-				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
-					processTempSample(rawTemp, timeDelta);
-				}
-			);
+				[&](int16_t sample, float TempTs) {
+					processTempSample(sample, TempTs);
+				},
+			});
+			if (overwhelmed) {
+				calibrator.signalOverwhelmed();
+			}
 			if (!m_fusion.isUpdated()) {
 				checkSensorTimeout();
 				return;
@@ -395,61 +328,55 @@ public:
 
 		m_status = SensorStatus::SENSOR_OK;
 		working = true;
-		[[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000);
-		if constexpr (UpsideDownCalibrationInit) {
-			auto gravity = static_cast<sensor_real_t>(
-				AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
-			);
-			m_Logger.info(
-				"Gravity read: %.1f (need < -7.5 to start calibration)",
-				gravity
-			);
-			if (gravity < -7.5f) {
-				ledManager.on();
-				m_Logger.info("Flip front in 5 seconds to start calibration");
-				lastRawSample = eatSamplesReturnLast(5000);
-				gravity = static_cast<sensor_real_t>(
-					AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
-				);
-				if (gravity > 7.5f) {
-					m_Logger.debug("Starting calibration...");
-					startCalibration(0);
-				} else {
-					m_Logger.info("Flip not detected. Skipping calibration.");
-				}
 
-				ledManager.off();
+		calibrator.checkStartupCalibration();
+
+		if constexpr (Consts::SupportsMags) {
+			magDriver.init(
+				SoftFusion::MagInterface{
+					.readByte
+					= [&](uint8_t address) { return m_sensor.readAux(address); },
+					.writeByte = [&](uint8_t address, uint8_t value) {},
+					.setDeviceId
+					= [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); },
+					.startPolling
+					= [&](uint8_t dataReg, SoftFusion::MagDataWidth dataWidth
+					  ) { m_sensor.startAuxPolling(dataReg, dataWidth); },
+					.stopPolling = [&]() { m_sensor.stopAuxPolling(); },
+				},
+				Consts::Supports9ByteMag
+			);
+
+			if (toggles.getToggle(SensorToggles::MagEnabled)) {
+				magDriver.startPolling();
 			}
 		}
+
+		toggles.onToggleChange([&](SensorToggles toggle, bool value) {
+			if (toggle == SensorToggles::MagEnabled) {
+				if (value) {
+					magDriver.startPolling();
+				} else {
+					magDriver.stopPolling();
+				}
+			}
+		});
 	}
 
 	void startCalibration(int calibrationType) final {
-		calibrator.startCalibration(
-			calibrationType,
-			[&](const uint32_t seconds) { eatSamplesForSeconds(seconds); },
-			[&](const uint32_t millis) { return eatSamplesReturnLast(millis); }
-		);
+		calibrator.startCalibration(calibrationType);
 	}
 
-	bool isFlagSupported(SensorToggles toggle) const final {
+	[[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final {
 		return toggle == SensorToggles::CalibrationEnabled
 			|| toggle == SensorToggles::TempGradientCalibrationEnabled;
 	}
 
 	SensorStatus getSensorState() final { return m_status; }
 
-	SensorFusionRestDetect m_fusion;
+	SensorFusion m_fusion;
 	SensorType m_sensor;
-	Calib calibrator{
-		m_fusion,
-		m_sensor,
-		sensorId,
-		m_Logger,
-		getDefaultTempTs(),
-		AScale,
-		GScale,
-		toggles
-	};
+	Calib calibrator{m_fusion, m_sensor, sensorId, m_Logger, toggles};
 
 	SensorStatus m_status = SensorStatus::SENSOR_OFFLINE;
 	uint32_t m_lastPollTime = micros();
@@ -459,6 +386,8 @@ public:
 
 	RestCalibrationDetector calibrationDetector;
 
+	SoftFusion::MagDriver magDriver;
+
 	static bool checkPresent(const RegisterInterface& imuInterface) {
 		I2Cdev::readTimeout = 100;
 		auto value = imuInterface.readReg(SensorType::Regs::WhoAmI::reg);
@@ -477,6 +406,10 @@ public:
 			return false;
 		}
 	}
+
+	const char* getAttachedMagnetometer() const final {
+		return magDriver.getAttachedMagName();
+	}
 };
 
 }  // namespace SlimeVR::Sensors

src/serial/serialcommands.cpp

@@ -31,14 +31,38 @@
 #include "logging/Logger.h"
 #include "utils.h"
 
-#if ESP32
+#ifdef ESP32
 #include "nvs_flash.h"
 #endif
 
+#ifdef EXT_SERIAL_COMMANDS
+#define CALLBACK_SIZE 7  // Increase callback size to allow for debug commands
+#include "i2cscan.h"
+#endif
+
+#ifndef CALLBACK_SIZE
+#define CALLBACK_SIZE 6  // Default callback size
+#endif
+
+#if defined(VENDOR_URL) && defined(VENDOR_NAME) && defined(PRODUCT_NAME) \
+	&& defined(UPDATE_ADDRESS) && defined(UPDATE_NAME)
+constexpr const char* FULL_VENDOR_STR
+	= "Vendor: " VENDOR_NAME " (" VENDOR_URL "), product: " PRODUCT_NAME
+	  ", firmware update url: " UPDATE_ADDRESS ", name: " UPDATE_NAME;
+#elif defined(VENDOR_URL) && defined(VENDOR_NAME) && defined(PRODUCT_NAME)
+constexpr const char* FULL_VENDOR_STR
+	= "Vendor: " VENDOR_NAME " (" VENDOR_URL "), product: " PRODUCT_NAME;
+#elif defined(VENDOR_NAME) && defined(PRODUCT_NAME)
+constexpr const char* FULL_VENDOR_STR
+	= "Vendor: " VENDOR_NAME ", product: " PRODUCT_NAME;
+#else
+constexpr const char* FULL_VENDOR_STR = "Vendor: Unknown, product: Unknown";
+#endif
+
 namespace SerialCommands {
 SlimeVR::Logging::Logger logger("SerialCommands");
 
-CmdCallback<6> cmdCallbacks;
+CmdCallback<CALLBACK_SIZE> cmdCallbacks;
 CmdParser cmdParser;
 CmdBuffer<256> cmdBuffer;
 
@@ -85,7 +109,7 @@ void cmdSet(CmdParser* parser) {
 					return;
 				}
 
-				WiFiNetwork::setWiFiCredentials(sc_ssid, sc_pw);
+				wifiNetwork.setWiFiCredentials(sc_ssid, sc_pw);
 				logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting");
 			}
 		} else if (parser->equalCmdParam(1, "BWIFI")) {
@@ -131,7 +155,7 @@ void cmdSet(CmdParser* parser) {
 					// set the pointer for pass to null for no password
 					ppass = NULL;
 				}
-				WiFiNetwork::setWiFiCredentials(ssid, ppass);
+				wifiNetwork.setWiFiCredentials(ssid, ppass);
 				logger.info("CMD SET BWIFI OK: New wifi credentials set, reconnecting");
 			}
 		} else {
@@ -150,11 +174,14 @@ void printState() {
 		HARDWARE_MCU,
 		PROTOCOL_VERSION,
 		FIRMWARE_VERSION,
-		WiFiNetwork::getAddress().toString().c_str(),
+		wifiNetwork.getAddress().toString().c_str(),
 		WiFi.macAddress().c_str(),
 		statusManager.getStatus(),
-		WiFiNetwork::getWiFiState()
+		wifiNetwork.getWiFiState()
 	);
+
+	logger.info("%s", FULL_VENDOR_STR);
+
 	for (auto& sensor : sensorManager.getSensors()) {
 		logger.info(
 			"Sensor[%d]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
@@ -164,6 +191,10 @@ void printState() {
 			sensor->isWorking() ? "true" : "false",
 			sensor->getHadData() ? "true" : "false"
 		);
+		const char* mag = sensor->getAttachedMagnetometer();
+		if (mag) {
+			logger.info("Sensor[%d] magnetometer: %s", sensor->getSensorId(), mag);
+		}
 	}
 	logger.info(
 		"Battery voltage: %.3f, level: %.1f%%",
@@ -172,7 +203,7 @@ void printState() {
 	);
 }
 
-#if ESP32
+#ifdef ESP32
 String getEncryptionTypeName(wifi_auth_mode_t type) {
 	switch (type) {
 		case WIFI_AUTH_OPEN:
@@ -274,10 +305,10 @@ void cmdGet(CmdParser* parser) {
 			HARDWARE_MCU,
 			PROTOCOL_VERSION,
 			FIRMWARE_VERSION,
-			WiFiNetwork::getAddress().toString().c_str(),
+			wifiNetwork.getAddress().toString().c_str(),
 			WiFi.macAddress().c_str(),
 			statusManager.getStatus(),
-			WiFiNetwork::getWiFiState()
+			wifiNetwork.getWiFiState()
 		);
 		auto& sensor0 = sensorManager.getSensors()[0];
 		sensor0->motionLoop();
@@ -288,6 +319,14 @@ void cmdGet(CmdParser* parser) {
 			sensor0->isWorking() ? "true" : "false",
 			sensor0->getHadData() ? "true" : "false"
 		);
+
+		const char* mag = sensor0->getAttachedMagnetometer();
+		if (mag) {
+			logger.info("[TEST] Sensor[0] magnetometer: %s", mag);
+		} else {
+			logger.info("[TEST] Sensor[0] has no magnetometer attached");
+		}
+
 		if (!sensor0->getHadData()) {
 			logger.error("[TEST] Sensor[0] didn't send any data yet!");
 		} else {
@@ -302,8 +341,8 @@ void cmdGet(CmdParser* parser) {
 		if (WiFi.status() != WL_CONNECTED) {
 			WiFi.disconnect();
 		}
-		if (WiFiNetwork::isProvisioning()) {
-			WiFiNetwork::stopProvisioning();
+		if (wifiProvisioning.isProvisioning()) {
+			wifiProvisioning.stopProvisioning();
 		}
 
 		WiFi.scanNetworks();
@@ -346,7 +385,7 @@ void cmdFactoryReset(CmdParser* parser) {
 	WiFi.disconnect(true);  // Clear WiFi credentials
 #if ESP8266
 	ESP.eraseConfig();  // Clear ESP config
-#elif ESP32
+#elif defined(ESP32)
 	nvs_flash_erase();
 #else
 #warning SERIAL COMMAND FACTORY RESET NOT SUPPORTED
@@ -412,6 +451,13 @@ void cmdDeleteCalibration(CmdParser* parser) {
 	configuration.eraseSensors();
 }
 
+#if EXT_SERIAL_COMMANDS
+void cmdScanI2C(CmdParser* parser) {
+	logger.info("Forcing I2C scan...");
+	I2CSCAN::scani2cports();
+}
+#endif
+
 void setUp() {
 	cmdCallbacks.addCmd("SET", &cmdSet);
 	cmdCallbacks.addCmd("GET", &cmdGet);
@@ -419,6 +465,9 @@ void setUp() {
 	cmdCallbacks.addCmd("REBOOT", &cmdReboot);
 	cmdCallbacks.addCmd("DELCAL", &cmdDeleteCalibration);
 	cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration);
+#if EXT_SERIAL_COMMANDS
+	cmdCallbacks.addCmd("SCANI2C", &cmdScanI2C);
+#endif
 }
 
 void update() { cmdCallbacks.updateCmdProcessing(&cmdParser, &cmdBuffer, &Serial); }