Add nrf const to consts.h (#505)

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

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

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

.clang-format

@@ -0,0 +1,24 @@
+---
+BasedOnStyle: Google
+AccessModifierOffset: -4
+AlignAfterOpenBracket: BlockIndent
+AlignOperands: AlignAfterOperator
+AlignTrailingComments: false
+AllowAllArgumentsOnNextLine: false
+AllowAllConstructorInitializersOnNextLine: false
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: Never
+AllowShortLoopsOnASingleLine: false
+BinPackArguments: false
+BinPackParameters: false
+BreakBeforeBinaryOperators: All
+BreakConstructorInitializers: BeforeComma
+BreakInheritanceList: BeforeComma
+ColumnLimit: 88
+DerivePointerAlignment: false
+IndentWidth: 4
+InsertBraces: true
+InsertTrailingCommas: Wrapped
+PackConstructorInitializers: Never
+TabWidth: 4
+UseTab: ForContinuationAndIndentation

.clang-tidy

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

.editorconfig

@@ -0,0 +1,16 @@
+# EditorConfig is awesome: https://EditorConfig.org
+
+# top-most EditorConfig file
+root = true
+
+[*]
+indent_style = tab
+indent_size = 4
+end_of_line = lf
+charset = utf-8
+trim_trailing_whitespace = true
+insert_final_newline = true
+
+# the files need the trailing withe space at the end of = else it does not work
+[{platformio.ini,platformio-tools.ini}]
+trim_trailing_whitespace = false

.github/CODEOWNERS

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

.github/FUNDING.yml

@@ -0,0 +1,3 @@
+# These are supported funding model platforms
+
+github: SlimeVR

.github/dependabot.yml

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

.github/workflows/actions.yml

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

.gitignore

@@ -1,3 +1,10 @@
 .pio
 .vscode/*
 build/
+venv/
+cache/
+.idea/
+compile_commands.json
+node_modules/
+dist/
+.nix-platformio

LICENSE-APACHE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright 2020 Eiren Rain and SlimeVR Contributors
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

LICENSE-MIT

@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2020 Eiren Rain
+Copyright (c) 2020 Eiren Rain and 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

README.md

@@ -23,16 +23,24 @@ The following IMUs and their corresponding `IMU` values are supported by the fir
 * MPU-9250 (IMU_MPU9250)
   * Using Mahony sensor fusion of Gyroscope, Magnetometer and Accelerometer, requires good magnetic environment.
   * See *Sensor calibration* below for info on calibrating this sensor.
-  * Specify `IMU_MPU6500` in your `defines.h` to use without magentometer in 6DoF mode.
+  * Specify `IMU_MPU6500` in your `defines.h` to use without magnetometer in 6DoF mode.
   * Experimental support!
 * BMI160 (IMU_BMI160)
-  * Using Mahony sensor fusion of Gyroscope and Accelerometer
-  * See *Sensor calibration* below for info on calibrating this sensor.
+  * Using sensor fusion of Gyroscope and Accelerometer.
+  * **See Sensor calibration** below for info on calibrating this sensor.
+  * Calibration file format is unstable and may not be able to load using newer firmware versions.
   * Experimental support!
+  * Support for the following magnetometers is implemented (even more experimental): HMC5883L, QMC5883L.
 * ICM-20948 (IMU_ICM20948)
-  * Using fision in internal DMP for 6Dof or 9DoF, 9DoF mode requires good magnetic environment.
+  * Using fusion in internal DMP for 6Dof or 9DoF, 9DoF mode requires good magnetic environment.
   * Comment out `USE_6DOF` in `debug.h` for 9DoF mode.
   * Experimental support!
+* BMI270 (IMU_BMI270), ICM-42688 (IMU_ICM42688), LSM6DS3TR-C (IMU_LSM6DS3TRC), LSM6DSV (IMU_LSM6DSV), LSM6DSO (IMU_LSM6DSO), LSM6DSR (IMU_LSM6DSR), MPU-6050 (IMU_MPU6050_SF)
+  * Using common code: SoftFusionSensor for sensor fusion of Gyroscope and Accelerometer.
+  * Gyro&Accel sample rate, gyroscope offset and 6-side accelerometer calibration supported.
+  * In case of BMI270, gyroscope sensitivity auto-calibration (CRT) is additionally performed.
+  * Support for magnetometers is currently not implemented.
+  * VERY experimental support!
 
 Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set your pinout properly according to how you connected the IMU.
 
@@ -41,19 +49,76 @@ Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set y
 *It is generally recommended to turn trackers on and let them lay down on a flat surface for a few seconds.** This will calibrate them better.
 
 **Some trackers require special calibration steps on startup:**
-* MPU-9250, BMI160
+### MPU-9250
   * Turn them on with chip facing down. Flip up and put on a surface for a couple of seconds, the LED will light up.
   * After a few blinks, the LED will light up again
-  * Slowly rotate the tracker in an 8-motion facing different derections for about 30 seconds, while LED is blinking
+  * Slowly rotate the tracker in an 8-motion facing different directions for about 30 seconds, while LED is blinking
   * LED will turn off when calibration is complete
-  * You don't have to calibrate next time you power it off, calibration values will be saved for the next use
+  * You don't have to calibrate next time you power it on, calibration values will be saved for the next use
+
+### BMI160
+
+  If you have any problems with this procedure, connect the device via USB and open the serial console to check for any warnings or errors that may indicate hardware problems.
+
+  - **Step 0: Power up with the chip facing down.** Or press the reset/reboot button.
+
+    > The LED will be lit continuously. If you have the tracker connected via USB and open the serial console, you will see text prompts in addition to the LEDs. You can only calibrate 1 IMU at a time.
+
+    Flip it back up while the LED is still solid. Wait a few seconds, do not touch the device.
+    
+  - **Step 1: It will flash 3 times when gyroscope calibration begins.**
+
+    > If done incorrectly, this step is the most likely source of constant drift.
+
+    Make sure the tracker does not move or vibrate for 5 seconds - still do not touch it.
+
+  - **Step 2: It will flash 6 times when accelerometer calibration begins.**
+
+    > The accelerometer calibration process requires you to **hold the device in 6 unique orientations** (e.g. sides of a cube),
+    > keep it still, and not hold or touch for 3 seconds each. It uses gravity as a reference and automatically detects when it is stabilized - this process is not time-sensitive.
+
+    > If you are unable to keep it on a flat surface without touching, press the device against a wall, it does not have to be absolutely perfect.
+
+    **There will be two very short blinks when each position is recorded.**
+    
+    Rotate the device 90 or 180 degrees in any direction. It should be on a different side each time. Continue to rotate until all 6 sides have been recorded.
+    
+    The last position has a long flash when recorded, indicating exit from calibration mode.
+
+  #### Additional info for BMI160
+  - For best results, **calibrate when the trackers are warmed up** - put them on for a few minutes,
+    wait for the temperature to stabilize at 30-40 degrees C, then calibrate.
+    Enable developer mode in SlimeVR settings to see tracker temperature.
+
+  - There is a legacy accelerometer calibration method that collects data during in-place rotation by holding it in hand instead.
+    If you are absolutely unable to use the default 6-point calibration method, you can switch it in config file `defines_bmi160.h`.
+
+  - For faster recalibration, you disable accelerometer calibration by setting `BMI160_ACCEL_CALIBRATION_METHOD` option to `ACCEL_CALIBRATION_METHOD_SKIP` in `defines_bmi160.h`.
+    Accelerometer calibration can be safely skipped if you don't have issues with pitch and roll.
+    You can check it by enabling developer mode in SlimeVR settings (*General / Interface*) and going back to the *"Home"* tab.
+    Press *"Preview"* button inside the tracker settings (of each tracker) to show the IMU visualizer.
+    Check if pitch/roll resembles its real orientation.
+
+  - Calibration data is written to the flash of your MCU and is unique for each BMI160, keep that in mind if you have detachable aux trackers.
 
 ## Uploading On Linux
 
-Follow the instructions in this link [Platformio](https://docs.platformio.org/en/latest//faq.html#platformio-udev-rules), this should solve any permission denied errors
+Follow the instructions in this link [PlatformIO](https://docs.platformio.org/en/latest//faq.html#platformio-udev-rules), this should solve any permission denied errors
+
 
 ## Contributions
+Any contributions submitted for inclusion in this repository will be dual-licensed under
+either:
 
-By contributing to this project you are placing all your code under MIT or less restricting licenses, and you certify that the code you have used is compatible with those licenses or is authored by you. If you're doing so on your work time, you certify that your employer is okay with this.
+- MIT License ([LICENSE-MIT](/LICENSE-MIT))
+- Apache License, Version 2.0 ([LICENSE-APACHE](/LICENSE-APACHE))
+
+Unless you explicitly state otherwise, any contribution intentionally submitted for
+inclusion in the work by you, as defined in the Apache-2.0 license, shall be dual
+licensed as above, without any additional terms or conditions.
+
+You also certify that the code you have used is compatible with those licenses or is
+authored by you. If you're doing so on your work time, you certify that your employer is
+okay with this and that you are authorized to provide the above licenses.
 
 For an explanation on how to contribute, see [`CONTRIBUTING.md`](CONTRIBUTING.md)

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

@@ -1,9 +1,11 @@
 import json
 import os
+import sys
 import shutil
 from enum import Enum
 from textwrap import dedent
 from typing import List
+import configparser
 
 COLOR_ESC = '\033['
 COLOR_RESET = f'{COLOR_ESC}0m'
@@ -13,136 +15,52 @@ COLOR_CYAN = f'{COLOR_ESC}36m'
 COLOR_GRAY = f'{COLOR_ESC}30;1m'
 
 
-class Board(Enum):
-    SLIMEVR = "BOARD_SLIMEVR"
-    WROOM32 = "BOARD_WROOM32"
-
-
 class DeviceConfiguration:
-    def __init__(self,  platform: str, board: Board, platformio_board: str) -> None:
+    def __init__(self,  platform: str, board: str, platformio_board: str) -> None:
         self.platform = platform
         self.board = board
         self.platformio_board = platformio_board
 
-    def get_platformio_section(self) -> str:
-        section = dedent(f"""
-        [env:{self.platformio_board}]
-        platform = {self.platform}
-        board = {self.platformio_board}""")
-
-        if self.platform == "espressif32 @ 3.5.0":
-            section += dedent("""
-            lib_deps =
-                ${env.lib_deps}
-                lorol/LittleFS_esp32 @ 1.0.6
-            """)
-
-        return section
-
     def filename(self) -> str:
-        return f"{self.platformio_board}.bin"
-
-    def build_header(self) -> str:
-        sda = ""
-        scl = ""
-        imu_int = ""
-        imu_int2 = ""
-        battery_level = ""
-        led_pin = 2
-        led_invert = False
-
-        if self.board == Board.SLIMEVR:
-            sda = "4"
-            scl = "5"
-            imu_int = "10"
-            imu_int2 = "13"
-            battery_level = "17"
-            led_invert = True
-        elif self.board == Board.WROOM32:
-            sda = "21"
-            scl = "22"
-            imu_int = "23"
-            imu_int2 = "25"
-            battery_level = "36"
-        else:
-            raise Exception(f"Unknown board: {self.board.value}")
-
-        return f"""
-#define IMU IMU_BNO085
-#define SECOND_IMU IMU
-#define BOARD {self.board.value}
-#define IMU_ROTATION DEG_90
-#define SECOND_IMU_ROTATION DEG_90
-
-#define BATTERY_MONITOR BAT_EXTERNAL
-#define BATTERY_SHIELD_RESISTANCE 180
-
-#define PIN_IMU_SDA {sda}
-#define PIN_IMU_SCL {scl}
-#define PIN_IMU_INT {imu_int}
-#define PIN_IMU_INT_2 {imu_int2}
-#define PIN_BATTERY_LEVEL {battery_level}
-#define LED_PIN {led_pin}
-#define LED_INVERTED {led_invert.__str__().lower()}
-"""
+        return f"{self.board}-firmware.bin"
 
     def __str__(self) -> str:
-        return f"{self.platform}@{self.board.value}"
+        return f"{self.platform}@{self.board}"
 
 
 def get_matrix() -> List[DeviceConfiguration]:
     matrix: List[DeviceConfiguration] = []
 
-    configFile = open("./ci/devices.json", "r")
-    config = json.load(configFile)
+    config = configparser.ConfigParser()
+    config.read("./platformio.ini")
+    for section in config.sections():
+        split = section.split(":")
+        if len(split) != 2 or split[0] != 'env':
+            continue
+
+        board = split[1]
+        platform = config[section]["platform"]
+        platformio_board = config[section]["board"]
 
-    for deviceConfig in config:
         matrix.append(DeviceConfiguration(
-            deviceConfig["platform"], Board[deviceConfig["board"]], deviceConfig["platformio_board"]))
+            platform,
+            board,
+            platformio_board))
 
     return matrix
 
 
 def prepare() -> None:
     print(f"🡢 {COLOR_CYAN}Preparation{COLOR_RESET}")
-
-    print(f"  🡢 {COLOR_GRAY}Backing up src/defines.h{COLOR_RESET}")
-    shutil.copy("src/defines.h", "src/defines.h.bak")
-
-    print(f"  🡢 {COLOR_GRAY}Backing up platformio.ini{COLOR_RESET}")
-    shutil.copy("./platformio.ini", "platformio.ini.bak")
-
-    print(f"  🡢 {COLOR_GRAY}Copying over build/platformio.ini{COLOR_RESET}")
-    shutil.copy("./ci/platformio.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 src/defines.h...{COLOR_RESET}")
-    shutil.copy("src/defines.h.bak", "src/defines.h")
-
-    print(f"  🡢 {COLOR_GRAY}Removing src/defines.h.bak...{COLOR_RESET}")
-    os.remove("src/defines.h.bak")
-
-    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}")
 
@@ -151,17 +69,13 @@ def build() -> int:
 
     matrix = get_matrix()
 
-    with open("./platformio.ini", "a") as f1:
-        for device in matrix:
-            f1.write(device.get_platformio_section())
-
     for device in matrix:
         print(f"  🡢 {COLOR_CYAN}Building for {device.platform}{COLOR_RESET}")
 
         status = build_for_device(device)
 
-        if status == False:
-            failed_builds.append(device.platformio_board)
+        if not status:
+            failed_builds.append(device.board)
 
     if len(failed_builds) > 0:
         print(f"  🡢 {COLOR_RED}Failed!{COLOR_RESET}")
@@ -181,14 +95,10 @@ def build_for_device(device: DeviceConfiguration) -> bool:
 
     print(f"::group::Build {device}")
 
-    with open("src/defines.h", "wt") as f:
-        f.write(device.build_header())
-
-    code = os.system(
-        f"platformio run -e {device.platformio_board}")
+    code = os.system(f"platformio run -e {device.board}")
 
     if code == 0:
-        shutil.copy(f".pio/build/{device.platformio_board}/firmware.bin",
+        shutil.copy(f".pio/build/{device.board}/firmware.bin",
                     f"build/{device.filename()}")
 
         print(f"    🡢 {COLOR_GREEN}Success!{COLOR_RESET}")
@@ -197,7 +107,7 @@ def build_for_device(device: DeviceConfiguration) -> bool:
 
         print(f"    🡢 {COLOR_RED}Failed!{COLOR_RESET}")
 
-    print(f"::endgroup::")
+    print("::endgroup::")
 
     return success
 
@@ -205,9 +115,8 @@ def build_for_device(device: DeviceConfiguration) -> bool:
 def main() -> None:
     prepare()
     code = build()
-    cleanup()
 
-    os._exit(code)
+    sys.exit(code)
 
 
 if __name__ == "__main__":

ci/devices.json

@@ -1,12 +0,0 @@
-[
-  {
-    "platform": "espressif8266",
-    "platformio_board": "esp12e",
-    "board": "SLIMEVR"
-  },
-  {
-    "platform": "espressif32 @ 3.5.0",
-    "platformio_board": "esp32dev",
-    "board": "WROOM32"
-  }
-]

ci/platformio.ini

@@ -1,8 +0,0 @@
-[env]
-lib_deps=
-  https://github.com/SlimeVR/CmdParser.git
-monitor_speed = 115200
-framework = arduino
-build_flags =
-  -O2
-build_unflags = -Os

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/.clang-format

@@ -0,0 +1,3 @@
+---
+DisableFormat: true
+SortIncludes: Never

lib/ICM20948/ICM_20948.cpp

@@ -240,7 +240,7 @@ float ICM_20948::getGyrDPS(int16_t axis_val)
 }
 
 //Gyro Bias
-ICM_20948_Status_e ICM_20948::SetBiasGyroX( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasGyroX( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char gyro_bias_reg[4];
@@ -252,7 +252,7 @@ ICM_20948_Status_e ICM_20948::SetBiasGyroX( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasGyroY( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasGyroY( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char gyro_bias_reg[4];
@@ -264,7 +264,7 @@ ICM_20948_Status_e ICM_20948::SetBiasGyroY( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char gyro_bias_reg[4];
@@ -276,34 +276,34 @@ ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasGyroX( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasGyroX( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasGyroY( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasGyroY( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 //Accel Bias
-ICM_20948_Status_e ICM_20948::SetBiasAccelX( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasAccelX( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char accel_bias_reg[4];
@@ -315,7 +315,7 @@ ICM_20948_Status_e ICM_20948::SetBiasAccelX( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasAccelY( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasAccelY( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char accel_bias_reg[4];
@@ -327,7 +327,7 @@ ICM_20948_Status_e ICM_20948::SetBiasAccelY( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char accel_bias_reg[4];
@@ -339,34 +339,34 @@ ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasAccelX( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasAccelX( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasAccelY( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasAccelY( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 //CPass Bias
-ICM_20948_Status_e ICM_20948::SetBiasCPassX( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasCPassX( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char cpass_bias_reg[4];
@@ -378,7 +378,7 @@ ICM_20948_Status_e ICM_20948::SetBiasCPassX( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasCPassY( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasCPassY( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char cpass_bias_reg[4];
@@ -390,7 +390,7 @@ ICM_20948_Status_e ICM_20948::SetBiasCPassY( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int newValue)
+ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char cpass_bias_reg[4];
@@ -402,30 +402,30 @@ ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasCPassX( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasCPassX( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasCPassY( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasCPassY( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int* bias)
+ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
-    bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
     return result;
 }
 

lib/ICM20948/ICM_20948.h

@@ -174,26 +174,26 @@ public:
   //DMP
 
   //Gyro Bias
-  ICM_20948_Status_e SetBiasGyroX( int newValue);
-  ICM_20948_Status_e SetBiasGyroY( int newValue);
-  ICM_20948_Status_e SetBiasGyroZ( int newValue);
-  ICM_20948_Status_e GetBiasGyroX( int* bias);
-  ICM_20948_Status_e GetBiasGyroY( int* bias);
-  ICM_20948_Status_e GetBiasGyroZ( int* bias);
+  ICM_20948_Status_e SetBiasGyroX( int32_t newValue);
+  ICM_20948_Status_e SetBiasGyroY( int32_t newValue);
+  ICM_20948_Status_e SetBiasGyroZ( int32_t newValue);
+  ICM_20948_Status_e GetBiasGyroX( int32_t* bias);
+  ICM_20948_Status_e GetBiasGyroY( int32_t* bias);
+  ICM_20948_Status_e GetBiasGyroZ( int32_t* bias);
   //Accel Bias
-  ICM_20948_Status_e SetBiasAccelX( int newValue);
-  ICM_20948_Status_e SetBiasAccelY( int newValue);
-  ICM_20948_Status_e SetBiasAccelZ( int newValue);
-  ICM_20948_Status_e GetBiasAccelX( int* bias);
-  ICM_20948_Status_e GetBiasAccelY( int* bias);
-  ICM_20948_Status_e GetBiasAccelZ( int* bias);
+  ICM_20948_Status_e SetBiasAccelX( int32_t newValue);
+  ICM_20948_Status_e SetBiasAccelY( int32_t newValue);
+  ICM_20948_Status_e SetBiasAccelZ( int32_t newValue);
+  ICM_20948_Status_e GetBiasAccelX( int32_t* bias);
+  ICM_20948_Status_e GetBiasAccelY( int32_t* bias);
+  ICM_20948_Status_e GetBiasAccelZ( int32_t* bias);
   //CPass Bias
-  ICM_20948_Status_e SetBiasCPassX( int newValue);
-  ICM_20948_Status_e SetBiasCPassY( int newValue);
-  ICM_20948_Status_e SetBiasCPassZ( int newValue);
-  ICM_20948_Status_e GetBiasCPassX( int* bias);
-  ICM_20948_Status_e GetBiasCPassY( int* bias);
-  ICM_20948_Status_e GetBiasCPassZ( int* bias);
+  ICM_20948_Status_e SetBiasCPassX( int32_t newValue);
+  ICM_20948_Status_e SetBiasCPassY( int32_t newValue);
+  ICM_20948_Status_e SetBiasCPassZ( int32_t newValue);
+  ICM_20948_Status_e GetBiasCPassX( int32_t* bias);
+  ICM_20948_Status_e GetBiasCPassY( int32_t* bias);
+  ICM_20948_Status_e GetBiasCPassZ( int32_t* bias);
 
   // Done:
   //  Configure DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL

lib/bmi160/BMI160.h

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

lib/bno080/BNO080.cpp

@@ -12,7 +12,6 @@
 
   This library handles the initialization of the BNO080 and is able to query the sensor
   for different readings.
-
   https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library
 
   Development environment specifics:
@@ -43,14 +42,14 @@
 
 //Attempt communication with the device
 //Return true if we got a 'Polo' back from Marco
-boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, uint8_t intPin)
+boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, PinInterface* intPin)
 {
 	_deviceAddress = deviceAddress; //If provided, store the I2C address from user
 	_i2cPort = &wirePort;			//Grab which port the user wants us to use
 	_int = intPin;					//Get the pin that the user wants to use for interrupts. By default, it's 255 and we'll not use it in dataAvailable() function.
-	if (_int != 255)
+	if (_int != nullptr)
 	{
-		pinMode(_int, INPUT_PULLUP);
+		_int->pinMode(INPUT_PULLUP);
 	}
 
 	//We expect caller to begin their I2C port, with the speed of their choice external to the library
@@ -67,18 +66,21 @@ boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, uint8_t intPin)
 	//Transmit packet on channel 2, 2 bytes
 	sendPacket(CHANNEL_CONTROL, 2);
 
-	//Now we wait for response
-	if (receivePacket() == true)
-	{
-		if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
-		{
+	uint32_t tInitialResetTimeMS = millis();
+	bool tBoardInfoReceived = false;
+
+	// Wait max 2.5s for the product_id_response and ignore other packets received during that time.
+	while (millis() - tInitialResetTimeMS < 2500 && (!tBoardInfoReceived)) {
+		receivePacket();
+		if (shtpHeader[2] == 2 && shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) {
+			tBoardInfoReceived = true;
+			swMajor = shtpData[2];
+			swMinor = shtpData[3];
+			swPartNumber = ((uint32_t)shtpData[7] << 24) | ((uint32_t)shtpData[6] << 16) | ((uint32_t)shtpData[5] << 8) | ((uint32_t)shtpData[4]);
+			swBuildNumber = ((uint32_t)shtpData[11] << 24) | ((uint32_t)shtpData[10] << 16) | ((uint32_t)shtpData[9] << 8) | ((uint32_t)shtpData[8]);
+			swVersionPatch = ((uint16_t)shtpData[13] << 8) | ((uint16_t)shtpData[12]);
 			if (_printDebug == true)
 			{
-				swMajor = shtpData[2];
-				swMinor = shtpData[3];
-				swPartNumber = ((uint32_t)shtpData[7] << 24) | ((uint32_t)shtpData[6] << 16) | ((uint32_t)shtpData[5] << 8) | ((uint32_t)shtpData[4]);
-				swBuildNumber = ((uint32_t)shtpData[11] << 24) | ((uint32_t)shtpData[10] << 16) | ((uint32_t)shtpData[9] << 8) | ((uint32_t)shtpData[8]);
-				swVersionPatch = ((uint16_t)shtpData[13] << 8) | ((uint16_t)shtpData[12]);
 				_debugPort->print(F("SW Version Major: 0x"));
 				_debugPort->print(swMajor, HEX);
 				_debugPort->print(F(" SW Version Minor: 0x"));
@@ -90,14 +92,13 @@ boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, uint8_t intPin)
 				_debugPort->print(F(" SW Version Patch: 0x"));
 				_debugPort->println(swVersionPatch, HEX);
 			}
-			return (true);
 		}
 	}
 
-	return (false); //Something went wrong
+	return tBoardInfoReceived;
 }
 
-boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_INTPin, uint8_t user_RSTPin, uint32_t spiPortSpeed, SPIClass &spiPort)
+boolean BNO080::beginSPI(PinInterface* user_CSPin, PinInterface* user_WAKPin, PinInterface* user_INTPin, PinInterface* user_RSTPin, uint32_t spiPortSpeed, SPIClass &spiPort)
 {
 	_i2cPort = NULL; //This null tells the send/receive functions to use SPI
 
@@ -112,18 +113,18 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I
 	_int = user_INTPin;
 	_rst = user_RSTPin;
 
-	pinMode(_cs, OUTPUT);
-	pinMode(_wake, OUTPUT);
-	pinMode(_int, INPUT_PULLUP);
-	pinMode(_rst, OUTPUT);
+	_cs->pinMode(OUTPUT);
+	_wake->pinMode(OUTPUT);
+	_int->pinMode(INPUT_PULLUP);
+	_rst->pinMode(OUTPUT);
 
-	digitalWrite(_cs, HIGH); //Deselect BNO080
+	_cs->digitalWrite(HIGH); //Deselect BNO080
 
 	//Configure the BNO080 for SPI communication
-	digitalWrite(_wake, HIGH); //Before boot up the PS0/WAK pin must be high to enter SPI mode
-	digitalWrite(_rst, LOW);   //Reset BNO080
+	_wake->digitalWrite(HIGH); //Before boot up the PS0/WAK pin must be high to enter SPI mode
+	_rst->digitalWrite(LOW);   //Reset BNO080
 	delay(2);				   //Min length not specified in datasheet?
-	digitalWrite(_rst, HIGH);  //Bring out of reset
+	_rst->digitalWrite(HIGH);  //Bring out of reset
 
 	//Wait for first assertion of INT before using WAK pin. Can take ~104ms
 	waitForSPI();
@@ -157,6 +158,7 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I
 	if (receivePacket() == true)
 	{
 		if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
+		{
 			if (_printDebug == true)
 			{
 				_debugPort->print(F("SW Version Major: 0x"));
@@ -174,6 +176,7 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I
 				_debugPort->println(SW_Version_Patch, HEX);
 			}
 			return (true);
+		}
 	}
 
 	return (false); //Something went wrong
@@ -199,9 +202,9 @@ uint16_t BNO080::getReadings(void)
 	//If we have an interrupt pin connection available, check if data is available.
 	//If int pin is not set, then we'll rely on receivePacket() to timeout
 	//See issue 13: https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/issues/13
-	if (_int != 255)
+	if (_int != nullptr)
 	{
-		if (digitalRead(_int) == HIGH)
+		if (_int->digitalRead() == HIGH)
 			return 0;
 	}
 
@@ -252,6 +255,13 @@ uint16_t BNO080::parseCommandReport(void)
 		if (command == COMMAND_ME_CALIBRATE)
 		{
 			calibrationStatus = shtpData[5 + 0]; //R0 - Status (0 = success, non-zero = fail)
+			_calibrationResponseStatus = shtpData[5 + 0];
+			_accelCalEnabled = shtpData[6 + 0];
+			_gyroCalEnabled = shtpData[7 + 0];
+			_magCalEnabled = shtpData[8 + 0];
+			_planarAccelCalEnabled = shtpData[9 + 0];
+			_onTableCalEnabled = shtpData[10 + 0];
+			_hasNewCalibrationStatus = true;
 		}
 		return shtpData[0];
 	}
@@ -274,12 +284,13 @@ uint16_t BNO080::parseCommandReport(void)
 //shtpData[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector)
 //shtpData[5 + 1]: Sequence number (See 6.5.18.2)
 //shtpData[5 + 2]: Status
-//shtpData[3]: Delay
-//shtpData[4:5]: i/accel x/gyro x/etc
-//shtpData[6:7]: j/accel y/gyro y/etc
-//shtpData[8:9]: k/accel z/gyro z/etc
-//shtpData[10:11]: real/gyro temp/etc
-//shtpData[12:13]: Accuracy estimate
+//shtpData[5 + 3]: Delay
+//shtpData[5 + 4:5]: i/accel x/gyro x/etc
+//shtpData[5 + 6:7]: j/accel y/gyro y/etc
+//shtpData[5 + 8:9]: k/accel z/gyro z/etc
+//shtpData[5 + 10:11]: real/gyro temp/etc
+//shtpData[5 + 12:13]: Accuracy estimate: Raw Accel/Gyro/Mag Timestap part1
+//shtpData[5 + 14:15]: Raw Accel/Gyro/Mag Timestap part2
 uint16_t BNO080::parseInputReport(void)
 {
 	//Calculate the number of data bytes in this packet
@@ -300,7 +311,7 @@ uint16_t BNO080::parseInputReport(void)
 		rawFastGyroX = (uint16_t)shtpData[9] << 8 | shtpData[8];
 		rawFastGyroY = (uint16_t)shtpData[11] << 8 | shtpData[10];
 		rawFastGyroZ = (uint16_t)shtpData[13] << 8 | shtpData[12];
-
+		hasNewFastGyro_ = true;
 		return SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR;
 	}
 
@@ -310,6 +321,7 @@ uint16_t BNO080::parseInputReport(void)
 	uint16_t data3 = (uint16_t)shtpData[5 + 9] << 8 | shtpData[5 + 8];
 	uint16_t data4 = 0;
 	uint16_t data5 = 0; //We would need to change this to uin32_t to capture time stamp value on Raw Accel/Gyro/Mag reports
+	uint32_t memstimeStamp = 0; //Timestamp of MEMS sensor reading
 
 	if (dataLength - 5 > 9)
 	{
@@ -319,6 +331,11 @@ uint16_t BNO080::parseInputReport(void)
 	{
 		data5 = (uint16_t)shtpData[5 + 13] << 8 | shtpData[5 + 12];
 	}
+	//only for Raw Reports 0x14, 0x15, 0x16
+	if (dataLength - 5 >= 15)
+	{
+		memstimeStamp = ((uint32_t)shtpData[5 + 15] << (8 * 3)) | ((uint32_t)shtpData[5 + 14] << (8 * 2)) | ((uint32_t)shtpData[5 + 13] << (8 * 1)) | ((uint32_t)shtpData[5 + 12] << (8 * 0));
+	}
 
 	//Store these generic values to their proper global variable
 	if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER || shtpData[5] == SENSOR_REPORTID_GRAVITY)
@@ -331,6 +348,7 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_LINEAR_ACCELERATION)
 	{
+		hasNewLinAccel_ = true;
 		accelLinAccuracy = status;
 		rawLinAccelX = data1;
 		rawLinAccelY = data2;
@@ -338,6 +356,7 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE)
 	{
+		hasNewGyro_ = true;
 		gyroAccuracy = status;
 		rawGyroX = data1;
 		rawGyroY = data2;
@@ -345,6 +364,7 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_MAGNETIC_FIELD)
 	{
+		hasNewMag_ = true;
 		magAccuracy = status;
 		rawMagX = data1;
 		rawMagY = data2;
@@ -407,21 +427,28 @@ uint16_t BNO080::parseInputReport(void)
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_RAW_ACCELEROMETER)
 	{
+		hasNewRawAccel_ = true;
 		memsRawAccelX = data1;
 		memsRawAccelY = data2;
 		memsRawAccelZ = data3;
+		memsAccelTimeStamp = memstimeStamp;
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_RAW_GYROSCOPE)
 	{
+		hasNewRawGyro_ = true;
 		memsRawGyroX = data1;
 		memsRawGyroY = data2;
 		memsRawGyroZ = data3;
+		memsRawGyroTemp = data4;
+		memsGyroTimeStamp = memstimeStamp;
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_RAW_MAGNETOMETER)
 	{
+		hasNewRawMag_ = true;
 		memsRawMagX = data1;
 		memsRawMagY = data2;
 		memsRawMagZ = data3;
+		memsMagTimeStamp = memstimeStamp;
 	}
 	else if (shtpData[5] == SHTP_REPORT_COMMAND_RESPONSE)
 	{
@@ -541,6 +568,16 @@ void BNO080::getQuat(float &i, float &j, float &k, float &real, float &radAccura
 	hasNewQuaternion = false;
 }
 
+bool BNO080::getNewQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
+{
+	if (hasNewQuaternion)
+	{
+		getQuat(i, j, k, real, radAccuracy, accuracy);
+		return true;
+	}
+	return false;
+}
+
 void BNO080::getGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy)
 {
 	i = qToFloat(rawGameQuatI, rotationVector_Q1);
@@ -551,6 +588,16 @@ void BNO080::getGameQuat(float &i, float &j, float &k, float &real, uint8_t &acc
 	hasNewGameQuaternion = false;
 }
 
+bool BNO080::getNewGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy)
+{
+	if (hasNewGameQuaternion)
+	{
+		getGameQuat(i, j, k, real, accuracy);
+		return true;
+	}
+	return false;
+}
+
 void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
 {
 	i = qToFloat(rawMagQuatI, rotationVector_Q1);
@@ -562,6 +609,16 @@ void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAcc
 	hasNewMagQuaternion = false;
 }
 
+bool BNO080::getNewMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
+{
+	if (hasNewMagQuaternion)
+	{
+		getMagQuat(i, j, k, real, radAccuracy, accuracy);
+		return true;
+	}
+	return false;
+}
+
 bool BNO080::hasNewQuat() {
 	return hasNewQuaternion;
 }
@@ -630,6 +687,16 @@ void BNO080::getAccel(float &x, float &y, float &z, uint8_t &accuracy)
 	hasNewAccel_ = false;
 }
 
+bool BNO080::getNewAccel(float &x, float &y, float &z, uint8_t &accuracy)
+{
+	if (hasNewAccel_)
+	{
+		getAccel(x, y, z, accuracy);
+		return true;
+	}
+	return false;
+}
+
 //Return the acceleration component
 float BNO080::getAccelX()
 {
@@ -667,6 +734,21 @@ void BNO080::getLinAccel(float &x, float &y, float &z, uint8_t &accuracy)
 	y = qToFloat(rawLinAccelY, linear_accelerometer_Q1);
 	z = qToFloat(rawLinAccelZ, linear_accelerometer_Q1);
 	accuracy = accelLinAccuracy;
+	hasNewLinAccel_ = false;
+}
+
+bool BNO080::getNewLinAccel(float &x, float &y, float &z, uint8_t &accuracy)
+{
+	if (hasNewLinAccel_)
+	{
+		getLinAccel(x, y, z, accuracy);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewLinAccel() {
+	return hasNewLinAccel_;
 }
 
 //Return the acceleration component
@@ -704,6 +786,7 @@ void BNO080::getGyro(float &x, float &y, float &z, uint8_t &accuracy)
 	y = qToFloat(rawGyroY, gyro_Q1);
 	z = qToFloat(rawGyroZ, gyro_Q1);
 	accuracy = gyroAccuracy;
+	hasNewGyro_ = false;
 }
 
 //Return the gyro component
@@ -733,6 +816,10 @@ uint8_t BNO080::getGyroAccuracy()
 	return (gyroAccuracy);
 }
 
+bool BNO080::hasNewGyro() {
+	return hasNewGyro_;
+}
+
 //Gets the full mag vector
 //x,y,z output floats
 void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy)
@@ -741,6 +828,7 @@ void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy)
 	y = qToFloat(rawMagY, magnetometer_Q1);
 	z = qToFloat(rawMagZ, magnetometer_Q1);
 	accuracy = magAccuracy;
+	hasNewMag_=false;
 }
 
 //Return the magnetometer component
@@ -770,6 +858,10 @@ uint8_t BNO080::getMagAccuracy()
 	return (magAccuracy);
 }
 
+bool BNO080::hasNewMag() {
+	return hasNewMag_;
+}
+
 //Gets the full high rate gyro vector
 //x,y,z output floats
 void BNO080::getFastGyro(float &x, float &y, float &z)
@@ -800,6 +892,10 @@ float BNO080::getFastGyroZ()
 	return (gyro);
 }
 
+bool BNO080::hasNewFastGyro() {
+	return hasNewFastGyro_;
+}
+
 //Return the tap detector
 uint8_t BNO080::getTapDetector()
 {
@@ -853,6 +949,29 @@ int16_t BNO080::getRawAccelZ()
 	return (memsRawAccelZ);
 }
 
+void BNO080::getRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	x = memsRawAccelX;
+	y = memsRawAccelY;
+	z = memsRawAccelZ;
+	timeStamp = memsAccelTimeStamp;
+	hasNewRawAccel_ = false;
+}
+
+bool BNO080::getNewRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	if (hasNewRawAccel_)
+	{
+		getRawAccel(x, y, z, timeStamp);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewRawAccel() {
+	return hasNewRawAccel_;
+}
+
 //Return raw mems value for the gyro
 int16_t BNO080::getRawGyroX()
 {
@@ -867,6 +986,42 @@ int16_t BNO080::getRawGyroZ()
 	return (memsRawGyroZ);
 }
 
+// From https://github.com/ceva-dsp/sh2/issues/15
+// Raw gyro temperature for BNO085 uses BMI055 gyro
+// memsRawGyroTemp is in 23°C + 0.5°C/LSB
+float BNO080::getGyroTemp()
+{
+	return (23.0 + (0.5f * memsRawGyroTemp));
+}
+
+void BNO080::resetNewRawGyro()
+{
+	hasNewRawGyro_ = false;
+}
+
+void BNO080::getRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	x = memsRawGyroX;
+	y = memsRawGyroY;
+	z = memsRawGyroZ;
+	timeStamp = memsGyroTimeStamp;
+	hasNewRawGyro_ = false;
+}
+
+bool BNO080::getNewRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	if (hasNewRawGyro_)
+	{
+		getRawGyro(x, y, z, timeStamp);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewRawGyro() {
+	return hasNewRawGyro_;
+}
+
 //Return raw mems value for the mag
 int16_t BNO080::getRawMagX()
 {
@@ -881,6 +1036,29 @@ int16_t BNO080::getRawMagZ()
 	return (memsRawMagZ);
 }
 
+void BNO080::getRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	x = memsRawMagX;
+	y = memsRawMagY;
+	z = memsRawMagZ;
+	timeStamp = memsMagTimeStamp;
+	hasNewRawMag_ = false;
+}
+
+bool BNO080::getNewRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp)
+{
+	if (hasNewRawMag_)
+	{
+		getRawMag(x, y, z, timeStamp);
+		return true;
+	}
+	return false;
+}
+
+bool BNO080::hasNewRawMag() {
+	return hasNewRawMag_;
+}
+
 //Given a record ID, read the Q1 value from the metaData record in the FRS (ya, it's complicated)
 //Q1 is used for all sensor data calculations
 int16_t BNO080::getQ1(uint16_t recordID)
@@ -1027,24 +1205,51 @@ bool BNO080::readFRSdata(uint16_t recordID, uint8_t startLocation, uint8_t words
 	}
 }
 
+// After power on or completed reset, the BNO will send two messages. One
+// reply and one unsolicited message.
+// 1) Reset message on SHTP channel 1
+// 2) Unsolicited initialization message on SHTP channel 2
+// See 5.2.1 on BNO08X datasheet.
+// Wait For both of these packets specifically up to a max time and exit
+// after both packets are read or max waiting time is reached.
+void BNO080::waitForCompletedReset(uint32_t timeout)
+{
+	uint32_t tInitialResetTimeMS = millis();
+	bool tResetCompleteReceived = false;
+	bool tUnsolicitedResponseReceived = false;
+	shtpHeader[2] = 0; // Make sure we aren't reading old data.
+	shtpData[0] = 0;
+
+	// Wait requested timeout for the two packets. OR Until we get both reset responses.
+	while (millis() - tInitialResetTimeMS < timeout &&
+	       (!tResetCompleteReceived || !tUnsolicitedResponseReceived)) {
+		#ifdef ESP8266
+			ESP.wdtFeed();
+		#endif
+		receivePacket();
+		if (shtpHeader[2] == 1 && shtpData[0] == 0x01) {
+			tResetCompleteReceived = true;
+		}
+		if (shtpHeader[2] == 2 && shtpData[0] == 0xF1) {
+			tUnsolicitedResponseReceived = true;
+		}
+	}
+}
+
 //Send command to reset IC
 //Read all advertisement packets from sensor
-//The sensor has been seen to reset twice if we attempt too much too quickly.
-//This seems to work reliably.
 void BNO080::softReset(void)
 {
+	// after power-on sensor is resetting by itself
+	// let's handle that POTENTIAL reset with shorter timeout
+	// in case sensor was not resetted - like after issuing RESET command
+	waitForCompletedReset(1000);
 	shtpData[0] = 1; //Reset
 
 	//Attempt to start communication with sensor
 	sendPacket(CHANNEL_EXECUTABLE, 1); //Transmit packet on channel 1, 1 byte
-
-	//Read all incoming data and flush it
-	delay(50);
-	while (receivePacket() == true)
-		; //delay(1);
-	delay(50);
-	while (receivePacket() == true)
-		; //delay(1);
+	// now that reset should occur for sure, so let's try with longer timeout
+	waitForCompletedReset(5000);
 }
 
 //Set the operating mode to "On"
@@ -1245,39 +1450,9 @@ void BNO080::enableActivityClassifier(uint16_t timeBetweenReports, uint32_t acti
 	setFeatureCommand(SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER, timeBetweenReports, activitiesToEnable);
 }
 
-//Sends the commands to begin calibration of the accelerometer
-void BNO080::calibrateAccelerometer()
-{
-	sendCalibrateCommand(CALIBRATE_ACCEL);
-}
-
-//Sends the commands to begin calibration of the gyro
-void BNO080::calibrateGyro()
-{
-	sendCalibrateCommand(CALIBRATE_GYRO);
-}
-
-//Sends the commands to begin calibration of the magnetometer
-void BNO080::calibrateMagnetometer()
-{
-	sendCalibrateCommand(CALIBRATE_MAG);
-}
-
-//Sends the commands to begin calibration of the planar accelerometer
-void BNO080::calibratePlanarAccelerometer()
-{
-	sendCalibrateCommand(CALIBRATE_PLANAR_ACCEL);
-}
-
-//See 2.2 of the Calibration Procedure document 1000-4044
-void BNO080::calibrateAll()
-{
-	sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG);
-}
-
 void BNO080::endCalibration()
 {
-	sendCalibrateCommand(CALIBRATE_STOP); //Disables all calibrations
+	sendCalibrateCommand(0); //Disables all calibrations
 }
 
 //See page 51 of reference manual - ME Calibration Response
@@ -1349,40 +1524,38 @@ void BNO080::sendCommand(uint8_t command)
 
 //This tells the BNO080 to begin calibrating
 //See page 50 of reference manual and the 1000-4044 calibration doc
+//The argument is a set of binary flags see SH2_CAL_ACCEL
 void BNO080::sendCalibrateCommand(uint8_t thingToCalibrate)
 {
-	/*shtpData[3] = 0; //P0 - Accel Cal Enable
-	shtpData[4] = 0; //P1 - Gyro Cal Enable
+	/*
+	shtpData[3] = 0; //P0 - Accel Cal Enable
+	shtpData[4] = 0; //P1 - Gyro In-Hand Cal Enable
 	shtpData[5] = 0; //P2 - Mag Cal Enable
 	shtpData[6] = 0; //P3 - Subcommand 0x00
 	shtpData[7] = 0; //P4 - Planar Accel Cal Enable
-	shtpData[8] = 0; //P5 - Reserved
+	shtpData[8] = 0; //P5 - Gyro On-Table Cal Enable
 	shtpData[9] = 0; //P6 - Reserved
 	shtpData[10] = 0; //P7 - Reserved
-	shtpData[11] = 0; //P8 - Reserved*/
+	shtpData[11] = 0; //P8 - Reserved
+	*/
 
 	for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array
 		shtpData[x] = 0;
 
-	if (thingToCalibrate == CALIBRATE_ACCEL)
+	if ((thingToCalibrate & SH2_CAL_ACCEL) > 1)
 		shtpData[3] = 1;
-	else if (thingToCalibrate == CALIBRATE_GYRO)
+	if ((thingToCalibrate & SH2_CAL_GYRO_IN_HAND) > 1)
 		shtpData[4] = 1;
-	else if (thingToCalibrate == CALIBRATE_MAG)
+	if ((thingToCalibrate & SH2_CAL_MAG) > 1)
 		shtpData[5] = 1;
-	else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL)
+	if ((thingToCalibrate & SH2_CAL_PLANAR) > 1)
 		shtpData[7] = 1;
-	else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG)
-	{
-		shtpData[3] = 1;
-		shtpData[4] = 1;
-		shtpData[5] = 1;
-	}
-	else if (thingToCalibrate == CALIBRATE_STOP)
-		; //Do nothing, bytes are set to zero
+	if ((thingToCalibrate & SH2_CAL_ON_TABLE) > 1)
+		shtpData[8] = 1;
 
 	//Make the internal calStatus variable non-zero (operation failed) so that user can test while we wait
 	calibrationStatus = 1;
+	_hasNewCalibrationStatus  = false;
 
 	//Using this shtpData packet, send a command
 	sendCommand(COMMAND_ME_CALIBRATE);
@@ -1406,7 +1579,7 @@ void BNO080::requestCalibrationStatus()
 		shtpData[x] = 0;
 
 	shtpData[6] = 0x01; //P3 - 0x01 - Subcommand: Get ME Calibration
-
+	_hasNewCalibrationStatus  = false;
 	//Using this shtpData packet, send a command
 	sendCommand(COMMAND_ME_CALIBRATE);
 }
@@ -1432,6 +1605,43 @@ void BNO080::saveCalibration()
 	sendCommand(COMMAND_DCD); //Save DCD command
 }
 
+void BNO080::saveCalibrationPeriodically(bool save)
+{
+	/*shtpData[3] = 0; //P0 - Enable/Disable Periodic DCD Save
+	shtpData[4] = 0; //P1 - Reserved
+	shtpData[5] = 0; //P2 - Reserved
+	shtpData[6] = 0; //P3 - Reserved
+	shtpData[7] = 0; //P4 - Reserved
+	shtpData[8] = 0; //P5 - Reserved
+	shtpData[9] = 0; //P6 - Reserved
+	shtpData[10] = 0; //P7 - Reserved
+	shtpData[11] = 0; //P8 - Reserved*/
+
+	for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array
+		shtpData[x] = 0;
+	shtpData[3] = save ? 1 : 0;
+
+	//Using this shtpData packet, send a command
+	sendCommand(COMMAND_DCD_PERIOD_SAVE); //Save DCD command
+}
+
+bool BNO080::hasNewCalibrationStatus()
+{
+	return _hasNewCalibrationStatus;
+}
+
+void BNO080::getCalibrationStatus(uint8_t &calibrationResponseStatus, uint8_t &accelCalEnabled, uint8_t &gyroCalEnabled, uint8_t &magCalEnabled, uint8_t &planarAccelCalEnabled, uint8_t &onTableCalEnabled)
+{
+	_hasNewCalibrationStatus = false;
+	calibrationResponseStatus = _calibrationResponseStatus;
+	accelCalEnabled = _accelCalEnabled;
+	gyroCalEnabled = _gyroCalEnabled;
+	magCalEnabled = _magCalEnabled;
+	planarAccelCalEnabled = _planarAccelCalEnabled;
+	onTableCalEnabled = _onTableCalEnabled;
+}
+
+
 //Wait a certain time for incoming I2C bytes before giving up
 //Returns false if failed
 boolean BNO080::waitForI2C()
@@ -1462,7 +1672,7 @@ boolean BNO080::waitForSPI()
 {
 	for (uint8_t counter = 0; counter < 125; counter++) //Don't got more than 255
 	{
-		if (digitalRead(_int) == LOW)
+		if (_int->digitalRead() == LOW)
 			return (true);
 		if (_printDebug == true)
 			_debugPort->println(F("SPI Wait"));
@@ -1480,7 +1690,7 @@ boolean BNO080::receivePacket(void)
 {
 	if (_i2cPort == NULL) //Do SPI
 	{
-		if (digitalRead(_int) == HIGH)
+		if (_int->digitalRead() == HIGH)
 			return (false); //Data is not available
 
 		//Old way: if (waitForSPI() == false) return (false); //Something went wrong
@@ -1488,7 +1698,7 @@ boolean BNO080::receivePacket(void)
 		//Get first four bytes to find out how much data we need to read
 
 		_spiPort->beginTransaction(SPISettings(_spiPortSpeed, MSBFIRST, SPI_MODE3));
-		digitalWrite(_cs, LOW);
+		_cs->digitalWrite(LOW);
 
 		//Get the first four bytes, aka the packet header
 		uint8_t packetLSB = _spiPort->transfer(0);
@@ -1523,7 +1733,7 @@ boolean BNO080::receivePacket(void)
 				shtpData[dataSpot] = incoming; //Store data into the shtpData array
 		}
 
-		digitalWrite(_cs, HIGH); //Release BNO080
+		_cs->digitalWrite(HIGH); //Release BNO080
 
 		_spiPort->endTransaction();
 		printPacket();
@@ -1629,7 +1839,7 @@ boolean BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
 		//BNO080 has max CLK of 3MHz, MSB first,
 		//The BNO080 uses CPOL = 1 and CPHA = 1. This is mode3
 		_spiPort->beginTransaction(SPISettings(_spiPortSpeed, MSBFIRST, SPI_MODE3));
-		digitalWrite(_cs, LOW);
+		_cs->digitalWrite(LOW);
 
 		//Send the 4 byte packet header
 		_spiPort->transfer(packetLength & 0xFF);			 //Packet length LSB
@@ -1643,7 +1853,7 @@ boolean BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
 			_spiPort->transfer(shtpData[i]);
 		}
 
-		digitalWrite(_cs, HIGH);
+		_cs->digitalWrite(HIGH);
 		_spiPort->endTransaction();
 	}
 	else //Do I2C

lib/bno080/BNO080.h

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

lib/bno080/PinInterface.h

@@ -1,6 +1,6 @@
 /*
     SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+    Copyright (c) 2024 Eiren Rain & SlimeVR contributors
 
     Permission is hereby granted, free of charge, to any person obtaining a copy
     of this software and associated documentation files (the "Software"), to deal
@@ -20,18 +20,18 @@
     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
     THE SOFTWARE.
 */
-#ifndef SLIMEVR_NETWORK_H_
-#define SLIMEVR_NETWORK_H_
+#pragma once
 
-#include "globals.h"
-#include "wifihandler.h"
-#include "udpclient.h"
-#include "packets.h"
-#include "wifiprovisioning.h"
+#include <cstdint>
+#include <string>
 
-namespace Network {
-    void update(Sensor * const sensor, Sensor * const sensor2);
-    void setUp();
-}
+class PinInterface
+{
+public:
+	virtual bool init() { return true; };
+	virtual int digitalRead() = 0;
+	virtual void pinMode(uint8_t mode) = 0;
+	virtual void digitalWrite(uint8_t val) = 0;
 
-#endif // SLIMEVR_NETWORK_H_
+	[[nodiscard]] virtual std::string toString() const = 0;
+};

lib/i2cdev/I2Cdev.cpp

@@ -278,7 +278,6 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
                 useWire->beginTransmission(devAddr);
                 useWire->write(regAddr);
                 useWire->endTransmission(false);
-                useWire->beginTransmission(devAddr);
                 useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
         
                 for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) {
@@ -414,7 +413,6 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
                 useWire->beginTransmission(devAddr);
                 useWire->write(regAddr);
                 useWire->endTransmission(false);
-                useWire->beginTransmission(devAddr);
                 useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
         
                 bool msb = true; // starts with MSB, then LSB

lib/i2cscan/i2cscan.cpp

@@ -1,92 +1,175 @@
 #include "i2cscan.h"
 
+#include <array>
+#include <cstdint>
+#include <string>
+
+#include "../../src/globals.h"
+#include "../../src/consts.h"
+
+namespace I2CSCAN {
+    enum class ScanState : uint8_t {
+        IDLE,
+        SCANNING,
+        DONE
+    };
+
+	namespace {
+		ScanState scanState = ScanState::IDLE;
+    	uint8_t currentSDA = 0;
+    	uint8_t currentSCL = 0;
+    	uint8_t currentAddress = 1;
+    	bool found = false;
+		uint8_t txFails = 0;
+    	std::vector<uint8_t> validPorts;
+
 #ifdef ESP8266
-uint8_t portArray[] = {16, 5, 4, 2, 14, 12, 13};
-String portMap[] = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"};
+		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)
-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"};
+		std::array<uint8_t, 16> portArray = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
+		std::array<std::string, 16> portMap = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
+		std::array<uint8_t, 1> portExclude = {LED_PIN};
 #endif
 
-namespace I2CSCAN
-{
+		bool selectNextPort() {
+			currentSCL++;
 
-    uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound) {
-        if(I2CSCAN::isI2CExist(addr1))
-            return addr1;
-        if(!I2CSCAN::isI2CExist(addr2)) {
-            if(scanIfNotFound) {
-                Serial.println("[ERR] I2C: Can't find I2C device on provided addresses, scanning for all I2C devices and returning");
-                I2CSCAN::scani2cports();
-            } else {
-                Serial.println("[ERR] I2C: Can't find I2C device on provided addresses");
-            }
-            return 0;
+			if(validPorts[currentSCL] == validPorts[currentSDA]) currentSCL++;
+
+			if (currentSCL < validPorts.size()) {
+				Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]); //NOLINT
+				return true;
+			}
+
+			currentSCL = 0;
+			currentSDA++;
+
+			if (currentSDA >= validPorts.size()) {
+				if (!found) {
+					Serial.println("[ERROR] I2C: No I2C devices found"); //NOLINT
+				}
+	#ifdef ESP32
+				Wire.end();
+	#endif
+				Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+				scanState = ScanState::DONE;
+				return false;
+			}
+
+			Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+			return true;
+		}
+		template <uint8_t size1, uint8_t size2>
+		uint8_t countCommonElements(
+			const std::array<uint8_t, size1>& array1,
+			const std::array<uint8_t, size2>& array2) {
+
+			uint8_t count = 0;
+			for (const auto& elem1 : array1) {
+				for (const auto& elem2 : array2) {
+					if (elem1 == elem2) {
+						count++;
+					}
+				}
+			}
+
+			return count;
+		}
+	}  // anonymous namespace
+
+    void scani2cports() {
+        if (scanState != ScanState::IDLE) {
+			if (scanState == ScanState::DONE) {
+				Serial.println("[DEBUG] I2C scan finished previously, resetting and scanning again..."); //NOLINT
+			} else {
+				return; // Already scanning, do not start again
+			}
         }
-        return addr2;
+
+        // Filter out excluded ports
+		validPorts.clear();
+		uint8_t excludes = countCommonElements<portArray.size(), portExclude.size()>(portArray, portExclude);
+		validPorts.reserve(portArray.size() - excludes); // Reserve space to avoid reallocations
+
+		for (const auto& port : portArray) {
+			if (std::find(portExclude.begin(), portExclude.end(), port) == portExclude.end()) {
+				validPorts.push_back(port); // Port is valid, add it to the list
+			}
+		}
+
+		// Reset scan variables and start scanning
+        found = false;
+        currentSDA = 0;
+        currentSCL = 1;
+        currentAddress = 1;
+		txFails = 0;
+        scanState = ScanState::SCANNING;
+	}
+
+    void update() {
+        if (scanState != ScanState::SCANNING) {
+            return;
+        }
+
+#ifdef ESP32
+		if (currentAddress == 1) {
+            Wire.end();
+		}
+#endif
+
+        Wire.beginTransmission(currentAddress);
+        const uint8_t error = Wire.endTransmission();
+
+        if (error == 0) {
+            Serial.printf("[INFO ] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x!\n",
+                            portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
+            found = true;
+        } else if (error == 4) { // Unable to start transaction, log and warn
+            Serial.printf("[WARN ] I2C (@ %s(%d) : %s(%d)): Unable to start transaction at address 0x%02x!\n",
+                            portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
+            txFails++;
+        }
+
+        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;
+        }
+
+        currentAddress = 1;
+        selectNextPort();
     }
 
-    void scani2cports()
-    {
-        bool found = false;
-        for (uint8_t i = 0; i < sizeof(portArray); i++)
-        {
-            for (uint8_t j = 0; j < sizeof(portArray); j++)
-            {
-                if (i != j)
-                {
-                    if(checkI2C(i, j))
-                        found = true;
-                }
-            }
+    bool hasDevOnBus(uint8_t addr) {
+        byte error;
+#if ESP32C3
+        int retries = 2;
+        do {
+#endif
+            Wire.beginTransmission(addr);
+            error = Wire.endTransmission(); // The return value of endTransmission is used to determine if a device is present
+#if ESP32C3
         }
-        if(!found) {
-            Serial.println("[ERR] I2C: No I2C devices found");
-        }
-    }
-
-    bool checkI2C(uint8_t i, uint8_t j)
-    {
-        bool found = false;
-        Wire.begin(portArray[i], portArray[j]);
-        byte error, address;
-        int nDevices;
-        nDevices = 0;
-        for (address = 1; address < 127; address++)
-        {
-            // The i2c_scanner uses the return value of
-            // the Write.endTransmisstion to see if
-            // a device did acknowledge to the address.
-            Wire.beginTransmission(address);
-            error = Wire.endTransmission();
-
-            if (error == 0)
-            {
-                Serial.print("[DBG] I2C (@ " + portMap[i] + " : " + portMap[j] + "): ");
-                Serial.print("I2C device found at address 0x");
-                if (address < 16)
-                    Serial.print("0");
-                Serial.print(address, HEX);
-                Serial.println("  !");
-
-                nDevices++;
-                found = true;
-            }
-            else if (error == 4)
-            {
-                Serial.print("[ERR] I2C (@ " + portMap[i] + " : " + portMap[j] + "): ");
-                Serial.print("Unknow error at address 0x");
-                if (address < 16)
-                    Serial.print("0");
-                Serial.println(address, HEX);
-            }
-        }
-        return found;
-    }
-
-    bool isI2CExist(uint8_t addr) {
-        Wire.beginTransmission(addr);
-        byte error = Wire.endTransmission();
+        while (error != 0 && retries--);
+#endif
         if(error == 0)
             return true;
         return false;
@@ -107,60 +190,55 @@ namespace I2CSCAN
      * NSW Australia, www.forward.com.au
      * This code may be freely used for both private and commerical use
      */
-    int clearBus(uint8_t SDA, uint8_t SCL) {
-        #if defined(TWCR) && defined(TWEN)
-        TWCR &= ~(_BV(TWEN)); //Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly
-        #endif
 
-        pinMode(SDA, INPUT_PULLUP); // Make SDA (data) and SCL (clock) pins Inputs with pullup.
+    int clearBus(uint8_t SDA, uint8_t SCL) {
+#if defined(TWCR) && defined(TWEN)
+        TWCR &= ~(_BV(TWEN)); // Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly
+#endif
+
+        pinMode(SDA, INPUT_PULLUP);
         pinMode(SCL, INPUT_PULLUP);
 
-        boolean SCL_LOW = (digitalRead(SCL) == LOW); // Check is SCL is Low.
-        if (SCL_LOW) { //If it is held low Arduno cannot become the I2C master. 
-            return 1; //I2C bus error. Could not clear SCL clock line held low
+        boolean SCL_LOW = (digitalRead(SCL) == LOW);
+        if (SCL_LOW) {
+            return 1; // I2C bus error. Could not clear SCL, clock line held low.
         }
 
-        boolean SDA_LOW = (digitalRead(SDA) == LOW);  // vi. Check SDA input.
+        boolean SDA_LOW = (digitalRead(SDA) == LOW);
         int clockCount = 20; // > 2x9 clock
 
-        while (SDA_LOW && (clockCount > 0)) { //  vii. If SDA is Low,
+        while (SDA_LOW && (clockCount > 0)) {
             clockCount--;
-        // Note: I2C bus is open collector so do NOT drive SCL or SDA high.
-            pinMode(SCL, INPUT); // release SCL pullup so that when made output it will be LOW
-            pinMode(SCL, OUTPUT); // then clock SCL Low
-            delayMicroseconds(10); //  for >5uS
-            pinMode(SCL, INPUT); // release SCL LOW
-            pinMode(SCL, INPUT_PULLUP); // turn on pullup resistors again
-            // do not force high as slave may be holding it low for clock stretching.
-            delayMicroseconds(10); //  for >5uS
-            // The >5uS is so that even the slowest I2C devices are handled.
-            SCL_LOW = (digitalRead(SCL) == LOW); // Check if SCL is Low.
-            int counter = 20;
-            while (SCL_LOW && (counter > 0)) {  //  loop waiting for SCL to become High only wait 2sec.
-            counter--;
-            delay(100);
+            pinMode(SCL, INPUT);
+            pinMode(SCL, OUTPUT);
+            delayMicroseconds(10);
+            pinMode(SCL, INPUT);
+            pinMode(SCL, INPUT_PULLUP);
+            delayMicroseconds(10);
             SCL_LOW = (digitalRead(SCL) == LOW);
+            int counter = 20;
+            while (SCL_LOW && (counter > 0)) {
+                counter--;
+                delay(100);
+                SCL_LOW = (digitalRead(SCL) == LOW);
             }
-            if (SCL_LOW) { // still low after 2 sec error
-            return 2; // I2C bus error. Could not clear. SCL clock line held low by slave clock stretch for >2sec
+            if (SCL_LOW) {
+                return 2;
             }
-            SDA_LOW = (digitalRead(SDA) == LOW); //   and check SDA input again and loop
+            SDA_LOW = (digitalRead(SDA) == LOW);
         }
-        if (SDA_LOW) { // still low
-            return 3; // I2C bus error. Could not clear. SDA data line held low
+        if (SDA_LOW) {
+            return 3;
         }
 
-        // else pull SDA line low for Start or Repeated Start
-        pinMode(SDA, INPUT); // remove pullup.
-        pinMode(SDA, OUTPUT);  // and then make it LOW i.e. send an I2C Start or Repeated start control.
-        // When there is only one I2C master a Start or Repeat Start has the same function as a Stop and clears the bus.
-        /// A Repeat Start is a Start occurring after a Start with no intervening Stop.
-        delayMicroseconds(10); // wait >5uS
-        pinMode(SDA, INPUT); // remove output low
-        pinMode(SDA, INPUT_PULLUP); // and make SDA high i.e. send I2C STOP control.
-        delayMicroseconds(10); // x. wait >5uS
-        pinMode(SDA, INPUT); // and reset pins as tri-state inputs which is the default state on reset
+        pinMode(SDA, INPUT);
+        pinMode(SDA, OUTPUT);
+        delayMicroseconds(10);
+        pinMode(SDA, INPUT);
+        pinMode(SDA, INPUT_PULLUP);
+        delayMicroseconds(10);
+        pinMode(SDA, INPUT);
         pinMode(SCL, INPUT);
-        return 0; // all ok
+        return 0;
     }
 }

lib/i2cscan/i2cscan.h

@@ -6,10 +6,12 @@
 
 namespace I2CSCAN {
     void scani2cports();
+    void update();
     bool checkI2C(uint8_t i, uint8_t j);
-    bool isI2CExist(uint8_t addr);
+    bool hasDevOnBus(uint8_t addr);
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound);
     int clearBus(uint8_t SDA, uint8_t SCL);
+    bool inArray(uint8_t value, const uint8_t *array, size_t arraySize);
 }
 
-#endif // _I2CSCAN_H_
+#endif // _I2CSCAN_H_

lib/magneto/dmpmag.h

@@ -1,27 +1,20 @@
-#include "quat.h"
-#include "vector3.h"
+#ifndef _DMPMAG_H_
+#define _DMPMAG_H_
 
-//Get rotation quaternion from gravity vector and geomagnetic vector by Direction Cosine Matrix
-//https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-attitudetran
-Quat getQuatDCM(float* acc, float* mag){
-    Vector3 Mv(mag[0], mag[1], mag[2]);
-    Vector3 Dv(acc[0], acc[1], acc[2]);
-    Dv.normalize();
-    Vector3 Rv = Dv.cross(Mv);
-    Rv.normalize();
-    Vector3 Fv = Rv.cross(Dv);
-    Fv.normalize();
-    float q04 = 2*sqrt(1+Fv.x+Rv.y+Dv.z);
-    return Quat(Rv.z-Dv.y,Dv.x-Fv.z,Fv.y-Rv.x,q04*q04/4).normalized();    
-}
-Quat getCorrection(float* acc,float* mag,Quat quat)
-{
-    Quat magQ = getQuatDCM(acc,mag);
-    //dmp.w=DCM.z
-    //dmp.x=DCM.y
-    //dmp.y=-DCM.x
-    //dmp.z=DCM.w
-    Quat trans(magQ.x, magQ.y, magQ.w, magQ.z);
-    Quat result = trans*quat.inverse();
-    return result;
-}
+#include "quat.h"
+
+template<typename T>
+class DMPMag {
+    static constexpr T magCorrRatio = 0.02;
+public:
+    void update(T oqwxyz[4], const T iqwxyz[4], const T Grav[3], const T Mxyz[3]);
+private:
+    Quat getQuatDCM(const T acc[3], const T mag[3]);
+    Quat getCorrection(const T acc[3], const T mag[3], Quat quat);
+
+    Quat correction{0, 0, 0, 0};
+};
+
+#include "dmpmag.hpp"
+
+#endif /* _DMPMAG_H_ */

lib/magneto/dmpmag.hpp

@@ -0,0 +1,52 @@
+#include "dmpmag.h"
+
+//Get rotation quaternion from gravity vector and geomagnetic vector by Direction Cosine Matrix
+//https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-attitudetran
+
+template<typename T>
+Quat DMPMag<T>::getQuatDCM(const T acc[3], const T mag[3])
+{
+    Vector3 Mv(mag[0], mag[1], mag[2]);
+    Vector3 Dv(acc[0], acc[1], acc[2]);
+    Dv.normalize();
+    Vector3 Rv = Dv.cross(Mv);
+    Rv.normalize();
+    Vector3 Fv = Rv.cross(Dv);
+    Fv.normalize();
+    float q04 = 2*sqrt(1+Fv.x+Rv.y+Dv.z);
+    return Quat(Rv.z-Dv.y, Dv.x-Fv.z, Fv.y-Rv.x, q04*q04/4).normalized();    
+}
+
+template<typename T>
+Quat DMPMag<T>::getCorrection(const T acc[3], const T mag[3], Quat quat)
+{
+    Quat magQ = getQuatDCM(acc,mag);
+    //dmp.w=DCM.z
+    //dmp.x=DCM.y
+    //dmp.y=-DCM.x
+    //dmp.z=DCM.w
+    Quat trans(magQ.x, magQ.y, magQ.w, magQ.z);
+    Quat result = trans*quat.inverse();
+    return result;
+}
+
+template<typename T>
+void DMPMag<T>::update(T oqwxyz[4], const T iqwxyz[4], const T Grav[3], const T Mxyz[3])
+{
+    // Map DMP axis to sensor axis
+    Quat quat(-iqwxyz[2], iqwxyz[1], iqwxyz[3], iqwxyz[0]);
+    if (correction.length_squared() == 0.0f) {
+        correction = getCorrection(Grav, Mxyz, quat);
+    } else {
+        Quat newCorr = getCorrection(Grav, Mxyz, quat);
+
+        if(!__isnanf(newCorr.w)) {
+            correction = correction.slerp(newCorr, magCorrRatio);
+        }
+    }
+    Quat fusedquat = correction * quat;
+    oqwxyz[0] = fusedquat.w; 
+    oqwxyz[1] = fusedquat.x;
+    oqwxyz[2] = fusedquat.y;
+    oqwxyz[3] = fusedquat.z;
+}

lib/magneto/madgwick.cpp

@@ -1,168 +0,0 @@
-#include "madgwick.h"
-
-volatile float beta = 0.1f;
-
-void madgwickQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float deltat)
-{
-    float recipNorm;
-	float s0, s1, s2, s3;
-	float qDot1, qDot2, qDot3, qDot4;
-	float _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 -= beta * s0;
-		qDot2 -= beta * s1;
-		qDot3 -= beta * s2;
-		qDot4 -= beta * 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;
-}
-void madgwickQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
-{
-    float recipNorm;
-	float s0, s1, s2, s3;
-	float qDot1, qDot2, qDot3, qDot4;
-	float hx, hy;
-	float _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)) {
-		madgwickQuaternionUpdate(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 -= beta * s0;
-		qDot2 -= beta * s1;
-		qDot3 -= beta * s2;
-		qDot4 -= beta * 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/madgwick.h

@@ -1,9 +0,0 @@
-#ifndef _MADGWICK_H_
-#define _MADGWICK_H_
-
-#include "helper_3dmath.h"
-
-void madgwickQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float deltat);
-void madgwickQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
-
-#endif /* _MADGWICK_H_ */

lib/magneto/magneto1.4.h

@@ -1,80 +1,52 @@
 #ifndef __MAGENTO_1_4__
 #define __MAGENTO_1_4__
 
+/* Copyright (C) 2013 www.sailboatinstruments.blogspot.com
+ *
+ * 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.
+ */
+
 // In https://github.com/jremington/MPU-9250-AHRS
 // magneto 1.4 magnetometer/accelerometer calibration code
 // from http://sailboatinstruments.blogspot.com/2011/08/improved-magnetometer-calibration.html
-// tested and works with Code::Blocks 10.05 through 20.03
 
-// Command line version slightly modified from original, sjames_remington@gmail.com
-// Now includes option to reject outliers in units of sigma, deviation of data vector length
-// from mean values of the sample. Suggest using 2 as the rejection criterion
+/// @brief Accelerometer/Magnetometer calibration assistant
+class MagnetoCalibration {
+    public:
+        /// @brief Updates calibration with the sample (x, y, z)
+        void sample(double x, double y, double z);
 
-// comma separated ASCII input data file expected, three columns x, y, z
+        /// @brief Outputs the calibration matrix corresponding to all previous samples
+        /// 
+        /// This does not consume/destroy the calibration struct, allowing for the possibility of
+        /// continuously updating the calibration. That said, there may be limitations on how many
+        /// samples are useful to collect, due to the limitations of finite-storage numbers.
+        ///
+        /// @param BAinv 
+        void current_calibration(float BAinv[4][3]);
 
-#include <stdio.h>
-#include <math.h>
-#include <malloc.h>
-#include <string.h>
-#include <float.h>
-
-// Forward declarations of mymathlib.com routines
-void Multiply_Self_Transpose(double*, double*, int, int);
-void Get_Submatrix(double*, int, int, double*, int, int, int);
-int Choleski_LU_Decomposition(double*, int);
-int Choleski_LU_Inverse(double*, int);
-void Multiply_Matrices(double*, double*, int, int, double*, int);
-void Identity_Matrix(double*, int);
-
-int Hessenberg_Form_Elementary(double*, double*, int);
-void Hessenberg_Elementary_Transform(double*, double*, int[], int);
-
-void Copy_Vector(double*, double*, int);
-
-int QR_Hessenberg_Matrix(double*, double*, double[], double[], int, int);
-void One_Real_Eigenvalue(double[], double[], double[], int, double);
-void Two_Eigenvalues(double*, double*, double[], double[], int, int, double);
-void Update_Row(double*, double, double, int, int);
-void Update_Column(double*, double, double, int, int);
-void Update_Transformation(double*, double, double, int, int);
-void Double_QR_Iteration(double*, double*, int, int, int, double*, int);
-void Product_and_Sum_of_Shifts(double*, int, int, double*, double*, double*, int);
-int Two_Consecutive_Small_Subdiagonal(double*, int, int, int, double, double);
-void Double_QR_Step(double*, int, int, int, double, double, double*, int);
-void BackSubstitution(double*, double[], double[], int);
-void BackSubstitute_Real_Vector(double*, double[], double[], int, double, int);
-void BackSubstitute_Complex_Vector(double*, double[], double[], int, double, int);
-void Calculate_Eigenvectors(double*, double*, double[], double[], int);
-void Complex_Division(double, double, double, double, double*, double*);
-
-void Transpose_Square_Matrix(double*, int);
-
-int Lower_Triangular_Solve(double* L, double B[], double x[], int n);
-int Lower_Triangular_Inverse(double* L, int n);
-int Upper_Triangular_Solve(double* U, double B[], double x[], int n);
-
-void Interchange_Rows(double* A, int row1, int row2, int ncols);
-void Interchange_Columns(double* A, int col1, int col2, int nrows, int ncols);
-void Identity_Matrix(double* A, int n);
-void Copy_Vector(double* d, double* s, int n);
-
-void Hessenberg_Elementary_Transform(double* H, double* S, int perm[], int n);
-
-void One_Real_Eigenvalue(double Hrow[], double eigen_real[], double eigen_imag[], int row, double shift);
-void Two_Eigenvalues(double* H, double* S, double eigen_real[], double eigen_imag[], int n, int k, double t);
-void Update_Row(double* Hrow, double cos, double sin, int n, int k);
-void Update_Column(double* H, double cos, double sin, int n, int k);
-void Update_Transformation(double* S, double cos, double sin, int n, int k);
-void Double_QR_Iteration(double* H, double* S, int row, int min_row, int n, double* shift, int iteration);
-void Product_and_Sum_of_Shifts(double* H, int n, int max_row, double* shift, double* trace, double* det, int iteration);
-int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, int max_row, int n, double trace, double det);
-void Double_QR_Step(double* H, int min_row, int max_row, int min_col, double trace, double det, double* S, int n);
-void Complex_Division(double x, double y, double u, double v, double* a, double* b);
-void BackSubstitution(double* H, double eigen_real[], double eigen_imag[], int n);
-void BackSubstitute_Real_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n);
-void BackSubstitute_Complex_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n);
-void Calculate_Eigenvectors(double* H, double* S, double eigen_real[], double eigen_imag[], int n);
-
-void CalculateCalibration(float *buf, int sampleCount, float BAinv[4][3]);
+    private:
+        // internal 10x10 matrix representing the 10x<sample count> matrix multiplied by its transpose,
+        // resulting in a 10x10 symmetric matrix
+        double ata[100] = {0.0};
+        double norm_sum = 0.0;
+        double sample_count = 0.0;
+};
 
 #endif // __MAGENTO_1_4__

lib/magneto/mahony.cpp

@@ -1,212 +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"
-
-// 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.
-#define Kp 10.0f
-#define Ki 0.0f
-
-static float ix = 0.0f, iy = 0.0f, iz = 0.0f;  //integral feedback terms
-
-// 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
-void mahonyQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
-{
-    // short name local variable for readability
-    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
-    float norm;
-    float hx, hy, hz;  //observed West vector W = AxM
-    float ux, uy, uz, wx, wy, wz; //calculated A (Up) and W in body frame
-    float ex, ey, ez;
-    float qa, qb, qc;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    float q1q1 = q1 * q1;
-    float q1q2 = q1 * q2;
-    float q1q3 = q1 * q3;
-    float q1q4 = q1 * q4;
-    float q2q2 = q2 * q2;
-    float q2q3 = q2 * q3;
-    float q2q4 = q2 * q4;
-    float q3q3 = q3 * q3;
-    float q3q4 = q3 * q4;
-    float q4q4 = q4 * q4;
-
-    // Compute feedback only if magnetometer measurement valid (avoids NaN in magnetometer normalisation)
-    float tmp = mx * mx + my * my + mz * mz;
-    if (tmp == 0.0f) {
-        mahonyQuaternionUpdate(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;
-}
-
-void mahonyQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float deltat)
-{
-    // short name local variable for readability
-    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
-    float norm;
-    float vx, vy, vz;
-    float ex, ey, ez;  //error terms
-    float qa, qb, qc;
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    float 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/magneto/mahony.h

@@ -1,32 +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"
-
-void mahonyQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float deltat);
-void mahonyQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
-
-#endif /* _MAHONY_H_ */

lib/magneto/mymathlib_matrix.h

@@ -0,0 +1,67 @@
+#pragma once
+#ifndef __MYMATHLIB_MATRIX_H__
+
+/* Note: This code is from http://www.mymathlib.com/matrices/ and appears to have no license attached.
+ * I doubt that anyone will pursue us for the use of this library with a copyright date of 2004
+ * that's been distributed along with magneto for awhile, and the intent of the site seems to be to
+ * provide code for people to use, but it's something to be aware of.
+ */
+namespace mymathlib::matrix {
+    void Multiply_Self_Transpose(double*, double*, int, int);
+    void Get_Submatrix(double*, int, int, double*, int, int, int);
+    int Choleski_LU_Decomposition(double*, int);
+    int Choleski_LU_Inverse(double*, int);
+    void Multiply_Matrices(double*, double*, int, int, double*, int);
+    void Identity_Matrix(double*, int);
+
+    int Hessenberg_Form_Elementary(double*, double*, int);
+    void Hessenberg_Elementary_Transform(double*, double*, int[], int);
+
+    void Copy_Vector(double*, double*, int);
+
+    int QR_Hessenberg_Matrix(double*, double*, double[], double[], int, int);
+    void One_Real_Eigenvalue(double[], double[], double[], int, double);
+    void Two_Eigenvalues(double*, double*, double[], double[], int, int, double);
+    void Update_Row(double*, double, double, int, int);
+    void Update_Column(double*, double, double, int, int);
+    void Update_Transformation(double*, double, double, int, int);
+    void Double_QR_Iteration(double*, double*, int, int, int, double*, int);
+    void Product_and_Sum_of_Shifts(double*, int, int, double*, double*, double*, int);
+    int Two_Consecutive_Small_Subdiagonal(double*, int, int, int, double, double);
+    void Double_QR_Step(double*, int, int, int, double, double, double*, int);
+    void BackSubstitution(double*, double[], double[], int);
+    void BackSubstitute_Real_Vector(double*, double[], double[], int, double, int);
+    void BackSubstitute_Complex_Vector(double*, double[], double[], int, double, int);
+    void Calculate_Eigenvectors(double*, double*, double[], double[], int);
+    void Complex_Division(double, double, double, double, double*, double*);
+
+    void Transpose_Square_Matrix(double*, int);
+
+    int Lower_Triangular_Solve(double* L, double B[], double x[], int n);
+    int Lower_Triangular_Inverse(double* L, int n);
+    int Upper_Triangular_Solve(double* U, double B[], double x[], int n);
+
+    void Interchange_Rows(double* A, int row1, int row2, int ncols);
+    void Interchange_Columns(double* A, int col1, int col2, int nrows, int ncols);
+    void Identity_Matrix(double* A, int n);
+    void Copy_Vector(double* d, double* s, int n);
+
+    void Hessenberg_Elementary_Transform(double* H, double* S, int perm[], int n);
+
+    void One_Real_Eigenvalue(double Hrow[], double eigen_real[], double eigen_imag[], int row, double shift);
+    void Two_Eigenvalues(double* H, double* S, double eigen_real[], double eigen_imag[], int n, int k, double t);
+    void Update_Row(double* Hrow, double cos, double sin, int n, int k);
+    void Update_Column(double* H, double cos, double sin, int n, int k);
+    void Update_Transformation(double* S, double cos, double sin, int n, int k);
+    void Double_QR_Iteration(double* H, double* S, int row, int min_row, int n, double* shift, int iteration);
+    void Product_and_Sum_of_Shifts(double* H, int n, int max_row, double* shift, double* trace, double* det, int iteration);
+    int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, int max_row, int n, double trace, double det);
+    void Double_QR_Step(double* H, int min_row, int max_row, int min_col, double trace, double det, double* S, int n);
+    void Complex_Division(double x, double y, double u, double v, double* a, double* b);
+    void BackSubstitution(double* H, double eigen_real[], double eigen_imag[], int n);
+    void BackSubstitute_Real_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n);
+    void BackSubstitute_Complex_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n);
+    void Calculate_Eigenvectors(double* H, double* S, double eigen_real[], double eigen_imag[], int n);
+}
+
+#endif

lib/magnetometers/hmc5883l.h

@@ -0,0 +1,33 @@
+#define HMC_DEVADDR 0x1E
+#define HMC_RA_CFGA 0x00
+#define HMC_RA_CFGB 0x01
+#define HMC_RA_MODE 0x02
+#define HMC_RA_DATA 0x03
+
+#define HMC_CFGA_DATA_RATE_0_75 0b000 << 2
+#define HMC_CFGA_DATA_RATE_1_5  0b001 << 2
+#define HMC_CFGA_DATA_RATE_3    0b010 << 2
+#define HMC_CFGA_DATA_RATE_7_5  0b011 << 2
+#define HMC_CFGA_DATA_RATE_15   0b100 << 2
+#define HMC_CFGA_DATA_RATE_30   0b101 << 2
+#define HMC_CFGA_DATA_RATE_75   0b110 << 2
+#define HMC_CFGA_AVG_SAMPLES_1  0b00 << 5
+#define HMC_CFGA_AVG_SAMPLES_2  0b01 << 5
+#define HMC_CFGA_AVG_SAMPLES_4  0b10 << 5
+#define HMC_CFGA_AVG_SAMPLES_8  0b11 << 5
+#define HMC_CFGA_BIAS_NORMAL    0b00
+#define HMC_CFGA_BIAS_POS       0b01
+#define HMC_CFGA_BIAS_NEG       0b10
+
+#define HMC_CFGB_GAIN_0_88 0
+#define HMC_CFGB_GAIN_1_30 1 << 5
+#define HMC_CFGB_GAIN_1_90 2 << 5
+#define HMC_CFGB_GAIN_2_50 3 << 5
+#define HMC_CFGB_GAIN_4_00 4 << 5
+#define HMC_CFGB_GAIN_4_70 5 << 5
+#define HMC_CFGB_GAIN_5_60 6 << 5
+#define HMC_CFGB_GAIN_8_10 7 << 5
+
+#define HMC_MODE_HIGHSPEED 1 << 7
+#define HMC_MODE_READ_CONTINUOUS 0b00
+#define HMC_MODE_READ_SINGLEMEAS 0b01

lib/magnetometers/qmc5883l.h

@@ -0,0 +1,17 @@
+#define QMC_DEVADDR    0x0D
+#define QMC_RA_DATA    0x00
+#define QMC_RA_CONTROL 0x09
+#define QMC_RA_RESET   0x0B
+
+#define QMC_CFG_MODE_STANDBY    0b00
+#define QMC_CFG_MODE_CONTINUOUS 0b01
+#define QMC_CFG_ODR_10HZ        0b00 << 2
+#define QMC_CFG_ODR_50HZ        0b01 << 2
+#define QMC_CFG_ODR_100HZ       0b10 << 2
+#define QMC_CFG_ODR_200HZ       0b11 << 2
+#define QMC_CFG_RNG_2G          0b00 << 4
+#define QMC_CFG_RNG_8G          0b01 << 4
+#define QMC_CFG_OSR_512         0b00 << 6
+#define QMC_CFG_OSR_256         0b01 << 6
+#define QMC_CFG_OSR_128         0b10 << 6
+#define QMC_CFG_OSR_64          0b11 << 6

lib/math/basis.h

@@ -109,7 +109,6 @@ public:
 	void set_euler_zyx(const Vector3& p_euler);
 
 	Quat get_quat() const;
-	void set_quat(const Quat& p_quat);
 
 	Vector3 get_euler() const { return get_euler_yxz(); }
 	void set_euler(const Vector3& p_euler) { set_euler_yxz(p_euler); }
@@ -240,7 +239,6 @@ public:
 
 	operator Quat() const { return get_quat(); }
 
-	Basis(const Quat& p_quat) { set_quat(p_quat); };
 	Basis(const Quat& p_quat, const Vector3& p_scale) { set_quat_scale(p_quat, p_scale); }
 
 	Basis(const Vector3& p_euler) { set_euler(p_euler); }

lib/math/quat.cpp

@@ -57,14 +57,6 @@ void Quat::set_euler_xyz(const Vector3& p_euler) {
 		-sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3);
 }
 
-// get_euler_xyz returns a vector containing the Euler angles in the format
-// (ax,ay,az), where ax is the angle of rotation around x axis,
-// and similar for other axes.
-// This implementation uses XYZ convention (Z is the first rotation).
-Vector3 Quat::get_euler_xyz() const {
-	Basis m(*this);
-	return m.get_euler_xyz();
-}
 
 // set_euler_yxz expects a vector containing the Euler angles in the format
 // (ax,ay,az), where ax is the angle of rotation around x axis,
@@ -92,18 +84,6 @@ void Quat::set_euler_yxz(const Vector3& p_euler) {
 		sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3);
 }
 
-// get_euler_yxz returns a vector containing the Euler angles in the format
-// (ax,ay,az), where ax is the angle of rotation around x axis,
-// and similar for other axes.
-// This implementation uses YXZ convention (Z is the first rotation).
-Vector3 Quat::get_euler_yxz() const {
-#ifdef MATH_CHECKS
-	ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized.");
-#endif
-	Basis m(*this);
-	return m.get_euler_yxz();
-}
-
 void Quat::operator*=(const Quat& q) {
 	set(w * q.x + x * q.w + y * q.z - z * q.y,
 		w * q.y + y * q.w + z * q.x - x * q.z,
@@ -249,3 +229,12 @@ void Quat::set_axis_angle(const Vector3& axis, const float& angle) {
 			cos_angle);
 	}
 }
+
+void Quat::sandwich(Vector3& v) {
+	float tempX, tempY;
+	tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x;
+	tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x * v.y;
+	v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x - y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z;
+	v.x = tempX;
+	v.y = tempY;
+}

lib/math/quat.h

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

lib/mpu6050/MPU6050.cpp

@@ -67,7 +67,12 @@ void MPU6050::initialize(uint8_t address) {
  */
 bool MPU6050::testConnection() {
     uint8_t deviceId = getDeviceID();
-    return deviceId == 0x68 || deviceId == 0x70 || deviceId == 0x71 || deviceId == 0x73; // Allow any MPUs
+    // 0x68 -> MPU-6050
+    // 0x70 -> MPU-6500
+    // 0x71 -> MPU-9250
+    // 0x73 -> MPU-9255
+    // 0x74 -> MPU-6515
+    return deviceId == 0x68 || deviceId == 0x70 || deviceId == 0x71 || deviceId == 0x73 || deviceId == 0x74; 
 }
 
 // AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)

lib/ota/ota.cpp

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

lib/vqf/basicvqf.cpp

@@ -0,0 +1,407 @@
+// SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
+//
+// SPDX-License-Identifier: MIT
+
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
+// Removed batch update functions
+
+#include "basicvqf.h"
+
+#include <algorithm>
+#include <limits>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <assert.h>
+
+#define EPS std::numeric_limits<vqf_real_t>::epsilon()
+#define NaN std::numeric_limits<vqf_real_t>::quiet_NaN()
+
+inline vqf_real_t square(vqf_real_t x) { return x*x; }
+
+BasicVQFParams::BasicVQFParams()
+    : tauAcc(3.0)
+    , tauMag(9.0)
+{
+
+}
+
+BasicVQF::BasicVQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs)
+{
+    coeffs.gyrTs = gyrTs;
+    coeffs.accTs = accTs > 0 ? accTs : gyrTs;
+    coeffs.magTs = magTs > 0 ? magTs : gyrTs;
+
+    setup();
+}
+
+BasicVQF::BasicVQF(const BasicVQFParams &params, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs)
+{
+    this->params = params;
+
+    coeffs.gyrTs = gyrTs;
+    coeffs.accTs = accTs > 0 ? accTs : gyrTs;
+    coeffs.magTs = magTs > 0 ? magTs : gyrTs;
+
+    setup();
+}
+
+void BasicVQF::updateGyr(const vqf_real_t gyr[3], double gyrTs)
+{
+    // gyroscope prediction step
+    vqf_real_t gyrNorm = norm(gyr, 3);
+    vqf_real_t angle = gyrNorm * gyrTs;
+    if (gyrNorm > EPS) {
+        vqf_real_t c = cos(angle/2);
+        vqf_real_t s = sin(angle/2)/gyrNorm;
+        vqf_real_t gyrStepQuat[4] = {c, s*gyr[0], s*gyr[1], s*gyr[2]};
+        quatMultiply(state.gyrQuat, gyrStepQuat, state.gyrQuat);
+        normalize(state.gyrQuat, 4);
+    }
+}
+
+void BasicVQF::updateAcc(const vqf_real_t acc[3])
+{
+    // ignore [0 0 0] samples
+    if (acc[0] == vqf_real_t(0.0) && acc[1] == vqf_real_t(0.0) && acc[2] == vqf_real_t(0.0)) {
+        return;
+    }
+
+    vqf_real_t accEarth[3];
+
+    // filter acc in inertial frame
+    quatRotate(state.gyrQuat, acc, accEarth);
+    filterVec(accEarth, 3, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.accLpState, state.lastAccLp);
+
+    // transform to 6D earth frame and normalize
+    quatRotate(state.accQuat, state.lastAccLp, accEarth);
+    normalize(accEarth, 3);
+
+    // inclination correction
+    vqf_real_t accCorrQuat[4];
+    vqf_real_t q_w = sqrt((accEarth[2]+1)/2);
+    if (q_w > 1e-6) {
+        accCorrQuat[0] = q_w;
+        accCorrQuat[1] = 0.5*accEarth[1]/q_w;
+        accCorrQuat[2] = -0.5*accEarth[0]/q_w;
+        accCorrQuat[3] = 0;
+    } else {
+        // to avoid numeric issues when acc is close to [0 0 -1], i.e. the correction step is close (<= 0.00011°) to 180°:
+        accCorrQuat[0] = 0;
+        accCorrQuat[1] = 1;
+        accCorrQuat[2] = 0;
+        accCorrQuat[3] = 0;
+    }
+    quatMultiply(accCorrQuat, state.accQuat, state.accQuat);
+    normalize(state.accQuat, 4);
+}
+
+void BasicVQF::updateMag(const vqf_real_t mag[3])
+{
+    // ignore [0 0 0] samples
+    if (mag[0] == vqf_real_t(0.0) && mag[1] == vqf_real_t(0.0) && mag[2] == vqf_real_t(0.0)) {
+        return;
+    }
+
+    vqf_real_t magEarth[3];
+
+    // bring magnetometer measurement into 6D earth frame
+    vqf_real_t accGyrQuat[4];
+    getQuat6D(accGyrQuat);
+    quatRotate(accGyrQuat, mag, magEarth);
+
+    // calculate disagreement angle based on current magnetometer measurement
+    vqf_real_t magDisAngle = atan2(magEarth[0], magEarth[1]) - state.delta;
+
+    // make sure the disagreement angle is in the range [-pi, pi]
+    if (magDisAngle > vqf_real_t(M_PI)) {
+        magDisAngle -= vqf_real_t(2*M_PI);
+    } else if (magDisAngle < vqf_real_t(-M_PI)) {
+        magDisAngle += vqf_real_t(2*M_PI);
+    }
+
+    vqf_real_t k = coeffs.kMag;
+
+    // ensure fast initial convergence
+    if (state.kMagInit != vqf_real_t(0.0)) {
+        // make sure that the gain k is at least 1/N, N=1,2,3,... in the first few samples
+        if (k < state.kMagInit) {
+            k = state.kMagInit;
+        }
+
+        // iterative expression to calculate 1/N
+        state.kMagInit = state.kMagInit/(state.kMagInit+1);
+
+        // disable if t > tauMag
+        if (state.kMagInit*params.tauMag < coeffs.magTs) {
+            state.kMagInit = 0.0;
+        }
+    }
+
+    // first-order filter step
+    state.delta += k*magDisAngle;
+
+    // make sure delta is in the range [-pi, pi]
+    if (state.delta > vqf_real_t(M_PI)) {
+        state.delta -= vqf_real_t(2*M_PI);
+    } else if (state.delta < vqf_real_t(-M_PI)) {
+        state.delta += vqf_real_t(2*M_PI);
+    }
+}
+
+void BasicVQF::getQuat3D(vqf_real_t out[4]) const
+{
+    std::copy(state.gyrQuat, state.gyrQuat+4, out);
+}
+
+void BasicVQF::getQuat6D(vqf_real_t out[4]) const
+{
+    quatMultiply(state.accQuat, state.gyrQuat, out);
+}
+
+void BasicVQF::getQuat9D(vqf_real_t out[4]) const
+{
+    quatMultiply(state.accQuat, state.gyrQuat, out);
+    quatApplyDelta(out, state.delta, out);
+}
+
+vqf_real_t BasicVQF::getDelta() const
+{
+    return state.delta;
+}
+
+void BasicVQF::setTauAcc(vqf_real_t tauAcc)
+{
+    if (params.tauAcc == tauAcc) {
+        return;
+    }
+    params.tauAcc = tauAcc;
+    double newB[3];
+    double newA[3];
+
+    filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA);
+    filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState);
+
+    std::copy(newB, newB+3, coeffs.accLpB);
+    std::copy(newA, newA+2, coeffs.accLpA);
+}
+
+void BasicVQF::setTauMag(vqf_real_t tauMag)
+{
+    params.tauMag = tauMag;
+    coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs);
+}
+
+const BasicVQFParams& BasicVQF::getParams() const
+{
+    return params;
+}
+
+const BasicVQFCoefficients& BasicVQF::getCoeffs() const
+{
+    return coeffs;
+}
+
+const BasicVQFState& BasicVQF::getState() const
+{
+    return state;
+}
+
+void BasicVQF::setState(const BasicVQFState& state)
+{
+    this->state = state;
+}
+
+void BasicVQF::resetState()
+{
+    quatSetToIdentity(state.gyrQuat);
+    quatSetToIdentity(state.accQuat);
+    state.delta = 0.0;
+
+    std::fill(state.lastAccLp, state.lastAccLp+3, 0);
+    std::fill(state.accLpState, state.accLpState + 3*2, NaN);
+
+    state.kMagInit = 1.0;
+}
+
+void BasicVQF::quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4])
+{
+    vqf_real_t w = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
+    vqf_real_t x = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
+    vqf_real_t y = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
+    vqf_real_t z = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
+    out[0] = w; out[1] = x; out[2] = y; out[3] = z;
+}
+
+void BasicVQF::quatConj(const vqf_real_t q[4], vqf_real_t out[4])
+{
+    vqf_real_t w = q[0];
+    vqf_real_t x = -q[1];
+    vqf_real_t y = -q[2];
+    vqf_real_t z = -q[3];
+    out[0] = w; out[1] = x; out[2] = y; out[3] = z;
+}
+
+
+void BasicVQF::quatSetToIdentity(vqf_real_t out[4])
+{
+    out[0] = 1;
+    out[1] = 0;
+    out[2] = 0;
+    out[3] = 0;
+}
+
+void BasicVQF::quatApplyDelta(vqf_real_t q[], vqf_real_t delta, vqf_real_t out[])
+{
+    // out = quatMultiply([cos(delta/2), 0, 0, sin(delta/2)], q)
+    vqf_real_t c = cos(delta/2);
+    vqf_real_t s = sin(delta/2);
+    vqf_real_t w = c * q[0] - s * q[3];
+    vqf_real_t x = c * q[1] - s * q[2];
+    vqf_real_t y = c * q[2] + s * q[1];
+    vqf_real_t z = c * q[3] + s * q[0];
+    out[0] = w; out[1] = x; out[2] = y; out[3] = z;
+}
+
+void BasicVQF::quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3])
+{
+    vqf_real_t x = (1 - 2*q[2]*q[2] - 2*q[3]*q[3])*v[0] + 2*v[1]*(q[2]*q[1] - q[0]*q[3]) + 2*v[2]*(q[0]*q[2] + q[3]*q[1]);
+    vqf_real_t y = 2*v[0]*(q[0]*q[3] + q[2]*q[1]) + v[1]*(1 - 2*q[1]*q[1] - 2*q[3]*q[3]) + 2*v[2]*(q[2]*q[3] - q[1]*q[0]);
+    vqf_real_t z = 2*v[0]*(q[3]*q[1] - q[0]*q[2]) + 2*v[1]*(q[0]*q[1] + q[3]*q[2]) + v[2]*(1 - 2*q[1]*q[1] - 2*q[2]*q[2]);
+    out[0] = x; out[1] = y; out[2] = z;
+}
+
+vqf_real_t BasicVQF::norm(const vqf_real_t vec[], size_t N)
+{
+    vqf_real_t s = 0;
+    for(size_t i = 0; i < N; i++) {
+        s += vec[i]*vec[i];
+    }
+    return sqrt(s);
+}
+
+void BasicVQF::normalize(vqf_real_t vec[], size_t N)
+{
+    vqf_real_t n = norm(vec, N);
+    if (n < EPS) {
+        return;
+    }
+    for(size_t i = 0; i < N; i++) {
+        vec[i] /= n;
+    }
+}
+
+void BasicVQF::clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max)
+{
+    for(size_t i = 0; i < N; i++) {
+        if (vec[i] < min) {
+            vec[i] = min;
+        } else if (vec[i] > max) {
+            vec[i] = max;
+        }
+    }
+}
+
+vqf_real_t BasicVQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts)
+{
+    assert(Ts > 0);
+    if (tau < vqf_real_t(0.0)) {
+        return 0; // k=0 for negative tau (disable update)
+    } else if (tau == vqf_real_t(0.0)) {
+        return 1; // k=1 for tau=0
+    } else {
+        return 1 - exp(-Ts/tau);  // fc = 1/(2*pi*tau)
+    }
+}
+
+void BasicVQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA[])
+{
+    assert(tau > 0);
+    assert(Ts > 0);
+    // second order Butterworth filter based on https://stackoverflow.com/a/52764064
+    double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response
+    double C = tan(M_PI*fc*double(Ts));
+    double D = C*C + sqrt(2)*C + 1;
+    double b0 = C*C/D;
+    outB[0] = b0;
+    outB[1] = 2*b0;
+    outB[2] = b0;
+    // a0 = 1.0
+    outA[0] = 2*(C*C-1)/D; // a1
+    outA[1] = (1-sqrt(2)*C+C*C)/D; // a2
+}
+
+void BasicVQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[])
+{
+    // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter
+    // update equation)
+    out[0] = x0*(1 - b[0]);
+    out[1] = x0*(b[2] - a[1]);
+}
+
+void BasicVQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[],
+                                              const double a_old[], const double b_new[],
+                                              const double a_new[], double state[])
+{
+    if (isnan(state[0])) {
+        return;
+    }
+    for (size_t i = 0; i < N; i++) {
+        state[0+2*i] = state[0+2*i] + (b_old[0] - b_new[0])*last_y[i];
+        state[1+2*i] = state[1+2*i] + (b_old[1] - b_new[1] - a_old[0] + a_new[0])*last_y[i];
+    }
+}
+
+vqf_real_t BasicVQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2])
+{
+    // difference equations based on scipy.signal.lfilter documentation
+    // assumes that a0 == 1.0
+    double y = b[0]*x + state[0];
+    state[0] = b[1]*x - a[0]*y + state[1];
+    state[1] = b[2]*x - a[1]*y;
+    return y;
+}
+
+void BasicVQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3],
+                         const double a[2], double state[], vqf_real_t out[])
+{
+    assert(N>=2);
+
+    // to avoid depending on a single sample, average the first samples (for duration tau)
+    // and then use this average to calculate the filter initial state
+    if (isnan(state[0])) { // initialization phase
+        if (isnan(state[1])) { // first sample
+            state[1] = 0; // state[1] is used to store the sample count
+            for(size_t i = 0; i < N; i++) {
+                state[2+i] = 0; // state[2+i] is used to store the sum
+            }
+        }
+        state[1]++;
+        for (size_t i = 0; i < N; i++) {
+            state[2+i] += x[i];
+            out[i] = state[2+i]/state[1];
+        }
+        if (state[1]*Ts >= tau) {
+            for(size_t i = 0; i < N; i++) {
+               filterInitialState(out[i], b, a, state+2*i);
+            }
+        }
+        return;
+    }
+
+    for (size_t i = 0; i < N; i++) {
+        out[i] = filterStep(x[i], b, a, state+2*i);
+    }
+}
+
+void BasicVQF::setup()
+{
+    assert(coeffs.gyrTs > 0);
+    assert(coeffs.accTs > 0);
+    assert(coeffs.magTs > 0);
+
+    filterCoeffs(params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA);
+
+    coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs);
+
+    resetState();
+}

lib/vqf/basicvqf.h

@@ -0,0 +1,502 @@
+// SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
+//
+// SPDX-License-Identifier: MIT
+
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
+// Removed batch update functions
+
+#ifndef BASICVQF_HPP
+#define BASICVQF_HPP
+
+#include <stddef.h>
+
+#define VQF_SINGLE_PRECISION
+#define M_PI 3.14159265358979323846
+#define M_SQRT2 1.41421356237309504880
+
+/**
+ * @brief Typedef for the floating-point data type used for most operations.
+ *
+ * By default, all floating-point calculations are performed using `double`. Set the `VQF_SINGLE_PRECISION` define to
+ * change this type to `float`. Note that the Butterworth filter implementation will always use double precision as
+ * using floats can cause numeric issues.
+ */
+#ifndef VQF_SINGLE_PRECISION
+typedef double vqf_real_t;
+#else
+typedef float vqf_real_t;
+#endif
+
+/**
+ * @brief Struct containing all tuning parameters used by the BasicVQF class.
+ *
+ * The parameters influence the behavior of the algorithm and are independent of the sampling rate of the IMU data. The
+ * constructor sets all parameters to the default values.
+ *
+ * The basic version of this algoirthm only has two parameters: The time constants #tauAcc and #tauMag can be tuned to
+ * change the trust on the accelerometer and magnetometer measurements, respectively.
+ */
+struct BasicVQFParams
+{
+    /**
+     * @brief Constructor that initializes the struct with the default parameters.
+     */
+    BasicVQFParams();
+
+    /**
+     * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering in seconds.
+     *
+     * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer measurements and while large values of
+     * \f$\tau_\mathrm{acc}\f$ imply trust on the gyroscope measurements.
+     *
+     * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the
+     * second-order Butterworth low-pass filter as follows: \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$.
+     *
+     * Default value: 3.0 s
+     */
+    vqf_real_t tauAcc;
+    /**
+     * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds.
+     *
+     * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer measurements and while large values of
+     * \f$\tau_\mathrm{mag}\f$ imply trust on the gyroscope measurements.
+     *
+     * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the
+     * first-order low-pass filter for the heading correction as follows:
+     * \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$.
+     *
+     * Default value: 9.0 s
+     */
+    vqf_real_t tauMag;
+};
+
+/**
+ * @brief Struct containing the filter state of the BasicVQF class.
+ *
+ * The relevant parts of the state can be accessed via functions of the BasicVVQF class, e.g. BasicVQF::getQuat6D()
+ * and BasicVQF::getQuat9D(). To reset the state to the initial values, use VQF::resetState().
+ *
+ * Direct access to the full state is typically not needed but can be useful in some cases, e.g. for debugging. For this
+ * purpose, the state can be accessed by BasicVQF::getState() and set by BasicVQF::setState().
+ */
+struct BasicVQFState {
+    /**
+     * @brief Angular velocity strapdown integration quaternion \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$.
+     */
+    vqf_real_t gyrQuat[4];
+    /**
+     * @brief Inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$.
+     */
+    vqf_real_t accQuat[4];
+    /**
+     * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$.
+     *
+     * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 &
+     * \sin\frac{\delta}{2}\end{bmatrix}^T\f$.
+     */
+    vqf_real_t delta;
+
+    /**
+     * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame.
+     */
+    vqf_real_t lastAccLp[3];
+    /**
+     * @brief Internal low-pass filter state for #lastAccLp.
+     */
+    double accLpState[3*2];
+
+    /**
+     * @brief Gain used for heading correction to ensure fast initial convergence.
+     *
+     * This value is used as the gain for heading correction in the beginning if it is larger than the normal filter
+     * gain. It is initialized to 1 and then updated to 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is
+     * set to zero.
+     */
+    vqf_real_t kMagInit;
+};
+
+/**
+ * @brief Struct containing coefficients used by the BasicVQF class.
+ *
+ * Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps.
+ * They are calculated in BasicVQF::setup().
+ */
+struct BasicVQFCoefficients
+{
+    /**
+     * @brief Sampling time of the gyroscope measurements (in seconds).
+     */
+    vqf_real_t gyrTs;
+    /**
+     * @brief Sampling time of the accelerometer measurements (in seconds).
+     */
+    vqf_real_t accTs;
+    /**
+     * @brief Sampling time of the magnetometer measurements (in seconds).
+     */
+    vqf_real_t magTs;
+
+    /**
+     * @brief Numerator coefficients of the acceleration low-pass filter.
+     *
+     * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$.
+     */
+    double accLpB[3];
+    /**
+     * @brief Denominator coefficients of the acceleration low-pass filter.
+     *
+     * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$.
+     */
+    double accLpA[2];
+
+    /**
+     * @brief Gain of the first-order filter used for heading correction.
+     */
+    vqf_real_t kMag;
+};
+
+/**
+ * @brief A Versatile Quaternion-based Filter for IMU Orientation Estimation.
+ *
+ * \rst
+ * This class implements the basic version of the orientation estimation filter described in the following publication:
+ *
+ *
+ *     D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic
+ *     Disturbance Rejection." Information Fusion 2023, 91, 187--204.
+ *     `doi:10.1016/j.inffus.2022.10.014 <https://doi.org/10.1016/j.inffus.2022.10.014>`_.
+ *     [Accepted manuscript available at `arXiv:2203.17024 <https://arxiv.org/abs/2203.17024>`_.]
+ *
+ * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) sensor fusion and can also be used
+ * without magnetometer data. Different sampling rates for gyroscopes, accelerometers and magnetometers are
+ * supported as well. While in most cases, the defaults will be reasonable, the algorithm can be influenced via two
+ * tuning parameters.
+ *
+ * To use this C++ implementation,
+ *
+ * 1. create a instance of the class and provide the sampling time and, optionally, parameters
+ * 2. for every sample, call one of the update functions to feed the algorithm with IMU data
+ * 3. access the estimation results with :meth:`getQuat6D() <VQF.getQuat6D>`, :meth:`getQuat9D() <VQF.getQuat9D>` and
+ *    the other getter methods.
+ *
+ * If the full data is available in (row-major) data buffers, you can use :meth:`updateBatch() <VQF.updateBatch>`.
+ *
+ * This class is the C++ implementation of the basic algorithm version. This version does not include rest detection,
+ * gyroscope bias estimation, and magnetic disturbance detection and rejection. It is equivalent to the full version
+ * when the parameters :cpp:member:`VQFParams::motionBiasEstEnabled`, :cpp:member:`VQFParams::restBiasEstEnabled` and
+ * :cpp:member:`VQFParams::magDistRejectionEnabled` are set to false.
+ * Depending on use case and programming language of choice, the following alternatives might be useful:
+ *
+ * +------------------------+--------------------------+---------------------------+---------------------------+
+ * |                        | Full Version             | Basic Version             | Offline Version           |
+ * |                        |                          |                           |                           |
+ * +========================+==========================+===========================+===========================+
+ * | **C++**                | :cpp:class:`VQF`         | **BasicVQF (this class)** | :cpp:func:`offlineVQF`    |
+ * +------------------------+--------------------------+---------------------------+---------------------------+
+ * | **Python/C++ (fast)**  | :py:class:`vqf.VQF`      | :py:class:`vqf.BasicVQF`  | :py:meth:`vqf.offlineVQF` |
+ * +------------------------+--------------------------+---------------------------+---------------------------+
+ * | **Pure Python (slow)** | :py:class:`vqf.PyVQF`    | --                        | --                        |
+ * +------------------------+--------------------------+---------------------------+---------------------------+
+ * | **Pure Matlab (slow)** | :mat:class:`VQF.m <VQF>` | --                        | --                        |
+ * +------------------------+--------------------------+---------------------------+---------------------------+
+ * \endrst
+ */
+
+class BasicVQF
+{
+public:
+    /**
+     * Initializes the object with default parameters.
+     *
+     * In the most common case (using the default parameters and all data being sampled with the same frequency,
+     * create the class like this:
+     * \rst
+     * .. code-block:: c++
+     *
+     *     BasicVQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz
+     * \endrst
+     *
+     * @param gyrTs sampling time of the gyroscope measurements in seconds
+     * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1)
+     * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1)
+     *
+     */
+    BasicVQF(vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0);
+    /**
+     * @brief Initializes the object with custom parameters.
+     *
+     * Example code to create an object with a different value for tauAcc:
+     * \rst
+     * .. code-block:: c++
+     *
+     *     BasicVQFParams params;
+     *     params.tauAcc = 1.0;
+     *     BasicVQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz
+     * \endrst
+     *
+     * @param params BasicVQFParams struct containing the desired parameters
+     * @param gyrTs sampling time of the gyroscope measurements in seconds
+     * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1)
+     * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1)
+     */
+    BasicVQF(const BasicVQFParams& params, vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0);
+
+    /**
+     * @brief Performs gyroscope update step.
+     *
+     * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have
+     * different sampling rates. Otherwise, simply use #update().
+     *
+     * @param gyr gyroscope measurement in rad/s
+     */
+    void updateGyr(const vqf_real_t gyr[3], double gyrTs);
+    /**
+     * @brief Performs accelerometer update step.
+     *
+     * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have
+     * different sampling rates. Otherwise, simply use #update().
+     *
+     * Should be called after #updateGyr and before #updateMag.
+     *
+     * @param acc accelerometer measurement in m/s²
+     */
+    void updateAcc(const vqf_real_t acc[3]);
+    /**
+     * @brief Performs magnetometer update step.
+     *
+     * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have
+     * different sampling rates. Otherwise, simply use #update().
+     *
+     * Should be called after #updateAcc.
+     *
+     * @param mag magnetometer measurement in arbitrary units
+     */
+    void updateMag(const vqf_real_t mag[3]);
+
+    /**
+     * @brief Returns the angular velocity strapdown integration quaternion
+     * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$.
+     * @param out output array for the quaternion
+     */
+    void getQuat3D(vqf_real_t out[4]) const;
+    /**
+     * @brief Returns the 6D (magnetometer-free) orientation quaternion
+     * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$.
+     * @param out output array for the quaternion
+     */
+    void getQuat6D(vqf_real_t out[4]) const;
+    /**
+     * @brief Returns the 9D (with magnetometers) orientation quaternion
+     * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$.
+     * @param out output array for the quaternion
+     */
+    void getQuat9D(vqf_real_t out[4]) const;
+    /**
+     * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$.
+     *
+     * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 &
+     * \sin\frac{\delta}{2}\end{bmatrix}^T\f$.
+     *
+     * @return delta angle in rad (VQFState::delta)
+     */
+    vqf_real_t getDelta() const;
+
+    /**
+     * @brief Sets the time constant for accelerometer low-pass filtering.
+     *
+     * For more details, see VQFParams.tauAcc.
+     *
+     * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds
+     */
+    void setTauAcc(vqf_real_t tauAcc);
+    /**
+     * @brief Sets the time constant for the magnetometer update.
+     *
+     * For more details, see VQFParams.tauMag.
+     *
+     * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds
+     */
+    void setTauMag(vqf_real_t tauMag);
+
+    /**
+     * @brief Returns the current parameters.
+     */
+    const BasicVQFParams& getParams() const;
+    /**
+     * @brief Returns the coefficients used by the algorithm.
+     */
+    const BasicVQFCoefficients& getCoeffs() const;
+    /**
+     * @brief Returns the current state.
+     */
+    const BasicVQFState& getState() const;
+    /**
+     * @brief Overwrites the current state.
+     *
+     * This method allows to set a completely arbitrary filter state and is intended for debugging purposes. In
+     * combination with #getState, individual elements of the state can be modified.
+     *
+     * @param state A BasicVQFState struct containing the new state
+     */
+    void setState(const BasicVQFState& state);
+    /**
+     * @brief Resets the state to the default values at initialization.
+     *
+     * Resetting the state is equivalent to creating a new instance of this class.
+     */
+    void resetState();
+
+    /**
+     * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\f$).
+     */
+    static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]);
+    /**
+     * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}^*\f$).
+     */
+    static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]);
+    /**
+     * @brief Sets the output quaternion to the identity quaternion (\f$\mathbf{q}_\mathrm{out} =
+     * \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$).
+     */
+    static void quatSetToIdentity(vqf_real_t out[4]);
+    /**
+     * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion.
+     *
+     * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 &
+     * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$
+     */
+    static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]);
+    /**
+     * @brief Rotates a vector with a given quaternion.
+     *
+     * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} =
+     * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes \mathbf{q}^*\f$
+     */
+    static void quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]);
+    /**
+     * @brief Calculates the Euclidean norm of a vector.
+     * @param vec pointer to an array of N elements
+     * @param N number of elements
+     */
+    static vqf_real_t norm(const vqf_real_t vec[], size_t N);
+    /**
+     * @brief Normalizes a vector in-place.
+     * @param vec pointer to an array of N elements that will be normalized
+     * @param N number of elements
+     */
+    static void normalize(vqf_real_t vec[], size_t N);
+    /**
+     * @brief Clips a vector in-place.
+     * @param vec pointer to an array of N elements that will be clipped
+     * @param N number of elements
+     * @param min smallest allowed value
+     * @param max largest allowed value
+     */
+    static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max);
+    /**
+     * @brief Calculates the gain for a first-order low-pass filter from the 1/e time constant.
+     *
+     * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$
+     *
+     * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = \frac{1}{2\pi\tau}\f$.
+     *
+     * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update (\f$k=0\f$) or 0 to obtain
+     *        unfiltered values (\f$k=1\f$)
+     * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds
+     * @return filter gain *k*
+     */
+    static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts);
+    /**
+     * @brief Calculates coefficients for a second-order Butterworth low-pass filter.
+     *
+     * The filter is parametrized via the time constant of the dampened, non-oscillating part of step response and the
+     * resulting cutoff frequency is \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau}\f$.
+     *
+     * @param tau time constant \f$\tau\f$ in seconds
+     * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds
+     * @param outB output array for numerator coefficients
+     * @param outA output array for denominator coefficients (without \f$a_0=1\f$)
+     */
+    static void filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]);
+    /**
+     * @brief Calculates the initial filter state for a given steady-state value.
+     * @param x0 steady state value
+     * @param b numerator coefficients
+     * @param a denominator coefficients (without \f$a_0=1\f$)
+     * @param out output array for filter state
+     */
+    static void filterInitialState(vqf_real_t x0, const double b[], const double a[], double out[2]);
+    /**
+     * @brief Adjusts the filter state when changing coefficients.
+     *
+     * This function assumes that the filter is currently in a steady state, i.e. the last input values and the last
+     * output values are all equal. Based on this, the filter state is adjusted to new filter coefficients so that the
+     * output does not jump.
+     *
+     * @param last_y last filter output values (array of size N)
+     * @param N number of values in vector-valued signal
+     * @param b_old previous numerator coefficients
+     * @param a_old previous denominator coefficients (without \f$a_0=1\f$)
+     * @param b_new new numerator coefficients
+     * @param a_new new denominator coefficients (without \f$a_0=1\f$)
+     * @param state filter state (array of size N*2, will be modified)
+     */
+    static void filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3],
+                                               const double a_old[2], const double b_new[3],
+                                               const double a_new[2], double state[]);
+    /**
+     * @brief Performs a filter step for a scalar value.
+     * @param x input value
+     * @param b numerator coefficients
+     * @param a denominator coefficients (without \f$a_0=1\f$)
+     * @param state filter state array (will be modified)
+     * @return filtered value
+     */
+    static vqf_real_t filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]);
+    /**
+     * @brief Performs filter step for vector-valued signal with averaging-based initialization.
+     *
+     * During the first \f$\tau\f$ seconds, the filter output is the mean of the previous samples. At \f$t=\tau\f$, the
+     * initial conditions for the low-pass filter are calculated based on the current mean value and from then on,
+     * regular filtering with the rational transfer function described by the coefficients b and a is performed.
+     *
+     * @param x input values (array of size N)
+     * @param N number of values in vector-valued signal
+     * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization)
+     * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization)
+     * @param b numerator coefficients
+     * @param a denominator coefficients (without \f$a_0=1\f$)
+     * @param state filter state (array of size N*2, will be modified)
+     * @param out output array for filtered values (size N)
+     */
+    static void filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3],
+                          const double a[2], double state[], vqf_real_t out[]);
+
+protected:
+    /**
+     * @brief Calculates coefficients based on parameters and sampling rates.
+     */
+    void setup();
+
+    /**
+     * @brief Contains the current parameters.
+     *
+     * See #getParams. To set parameters, pass them to the constructor. The parameters can be changed with
+     * #setTauAcc and #setTauMag.
+     */
+    BasicVQFParams params;
+    /**
+     * @brief Contains the current state.
+     *
+     * See #getState, #getState and #resetState.
+     */
+    BasicVQFState state;
+    /**
+     * @brief Contains the current coefficients (calculated in #setup).
+     *
+     * See #getCoeffs.
+     */
+    BasicVQFCoefficients coeffs;
+};
+
+#endif // BASICVQF_HPP

lib/vqf/vqf.cpp

@@ -0,0 +1,912 @@
+// SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
+//
+// SPDX-License-Identifier: MIT
+
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
+// Removed batch update functions
+
+#include "vqf.h"
+
+#include <algorithm>
+#include <limits>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <assert.h>
+
+#define EPS std::numeric_limits<vqf_real_t>::epsilon()
+#define NaN std::numeric_limits<vqf_real_t>::quiet_NaN()
+
+inline vqf_real_t square(vqf_real_t x) { return x*x; }
+
+VQF::VQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs)
+{
+    coeffs.gyrTs = gyrTs;
+    coeffs.accTs = accTs > 0 ? accTs : gyrTs;
+    coeffs.magTs = magTs > 0 ? magTs : gyrTs;
+
+    setup();
+}
+
+VQF::VQF(const VQFParams &params, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs)
+{
+    this->params = params;
+
+    coeffs.gyrTs = gyrTs;
+    coeffs.accTs = accTs > 0 ? accTs : gyrTs;
+    coeffs.magTs = magTs > 0 ? magTs : gyrTs;
+
+    setup();
+}
+
+void VQF::updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
+{
+    // rest detection
+    if (params.restBiasEstEnabled || params.magDistRejectionEnabled) {
+        filterVec(gyr, 3, params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA,
+                  state.restGyrLpState, state.restLastGyrLp);
+
+        state.restLastSquaredDeviations[0] = square(gyr[0] - state.restLastGyrLp[0])
+                + square(gyr[1] - state.restLastGyrLp[1]) + square(gyr[2] - state.restLastGyrLp[2]);
+
+        vqf_real_t biasClip = params.biasClip*vqf_real_t(M_PI/180.0);
+        if (state.restLastSquaredDeviations[0] >= square(params.restThGyr*vqf_real_t(M_PI/180.0))
+                || fabs(state.restLastGyrLp[0]) > biasClip || fabs(state.restLastGyrLp[1]) > biasClip
+                || fabs(state.restLastGyrLp[2]) > biasClip) {
+            state.restT = 0.0;
+            state.restDetected = false;
+        }
+    }
+
+    // remove estimated gyro bias
+    vqf_real_t gyrNoBias[3] = {gyr[0]-state.bias[0], gyr[1]-state.bias[1], gyr[2]-state.bias[2]};
+    // gyroscope prediction step
+    vqf_real_t gyrNorm = norm(gyrNoBias, 3);
+    vqf_real_t angle = gyrNorm * gyrTs;
+    if (gyrNorm > EPS) {
+        vqf_real_t c = cos(angle/2);
+        vqf_real_t s = sin(angle/2)/gyrNorm;
+        vqf_real_t gyrStepQuat[4] = {c, s*gyrNoBias[0], s*gyrNoBias[1], s*gyrNoBias[2]};
+        quatMultiply(state.gyrQuat, gyrStepQuat, state.gyrQuat);
+        normalize(state.gyrQuat, 4);
+    }
+}
+
+void VQF::updateAcc(const vqf_real_t acc[3])
+{
+    // ignore [0 0 0] samples
+    if (acc[0] == vqf_real_t(0.0) && acc[1] == vqf_real_t(0.0) && acc[2] == vqf_real_t(0.0)) {
+        return;
+    }
+
+    // rest detection
+    if (params.restBiasEstEnabled) {
+        filterVec(acc, 3, params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA,
+                  state.restAccLpState, state.restLastAccLp);
+
+        state.restLastSquaredDeviations[1] = square(acc[0] - state.restLastAccLp[0])
+                + square(acc[1] - state.restLastAccLp[1]) + square(acc[2] - state.restLastAccLp[2]);
+
+        if (state.restLastSquaredDeviations[1] >= square(params.restThAcc)) {
+            state.restT = 0.0;
+            state.restDetected = false;
+        } else {
+            state.restT += coeffs.accTs;
+            if (state.restT >= params.restMinT) {
+                state.restDetected = true;
+            }
+        }
+    }
+
+    vqf_real_t accEarth[3];
+
+    // filter acc in inertial frame
+    quatRotate(state.gyrQuat, acc, accEarth);
+    filterVec(accEarth, 3, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.accLpState, state.lastAccLp);
+
+    // transform to 6D earth frame and normalize
+    quatRotate(state.accQuat, state.lastAccLp, accEarth);
+    normalize(accEarth, 3);
+
+    // inclination correction
+    vqf_real_t accCorrQuat[4];
+    vqf_real_t q_w = sqrt((accEarth[2]+1)/2);
+    if (q_w > 1e-6) {
+        accCorrQuat[0] = q_w;
+        accCorrQuat[1] = 0.5*accEarth[1]/q_w;
+        accCorrQuat[2] = -0.5*accEarth[0]/q_w;
+        accCorrQuat[3] = 0;
+    } else {
+        // to avoid numeric issues when acc is close to [0 0 -1], i.e. the correction step is close (<= 0.00011°) to 180°:
+        accCorrQuat[0] = 0;
+        accCorrQuat[1] = 1;
+        accCorrQuat[2] = 0;
+        accCorrQuat[3] = 0;
+    }
+    quatMultiply(accCorrQuat, state.accQuat, state.accQuat);
+    normalize(state.accQuat, 4);
+
+    // calculate correction angular rate to facilitate debugging
+    state.lastAccCorrAngularRate = acos(accEarth[2])/coeffs.accTs;
+
+    // bias estimation
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    if (params.motionBiasEstEnabled || params.restBiasEstEnabled) {
+        vqf_real_t biasClip = params.biasClip*vqf_real_t(M_PI/180.0);
+
+        vqf_real_t accGyrQuat[4];
+        vqf_real_t R[9];
+        vqf_real_t biasLp[2];
+
+        // get rotation matrix corresponding to accGyrQuat
+        getQuat6D(accGyrQuat);
+        R[0] = 1 - 2*square(accGyrQuat[2]) - 2*square(accGyrQuat[3]); // r11
+        R[1] = 2*(accGyrQuat[2]*accGyrQuat[1] - accGyrQuat[0]*accGyrQuat[3]); // r12
+        R[2] = 2*(accGyrQuat[0]*accGyrQuat[2] + accGyrQuat[3]*accGyrQuat[1]); // r13
+        R[3] = 2*(accGyrQuat[0]*accGyrQuat[3] + accGyrQuat[2]*accGyrQuat[1]); // r21
+        R[4] = 1 - 2*square(accGyrQuat[1]) - 2*square(accGyrQuat[3]); // r22
+        R[5] = 2*(accGyrQuat[2]*accGyrQuat[3] - accGyrQuat[1]*accGyrQuat[0]); // r23
+        R[6] = 2*(accGyrQuat[3]*accGyrQuat[1] - accGyrQuat[0]*accGyrQuat[2]); // r31
+        R[7] = 2*(accGyrQuat[0]*accGyrQuat[1] + accGyrQuat[3]*accGyrQuat[2]); // r32
+        R[8] = 1 - 2*square(accGyrQuat[1]) - 2*square(accGyrQuat[2]); // r33
+
+        // calculate R*b_hat (only the x and y component, as z is not needed)
+        biasLp[0] = R[0]*state.bias[0] + R[1]*state.bias[1] + R[2]*state.bias[2];
+        biasLp[1] = R[3]*state.bias[0] + R[4]*state.bias[1] + R[5]*state.bias[2];
+
+        // low-pass filter R and R*b_hat
+        filterVec(R, 9, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.motionBiasEstRLpState, R);
+        filterVec(biasLp, 2, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.motionBiasEstBiasLpState,
+                  biasLp);
+
+        // set measurement error and covariance for the respective Kalman filter update
+        vqf_real_t w[3];
+        vqf_real_t e[3];
+        if (state.restDetected && params.restBiasEstEnabled) {
+            e[0] = state.restLastGyrLp[0] - state.bias[0];
+            e[1] = state.restLastGyrLp[1] - state.bias[1];
+            e[2] = state.restLastGyrLp[2] - state.bias[2];
+            matrix3SetToScaledIdentity(1.0, R);
+            std::fill(w, w+3, coeffs.biasRestW);
+        } else if (params.motionBiasEstEnabled) {
+            e[0] = -accEarth[1]/coeffs.accTs + biasLp[0] - R[0]*state.bias[0] - R[1]*state.bias[1] - R[2]*state.bias[2];
+            e[1] = accEarth[0]/coeffs.accTs + biasLp[1] - R[3]*state.bias[0] - R[4]*state.bias[1] - R[5]*state.bias[2];
+            e[2] = - R[6]*state.bias[0] - R[7]*state.bias[1] - R[8]*state.bias[2];
+            w[0] = coeffs.biasMotionW;
+            w[1] = coeffs.biasMotionW;
+            w[2] = coeffs.biasVerticalW;
+        } else {
+            std::fill(w, w+3, -1); // disable update
+        }
+
+        // Kalman filter update
+        // step 1: P = P + V (also increase covariance if there is no measurement update!)
+        if (state.biasP[0] < coeffs.biasP0) {
+            state.biasP[0] += coeffs.biasV;
+        }
+        if (state.biasP[4] < coeffs.biasP0) {
+            state.biasP[4] += coeffs.biasV;
+        }
+        if (state.biasP[8] < coeffs.biasP0) {
+            state.biasP[8] += coeffs.biasV;
+        }
+        if (w[0] >= 0) {
+            // clip disagreement to -2..2 °/s
+            // (this also effectively limits the harm done by the first inclination correction step)
+            clip(e, 3, -biasClip, biasClip);
+
+            // step 2: K = P R^T inv(W + R P R^T)
+            vqf_real_t K[9];
+            matrix3MultiplyTpsSecond(state.biasP, R, K); // K = P R^T
+            matrix3Multiply(R, K, K); // K = R P R^T
+            K[0] += w[0];
+            K[4] += w[1];
+            K[8] += w[2]; // K = W + R P R^T
+            matrix3Inv(K, K); // K = inv(W + R P R^T)
+            matrix3MultiplyTpsFirst(R, K, K); // K = R^T inv(W + R P R^T)
+            matrix3Multiply(state.biasP, K, K); // K = P R^T inv(W + R P R^T)
+
+            // step 3: bias = bias + K (y - R bias) = bias + K e
+            state.bias[0] += K[0]*e[0] + K[1]*e[1] + K[2]*e[2];
+            state.bias[1] += K[3]*e[0] + K[4]*e[1] + K[5]*e[2];
+            state.bias[2] += K[6]*e[0] + K[7]*e[1] + K[8]*e[2];
+
+            // step 4: P = P - K R P
+            matrix3Multiply(K, R, K); // K = K R
+            matrix3Multiply(K, state.biasP, K); // K = K R P
+            for(size_t i = 0; i < 9; i++) {
+                state.biasP[i] -= K[i];
+            }
+
+            // clip bias estimate to -2..2 °/s
+            clip(state.bias, 3, -biasClip, biasClip);
+        }
+    }
+#else
+    // simplified implementation of bias estimation for the special case in which only rest bias estimation is enabled
+    if (params.restBiasEstEnabled) {
+        vqf_real_t biasClip = params.biasClip*vqf_real_t(M_PI/180.0);
+        if (state.biasP < coeffs.biasP0) {
+            state.biasP += coeffs.biasV;
+        }
+        if (state.restDetected) {
+            vqf_real_t e[3];
+            e[0] = state.restLastGyrLp[0] - state.bias[0];
+            e[1] = state.restLastGyrLp[1] - state.bias[1];
+            e[2] = state.restLastGyrLp[2] - state.bias[2];
+            clip(e, 3, -biasClip, biasClip);
+
+            // Kalman filter update, simplified scalar version for rest update
+            // (this version only uses the first entry of P as P is diagonal and all diagonal elements are the same)
+            // step 1: P = P + V (done above!)
+            // step 2: K = P R^T inv(W + R P R^T)
+            vqf_real_t k = state.biasP/(coeffs.biasRestW + state.biasP);
+            // step 3: bias = bias + K (y - R bias) = bias + K e
+            state.bias[0] += k*e[0];
+            state.bias[1] += k*e[1];
+            state.bias[2] += k*e[2];
+            // step 4: P = P - K R P
+            state.biasP -= k*state.biasP;
+            clip(state.bias, 3, -biasClip, biasClip);
+        }
+    }
+#endif
+}
+
+void VQF::updateMag(const vqf_real_t mag[3])
+{
+    // ignore [0 0 0] samples
+    if (mag[0] == vqf_real_t(0.0) && mag[1] == vqf_real_t(0.0) && mag[2] == vqf_real_t(0.0)) {
+        return;
+    }
+
+    vqf_real_t magEarth[3];
+
+    // bring magnetometer measurement into 6D earth frame
+    vqf_real_t accGyrQuat[4];
+    getQuat6D(accGyrQuat);
+    quatRotate(accGyrQuat, mag, magEarth);
+
+    if (params.magDistRejectionEnabled) {
+        state.magNormDip[0] = norm(magEarth, 3);
+        state.magNormDip[1] = -asin(magEarth[2]/state.magNormDip[0]);
+
+        if (params.magCurrentTau > 0) {
+            filterVec(state.magNormDip, 2, params.magCurrentTau, coeffs.magTs, coeffs.magNormDipLpB,
+                      coeffs.magNormDipLpA, state.magNormDipLpState, state.magNormDip);
+        }
+
+        // magnetic disturbance detection
+        if (fabs(state.magNormDip[0] - state.magRefNorm) < params.magNormTh*state.magRefNorm
+                && fabs(state.magNormDip[1] - state.magRefDip) < params.magDipTh*vqf_real_t(M_PI/180.0)) {
+            state.magUndisturbedT += coeffs.magTs;
+            if (state.magUndisturbedT >= params.magMinUndisturbedTime) {
+                state.magDistDetected = false;
+                state.magRefNorm += coeffs.kMagRef*(state.magNormDip[0] - state.magRefNorm);
+                state.magRefDip += coeffs.kMagRef*(state.magNormDip[1] - state.magRefDip);
+            }
+        } else {
+            state.magUndisturbedT = 0.0;
+            state.magDistDetected = true;
+        }
+
+        // new magnetic field acceptance
+        if (fabs(state.magNormDip[0] - state.magCandidateNorm) < params.magNormTh*state.magCandidateNorm
+                && fabs(state.magNormDip[1] - state.magCandidateDip) < params.magDipTh*vqf_real_t(M_PI/180.0)) {
+            if (norm(state.restLastGyrLp, 3) >= params.magNewMinGyr*M_PI/180.0) {
+                state.magCandidateT += coeffs.magTs;
+            }
+            state.magCandidateNorm += coeffs.kMagRef*(state.magNormDip[0] - state.magCandidateNorm);
+            state.magCandidateDip += coeffs.kMagRef*(state.magNormDip[1] - state.magCandidateDip);
+
+            if (state.magDistDetected && (state.magCandidateT >= params.magNewTime || (
+                    state.magRefNorm == 0.0 && state.magCandidateT >= params.magNewFirstTime))) {
+                state.magRefNorm = state.magCandidateNorm;
+                state.magRefDip = state.magCandidateDip;
+                state.magDistDetected = false;
+                state.magUndisturbedT = params.magMinUndisturbedTime;
+            }
+        } else {
+            state.magCandidateT = 0.0;
+            state.magCandidateNorm = state.magNormDip[0];
+            state.magCandidateDip = state.magNormDip[1];
+        }
+    }
+
+    // calculate disagreement angle based on current magnetometer measurement
+    state.lastMagDisAngle = atan2(magEarth[0], magEarth[1]) - state.delta;
+
+    // make sure the disagreement angle is in the range [-pi, pi]
+    if (state.lastMagDisAngle > vqf_real_t(M_PI)) {
+        state.lastMagDisAngle -= vqf_real_t(2*M_PI);
+    } else if (state.lastMagDisAngle < vqf_real_t(-M_PI)) {
+        state.lastMagDisAngle += vqf_real_t(2*M_PI);
+    }
+
+    vqf_real_t k = coeffs.kMag;
+
+    if (params.magDistRejectionEnabled) {
+        // magnetic disturbance rejection
+        if (state.magDistDetected) {
+            if (state.magRejectT <= params.magMaxRejectionTime) {
+                state.magRejectT += coeffs.magTs;
+                k = 0;
+            } else {
+                k /= params.magRejectionFactor;
+            }
+        } else {
+            state.magRejectT = std::max(state.magRejectT - params.magRejectionFactor*coeffs.magTs, vqf_real_t(0.0));
+        }
+    }
+
+    // ensure fast initial convergence
+    if (state.kMagInit != vqf_real_t(0.0)) {
+        // make sure that the gain k is at least 1/N, N=1,2,3,... in the first few samples
+        if (k < state.kMagInit) {
+            k = state.kMagInit;
+        }
+
+        // iterative expression to calculate 1/N
+        state.kMagInit = state.kMagInit/(state.kMagInit+1);
+
+        // disable if t > tauMag
+        if (state.kMagInit*params.tauMag < coeffs.magTs) {
+            state.kMagInit = 0.0;
+        }
+    }
+
+    // first-order filter step
+    state.delta += k*state.lastMagDisAngle;
+    // calculate correction angular rate to facilitate debugging
+    state.lastMagCorrAngularRate = k*state.lastMagDisAngle/coeffs.magTs;
+
+    // make sure delta is in the range [-pi, pi]
+    if (state.delta > vqf_real_t(M_PI)) {
+        state.delta -= vqf_real_t(2*M_PI);
+    } else if (state.delta < vqf_real_t(-M_PI)) {
+        state.delta += vqf_real_t(2*M_PI);
+    }
+}
+
+void VQF::getQuat3D(vqf_real_t out[4]) const
+{
+    std::copy(state.gyrQuat, state.gyrQuat+4, out);
+}
+
+void VQF::getQuat6D(vqf_real_t out[4]) const
+{
+    quatMultiply(state.accQuat, state.gyrQuat, out);
+}
+
+void VQF::getQuat9D(vqf_real_t out[4]) const
+{
+    quatMultiply(state.accQuat, state.gyrQuat, out);
+    quatApplyDelta(out, state.delta, out);
+}
+
+vqf_real_t VQF::getDelta() const
+{
+    return state.delta;
+}
+
+vqf_real_t VQF::getBiasEstimate(vqf_real_t out[3]) const
+{
+    if (out) {
+        std::copy(state.bias, state.bias+3, out);
+    }
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    // use largest absolute row sum as upper bound estimate for largest eigenvalue (Gershgorin circle theorem)
+    // and clip output to biasSigmaInit
+    vqf_real_t sum1 = fabs(state.biasP[0]) + fabs(state.biasP[1]) + fabs(state.biasP[2]);
+    vqf_real_t sum2 = fabs(state.biasP[3]) + fabs(state.biasP[4]) + fabs(state.biasP[5]);
+    vqf_real_t sum3 = fabs(state.biasP[6]) + fabs(state.biasP[7]) + fabs(state.biasP[8]);
+    vqf_real_t P = std::min(std::max(std::max(sum1, sum2), sum3), coeffs.biasP0);
+#else
+    vqf_real_t P = state.biasP;
+#endif
+    // convert standard deviation from 0.01deg to rad
+    return sqrt(P)*vqf_real_t(M_PI/100.0/180.0);
+}
+
+void VQF::setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma)
+{
+    std::copy(bias, bias+3, state.bias);
+    if (sigma > 0) {
+        vqf_real_t P = square(sigma*vqf_real_t(180.0*100.0/M_PI));
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+        matrix3SetToScaledIdentity(P, state.biasP);
+#else
+        state.biasP = P;
+#endif
+    }
+}
+
+bool VQF::getRestDetected() const
+{
+    return state.restDetected;
+}
+
+bool VQF::getMagDistDetected() const
+{
+    return state.magDistDetected;
+}
+
+void VQF::getRelativeRestDeviations(vqf_real_t out[2]) const
+{
+    out[0] = sqrt(state.restLastSquaredDeviations[0]) / (params.restThGyr*vqf_real_t(M_PI/180.0));
+    out[1] = sqrt(state.restLastSquaredDeviations[1]) / params.restThAcc;
+}
+
+vqf_real_t VQF::getMagRefNorm() const
+{
+    return state.magRefNorm;
+}
+
+vqf_real_t VQF::getMagRefDip() const
+{
+    return state.magRefDip;
+}
+
+void VQF::setMagRef(vqf_real_t norm, vqf_real_t dip)
+{
+    state.magRefNorm = norm;
+    state.magRefDip = dip;
+}
+
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+void VQF::setMotionBiasEstEnabled(bool enabled)
+{
+    if (params.motionBiasEstEnabled == enabled) {
+        return;
+    }
+    params.motionBiasEstEnabled = enabled;
+    std::fill(state.motionBiasEstRLpState, state.motionBiasEstRLpState + 9*2, NaN);
+    std::fill(state.motionBiasEstBiasLpState, state.motionBiasEstBiasLpState + 2*2, NaN);
+}
+#endif
+
+void VQF::setRestBiasEstEnabled(bool enabled)
+{
+    if (params.restBiasEstEnabled == enabled) {
+        return;
+    }
+    params.restBiasEstEnabled = enabled;
+    state.restDetected = false;
+    std::fill(state.restLastSquaredDeviations, state.restLastSquaredDeviations + 3, 0.0);
+    state.restT = 0.0;
+    std::fill(state.restLastGyrLp, state.restLastGyrLp + 3, 0.0);
+    std::fill(state.restGyrLpState, state.restGyrLpState + 3*2, NaN);
+    std::fill(state.restLastAccLp, state.restLastAccLp + 3, 0.0);
+    std::fill(state.restAccLpState, state.restAccLpState + 3*2, NaN);
+}
+
+void VQF::setMagDistRejectionEnabled(bool enabled)
+{
+    if (params.magDistRejectionEnabled == enabled) {
+        return;
+    }
+    params.magDistRejectionEnabled = enabled;
+    state.magDistDetected = true;
+    state.magRefNorm = 0.0;
+    state.magRefDip = 0.0;
+    state.magUndisturbedT = 0.0;
+    state.magRejectT = params.magMaxRejectionTime;
+    state.magCandidateNorm = -1.0;
+    state.magCandidateDip = 0.0;
+    state.magCandidateT = 0.0;
+    std::fill(state.magNormDipLpState, state.magNormDipLpState + 2*2, NaN);
+}
+
+void VQF::setTauAcc(vqf_real_t tauAcc)
+{
+    if (params.tauAcc == tauAcc) {
+        return;
+    }
+    params.tauAcc = tauAcc;
+    vqf_real_t newB[3];
+    vqf_real_t newA[3];
+
+    filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA);
+    filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState);
+
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    // For R and biasLP, the last value is not saved in the state.
+    // Since b0 is small (at reasonable settings), the last output is close to state[0].
+    vqf_real_t R[9];
+    for (size_t i = 0; i < 9; i++) {
+        R[i] = state.motionBiasEstRLpState[2*i];
+    }
+    filterAdaptStateForCoeffChange(R, 9, coeffs.accLpB, coeffs.accLpA, newB, newA, state.motionBiasEstRLpState);
+    vqf_real_t biasLp[2];
+    for (size_t i = 0; i < 2; i++) {
+        biasLp[i] = state.motionBiasEstBiasLpState[2*i];
+    }
+    filterAdaptStateForCoeffChange(biasLp, 2, coeffs.accLpB, coeffs.accLpA, newB, newA, state.motionBiasEstBiasLpState);
+#endif
+
+    std::copy(newB, newB+3, coeffs.accLpB);
+    std::copy(newA, newA+2, coeffs.accLpA);
+}
+
+void VQF::setTauMag(vqf_real_t tauMag)
+{
+    params.tauMag = tauMag;
+    coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs);
+}
+
+void VQF::setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc)
+{
+    params.restThGyr = thGyr;
+    params.restThAcc = thAcc;
+}
+
+const VQFParams& VQF::getParams() const
+{
+    return params;
+}
+
+const VQFCoefficients& VQF::getCoeffs() const
+{
+    return coeffs;
+}
+
+const VQFState& VQF::getState() const
+{
+    return state;
+}
+
+void VQF::setState(const VQFState& state)
+{
+    this->state = state;
+}
+
+void VQF::resetState()
+{
+    quatSetToIdentity(state.gyrQuat);
+    quatSetToIdentity(state.accQuat);
+    state.delta = 0.0;
+
+    state.restDetected = false;
+    state.magDistDetected = true;
+
+    std::fill(state.lastAccLp, state.lastAccLp+3, 0);
+    std::fill(state.accLpState, state.accLpState + 3*2, NaN);
+    state.lastAccCorrAngularRate = 0.0;
+
+    state.kMagInit = 1.0;
+    state.lastMagDisAngle = 0.0;
+    state.lastMagCorrAngularRate = 0.0;
+
+    std::fill(state.bias, state.bias+3, 0);
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    matrix3SetToScaledIdentity(coeffs.biasP0, state.biasP);
+#else
+    state.biasP = coeffs.biasP0;
+#endif
+
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    std::fill(state.motionBiasEstRLpState, state.motionBiasEstRLpState + 9*2, NaN);
+    std::fill(state.motionBiasEstBiasLpState, state.motionBiasEstBiasLpState + 2*2, NaN);
+#endif
+
+    std::fill(state.restLastSquaredDeviations, state.restLastSquaredDeviations + 3, 0.0);
+    state.restT = 0.0;
+    std::fill(state.restLastGyrLp, state.restLastGyrLp + 3, 0.0);
+    std::fill(state.restGyrLpState, state.restGyrLpState + 3*2, NaN);
+    std::fill(state.restLastAccLp, state.restLastAccLp + 3, 0.0);
+    std::fill(state.restAccLpState, state.restAccLpState + 3*2, NaN);
+
+    state.magRefNorm = 0.0;
+    state.magRefDip = 0.0;
+    state.magUndisturbedT = 0.0;
+    state.magRejectT = params.magMaxRejectionTime;
+    state.magCandidateNorm = -1.0;
+    state.magCandidateDip = 0.0;
+    state.magCandidateT = 0.0;
+    std::fill(state.magNormDip, state.magNormDip + 2, 0);
+    std::fill(state.magNormDipLpState, state.magNormDipLpState + 2*2, NaN);
+}
+
+void VQF::quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4])
+{
+    vqf_real_t w = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
+    vqf_real_t x = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
+    vqf_real_t y = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
+    vqf_real_t z = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
+    out[0] = w; out[1] = x; out[2] = y; out[3] = z;
+}
+
+void VQF::quatConj(const vqf_real_t q[4], vqf_real_t out[4])
+{
+    vqf_real_t w = q[0];
+    vqf_real_t x = -q[1];
+    vqf_real_t y = -q[2];
+    vqf_real_t z = -q[3];
+    out[0] = w; out[1] = x; out[2] = y; out[3] = z;
+}
+
+
+void VQF::quatSetToIdentity(vqf_real_t out[4])
+{
+    out[0] = 1;
+    out[1] = 0;
+    out[2] = 0;
+    out[3] = 0;
+}
+
+void VQF::quatApplyDelta(vqf_real_t q[], vqf_real_t delta, vqf_real_t out[])
+{
+    // out = quatMultiply([cos(delta/2), 0, 0, sin(delta/2)], q)
+    vqf_real_t c = cos(delta/2);
+    vqf_real_t s = sin(delta/2);
+    vqf_real_t w = c * q[0] - s * q[3];
+    vqf_real_t x = c * q[1] - s * q[2];
+    vqf_real_t y = c * q[2] + s * q[1];
+    vqf_real_t z = c * q[3] + s * q[0];
+    out[0] = w; out[1] = x; out[2] = y; out[3] = z;
+}
+
+void VQF::quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3])
+{
+    vqf_real_t x = (1 - 2*q[2]*q[2] - 2*q[3]*q[3])*v[0] + 2*v[1]*(q[2]*q[1] - q[0]*q[3]) + 2*v[2]*(q[0]*q[2] + q[3]*q[1]);
+    vqf_real_t y = 2*v[0]*(q[0]*q[3] + q[2]*q[1]) + v[1]*(1 - 2*q[1]*q[1] - 2*q[3]*q[3]) + 2*v[2]*(q[2]*q[3] - q[1]*q[0]);
+    vqf_real_t z = 2*v[0]*(q[3]*q[1] - q[0]*q[2]) + 2*v[1]*(q[0]*q[1] + q[3]*q[2]) + v[2]*(1 - 2*q[1]*q[1] - 2*q[2]*q[2]);
+    out[0] = x; out[1] = y; out[2] = z;
+}
+
+vqf_real_t VQF::norm(const vqf_real_t vec[], size_t N)
+{
+    vqf_real_t s = 0;
+    for(size_t i = 0; i < N; i++) {
+        s += vec[i]*vec[i];
+    }
+    return sqrt(s);
+}
+
+void VQF::normalize(vqf_real_t vec[], size_t N)
+{
+    vqf_real_t n = norm(vec, N);
+    if (n < EPS) {
+        return;
+    }
+    for(size_t i = 0; i < N; i++) {
+        vec[i] /= n;
+    }
+}
+
+void VQF::clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max)
+{
+    for(size_t i = 0; i < N; i++) {
+        if (vec[i] < min) {
+            vec[i] = min;
+        } else if (vec[i] > max) {
+            vec[i] = max;
+        }
+    }
+}
+
+vqf_real_t VQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts)
+{
+    assert(Ts > 0);
+    if (tau < vqf_real_t(0.0)) {
+        return 0; // k=0 for negative tau (disable update)
+    } else if (tau == vqf_real_t(0.0)) {
+        return 1; // k=1 for tau=0
+    } else {
+        return 1 - exp(-Ts/tau);  // fc = 1/(2*pi*tau)
+    }
+}
+
+void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[], vqf_real_t outA[])
+{
+    assert(tau > 0);
+    assert(Ts > 0);
+    // second order Butterworth filter based on https://stackoverflow.com/a/52764064
+    vqf_real_t fc = (M_SQRT2 / (2.0*M_PI))/vqf_real_t(tau); // time constant of dampened, non-oscillating part of step response
+    vqf_real_t C = tan(M_PI*fc*vqf_real_t(Ts));
+    vqf_real_t D = C*C + sqrt(2)*C + 1;
+    vqf_real_t b0 = C*C/D;
+    outB[0] = b0;
+    outB[1] = 2*b0;
+    outB[2] = b0;
+    // a0 = 1.0
+    outA[0] = 2*(C*C-1)/D; // a1
+    outA[1] = (1-sqrt(2)*C+C*C)/D; // a2
+}
+
+void VQF::filterInitialState(vqf_real_t x0, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t out[])
+{
+    // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter
+    // update equation)
+    out[0] = x0*(1 - b[0]);
+    out[1] = x0*(b[2] - a[1]);
+}
+
+void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const vqf_real_t b_old[],
+                                         const vqf_real_t a_old[], const vqf_real_t b_new[],
+                                         const vqf_real_t a_new[], vqf_real_t state[])
+{
+    if (isnan(state[0])) {
+        return;
+    }
+    for (size_t i = 0; i < N; i++) {
+        state[0+2*i] = state[0+2*i] + (b_old[0] - b_new[0])*last_y[i];
+        state[1+2*i] = state[1+2*i] + (b_old[1] - b_new[1] - a_old[0] + a_new[0])*last_y[i];
+    }
+}
+
+vqf_real_t VQF::filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2])
+{
+    // difference equations based on scipy.signal.lfilter documentation
+    // assumes that a0 == 1.0
+    vqf_real_t y = b[0]*x + state[0];
+    state[0] = b[1]*x - a[0]*y + state[1];
+    state[1] = b[2]*x - a[1]*y;
+    return y;
+}
+
+void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const vqf_real_t b[3],
+                    const vqf_real_t a[2], vqf_real_t state[], vqf_real_t out[])
+{
+    assert(N>=2);
+
+    // to avoid depending on a single sample, average the first samples (for duration tau)
+    // and then use this average to calculate the filter initial state
+    if (isnan(state[0])) { // initialization phase
+        if (isnan(state[1])) { // first sample
+            state[1] = 0; // state[1] is used to store the sample count
+            for(size_t i = 0; i < N; i++) {
+                state[2+i] = 0; // state[2+i] is used to store the sum
+            }
+        }
+        state[1]++;
+        for (size_t i = 0; i < N; i++) {
+            state[2+i] += x[i];
+            out[i] = state[2+i]/state[1];
+        }
+        if (state[1]*Ts >= tau) {
+            for(size_t i = 0; i < N; i++) {
+               filterInitialState(out[i], b, a, state+2*i);
+            }
+        }
+        return;
+    }
+
+    for (size_t i = 0; i < N; i++) {
+        out[i] = filterStep(x[i], b, a, state+2*i);
+    }
+}
+
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+void VQF::matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9])
+{
+    out[0] = scale;
+    out[1] = 0.0;
+    out[2] = 0.0;
+    out[3] = 0.0;
+    out[4] = scale;
+    out[5] = 0.0;
+    out[6] = 0.0;
+    out[7] = 0.0;
+    out[8] = scale;
+}
+
+void VQF::matrix3Multiply(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9])
+{
+    vqf_real_t tmp[9];
+    tmp[0] = in1[0]*in2[0] + in1[1]*in2[3] + in1[2]*in2[6];
+    tmp[1] = in1[0]*in2[1] + in1[1]*in2[4] + in1[2]*in2[7];
+    tmp[2] = in1[0]*in2[2] + in1[1]*in2[5] + in1[2]*in2[8];
+    tmp[3] = in1[3]*in2[0] + in1[4]*in2[3] + in1[5]*in2[6];
+    tmp[4] = in1[3]*in2[1] + in1[4]*in2[4] + in1[5]*in2[7];
+    tmp[5] = in1[3]*in2[2] + in1[4]*in2[5] + in1[5]*in2[8];
+    tmp[6] = in1[6]*in2[0] + in1[7]*in2[3] + in1[8]*in2[6];
+    tmp[7] = in1[6]*in2[1] + in1[7]*in2[4] + in1[8]*in2[7];
+    tmp[8] = in1[6]*in2[2] + in1[7]*in2[5] + in1[8]*in2[8];
+    std::copy(tmp, tmp+9, out);
+}
+
+void VQF::matrix3MultiplyTpsFirst(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9])
+{
+    vqf_real_t tmp[9];
+    tmp[0] = in1[0]*in2[0] + in1[3]*in2[3] + in1[6]*in2[6];
+    tmp[1] = in1[0]*in2[1] + in1[3]*in2[4] + in1[6]*in2[7];
+    tmp[2] = in1[0]*in2[2] + in1[3]*in2[5] + in1[6]*in2[8];
+    tmp[3] = in1[1]*in2[0] + in1[4]*in2[3] + in1[7]*in2[6];
+    tmp[4] = in1[1]*in2[1] + in1[4]*in2[4] + in1[7]*in2[7];
+    tmp[5] = in1[1]*in2[2] + in1[4]*in2[5] + in1[7]*in2[8];
+    tmp[6] = in1[2]*in2[0] + in1[5]*in2[3] + in1[8]*in2[6];
+    tmp[7] = in1[2]*in2[1] + in1[5]*in2[4] + in1[8]*in2[7];
+    tmp[8] = in1[2]*in2[2] + in1[5]*in2[5] + in1[8]*in2[8];
+    std::copy(tmp, tmp+9, out);
+}
+
+void VQF::matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9])
+{
+    vqf_real_t tmp[9];
+    tmp[0] = in1[0]*in2[0] + in1[1]*in2[1] + in1[2]*in2[2];
+    tmp[1] = in1[0]*in2[3] + in1[1]*in2[4] + in1[2]*in2[5];
+    tmp[2] = in1[0]*in2[6] + in1[1]*in2[7] + in1[2]*in2[8];
+    tmp[3] = in1[3]*in2[0] + in1[4]*in2[1] + in1[5]*in2[2];
+    tmp[4] = in1[3]*in2[3] + in1[4]*in2[4] + in1[5]*in2[5];
+    tmp[5] = in1[3]*in2[6] + in1[4]*in2[7] + in1[5]*in2[8];
+    tmp[6] = in1[6]*in2[0] + in1[7]*in2[1] + in1[8]*in2[2];
+    tmp[7] = in1[6]*in2[3] + in1[7]*in2[4] + in1[8]*in2[5];
+    tmp[8] = in1[6]*in2[6] + in1[7]*in2[7] + in1[8]*in2[8];
+    std::copy(tmp, tmp+9, out);
+}
+
+bool VQF::matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9])
+{
+    // in = [a b c; d e f; g h i]
+    vqf_real_t A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
+    vqf_real_t D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
+    vqf_real_t G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
+    vqf_real_t B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
+    vqf_real_t E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
+    vqf_real_t H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
+    vqf_real_t C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
+    vqf_real_t F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
+    vqf_real_t I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
+
+    vqf_real_t det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
+
+    if (det >= -EPS && det <= EPS) {
+        std::fill(out, out+9, 0);
+        return false;
+    }
+
+    // out = [A D G; B E H; C F I]/det
+    out[0] = A/det;
+    out[1] = D/det;
+    out[2] = G/det;
+    out[3] = B/det;
+    out[4] = E/det;
+    out[5] = H/det;
+    out[6] = C/det;
+    out[7] = F/det;
+    out[8] = I/det;
+
+    return true;
+}
+#endif
+
+void VQF::setup()
+{
+    assert(coeffs.gyrTs > 0);
+    assert(coeffs.accTs > 0);
+    assert(coeffs.magTs > 0);
+
+    filterCoeffs(params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA);
+
+    coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs);
+
+    coeffs.biasP0 = square(params.biasSigmaInit*100.0);
+    // the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds
+    updateBiasForgettingTime(params.biasForgettingTime);
+
+    filterCoeffs(params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA);
+    filterCoeffs(params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA);
+
+    coeffs.kMagRef = gainFromTau(params.magRefTau, coeffs.magTs);
+    if (params.magCurrentTau > 0) {
+        filterCoeffs(params.magCurrentTau, coeffs.magTs, coeffs.magNormDipLpB, coeffs.magNormDipLpA);
+    } else {
+        std::fill(coeffs.magNormDipLpB, coeffs.magNormDipLpB + 3, NaN);
+        std::fill(coeffs.magNormDipLpA, coeffs.magNormDipLpA + 2, NaN);
+    }
+
+    resetState();
+}
+
+void VQF::updateBiasForgettingTime(float biasForgettingTime) {
+    coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime;
+
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    vqf_real_t pMotion = square(params.biasSigmaMotion*100.0);
+    coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion;
+    coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10));
+#endif
+
+    vqf_real_t pRest = square(params.biasSigmaRest*100.0);
+    coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
+}

platformio.ini

@@ -11,22 +11,48 @@
 ; https://docs.slimevr.dev/firmware/configuring-project.html#1-configuring-platformioini
 ; ================================================
 
+[platformio]
+build_cache_dir = cache
+default_envs = BOARD_WEMOSD1MINI
+
 [env]
 lib_deps=
   https://github.com/SlimeVR/CmdParser.git
+  https://github.com/SlimeVR/base64_arduino.git
+  https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library.git
+  https://github.com/hideakitai/PCA9547.git
 monitor_speed = 115200
+monitor_echo = yes
+monitor_filters = colorize
+;monitor_rts = 0
+;monitor_dtr = 0
 framework = arduino
+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
 ; '" - quotes are necessary!
 ;  -DWIFI_CREDS_SSID='"SSID"'
 ;  -DWIFI_CREDS_PASSWD='"PASSWORD"'
 
+;If you want to set a static IP address, uncomment and edit the lines below
+;  -DWIFI_USE_STATICIP
+;  -DWIFI_STATIC_IP=192,168,XXX,XXX
+;  -DWIFI_STATIC_GATEWAY=192,168,XXX,XXX
+;  -DWIFI_STATIC_SUBNET=255,255,255,0
+
+;  -DSERVER_IP='"192.168.XXX.XXX"'
+;  -DSERVER_PORT=6969
+
+; Uncomment below if your board are using 40MHz crystal instead of 26MHz for ESP8266
+;  -DF_CRYSTAL=40000000
+
 ; Enable -O2 GCC optimization
   -O2
+  -std=gnu++2a
 
-build_unflags = -Os
+build_unflags = -Os -std=gnu++11 -std=gnu++17
 
 ; If you want to enable OTA Updates, uncomment and set OTA password here and in credentials.h
 ; You can set upload_port to device's ip after it's set up for the first time
@@ -36,27 +62,191 @@ build_unflags = -Os
 ;upload_flags =
 ;  --auth=SlimeVR-OTA
 
-; Settings for different boards
-
-[env:esp12e]
-platform = espressif8266
-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
+;platform = espressif8266 @ 4.2.1
 ;board = esp01_1m
+;board_build.arduino.ldscript = "eagle.flash.1m64.ld"
 
-; Uncomment below if you want to build for esp32
+[env:BOARD_WEMOSD1MINI]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_WEMOSD1MINI
+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
-; [env:esp32]
-; lib_deps =
-;   ${env.lib_deps}
-;   lorol/LittleFS_esp32 @ 1.0.6
-; platform = espressif32 @ 3.5.0
-; board = esp32dev
-; Comment out this line below if you have any trouble uploading the firmware - and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed
-; upload_speed = 921600
+; 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_BEETLE32C3]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+custom_slime_board = BOARD_BEETLE32C3
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = dfrobot_beetle_esp32c3
+monitor_filters = colorize, esp32_exception_decoder
+; If you want to use a ESP32C3, you can use this (experimental)
+; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards
+; There are 2 main Boardtypes:
+; 1. Boards that use a USB 2 Serial Chipset ( esp32-c3-devkitm-1, ttgo-t-oi-plus )
+; 2. Boards that relay on the USB interface of the ESP32C3 ( lolin_c3_mini , dfrobot_beetle_esp32c3)
+;    On this board there are 2 type some of them need to have set the build flag (menuconfig)
+;    -DARDUINO_USB_MODE=1
+;    -DARDUINO_USB_CDC_ON_BOOT=1
+
+[env:BOARD_ESP32C3DEVKITM1]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+custom_slime_board = BOARD_ESP32C3DEVKITM1
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = esp32-c3-devkitm-1
+monitor_filters = colorize, esp32_exception_decoder
+; If you want to use a ESP32C3, you can use this (experimental)
+; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards
+; There are 2 main Boardtypes:
+; 1. Boards that use a USB 2 Serial Chipset ( esp32-c3-devkitm-1, ttgo-t-oi-plus )
+; 2. Boards that relay on the USB interface of the ESP32C3 ( lolin_c3_mini , dfrobot_beetle_esp32c3)
+;    On this board there are 2 type some of them need to have set the build flag (menuconfig)
+;    -DARDUINO_USB_MODE=1
+;    -DARDUINO_USB_CDC_ON_BOOT=1
+
+[env:BOARD_ESP32C6DEVKITC1]
+platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
+custom_slime_board = BOARD_ESP32C6DEVKITC1
+build_flags =
+  ${env.build_flags}
+  -DESP32C6
+  -DARDUINO_USB_MODE=1
+  -DARDUINO_USB_CDC_ON_BOOT=1
+board = esp32-c6-devkitc-1
+
+[env:BOARD_XIAO_ESP32C3]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+custom_slime_board = BOARD_XIAO_ESP32C3
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = seeed_xiao_esp32c3
+monitor_filters = colorize, esp32_exception_decoder
+
+[env:BOARD_ESP32S3_SUPERMINI]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+custom_slime_board = BOARD_ESP32S3_SUPERMINI
+build_flags = 
+  ${env.build_flags}
+  -DARDUINO_USB_MODE=1
+  -DESP32S3
+board = esp32s3_supermini
+board_upload.use_1200bps_touch = 1
+board_upload.wait_for_upload_port = 1
+board_upload.require_upload_port = 1
+upload_speed = 921600
+
+[env:BOARD_SLIMEVR]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_SLIMEVR
+build_flags =
+  ${env.build_flags}
+  -D VENDOR_NAME='"SlimeVR"'
+  -D VENDOR_URL='"https://slimevr.dev"'
+  -D PRODUCT_NAME='"SlimeVR Tracker"'
+  -D UPDATE_ADDRESS='"SlimeVR/SlimeVR-Tracker-ESP"'
+  -D UPDATE_NAME='"BOARD_SLIMEVR-firmware"'
+
+[env:BOARD_SLIMEVR_V1_2]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_SLIMEVR_V1_2
+build_flags =
+  ${env.build_flags}
+  -D VENDOR_NAME='"SlimeVR"'
+  -D VENDOR_URL='"https://slimevr.dev"'
+  -D PRODUCT_NAME='"SlimeVR Tracker v1.2"'
+  -D UPDATE_ADDRESS='"SlimeVR/SlimeVR-Tracker-ESP"'
+  -D UPDATE_NAME='"BOARD_SLIMEVR_V1_2-firmware"'
+
+[env:BOARD_SLIMEVR_DEV]
+platform = espressif8266 @ 4.2.1
+board = esp12e
+custom_slime_board = BOARD_SLIMEVR_DEV
+build_flags =
+  ${env.build_flags}
+  -D VENDOR_NAME='"SlimeVR"'
+  -D PRODUCT_NAME='"SlimeVR Tracker (dev)"'
+
+[env:BOARD_GLOVE_IMU_SLIMEVR_DEV]
+platform = espressif32 @ 6.7.0
+platform_packages =
+  framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
+  framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip
+build_flags = 
+  ${env.build_flags}
+  -D BOARD=BOARD_GLOVE_IMU_SLIMEVR_DEV
+  -DESP32C3
+  -D PRODUCT_NAME='"SlimeVR Glove (dev)"'
+board = lolin_c3_mini
+monitor_filters = colorize, esp32_exception_decoder

scripts/get_git_commit.py

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

scripts/preprocessor.py

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

src/FSHelper.cpp

@@ -0,0 +1,90 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 TheDevMinerTV
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "./FSHelper.h"
+
+#include <functional>
+
+namespace SlimeVR::Utils {
+SlimeVR::Logging::Logger m_Logger("FSHelper");
+
+bool ensureDirectory(const char* directory) {
+	if (!LittleFS.exists(directory)) {
+		if (!LittleFS.mkdir(directory)) {
+			m_Logger.error("Failed to create directory: %s", directory);
+			return false;
+		}
+	}
+
+	auto dir = LittleFS.open(directory, "r");
+	auto isDirectory = dir.isDirectory();
+	dir.close();
+
+	if (!isDirectory) {
+		if (!LittleFS.remove(directory)) {
+			m_Logger.error("Failed to remove directory: %s", directory);
+			return false;
+		}
+
+		if (!LittleFS.mkdir(directory)) {
+			m_Logger.error("Failed to create directory: %s", directory);
+			return false;
+		}
+	}
+
+	return true;
+}
+
+File openFile(const char* path, const char* mode) {
+	return File(LittleFS.open(path, mode));
+}
+
+void forEachFile(const char* directory, std::function<void(File file)> callback) {
+	if (!ensureDirectory(directory)) {
+		return;
+	}
+
+#ifdef ESP32
+	auto dir = LittleFS.open(directory);
+	while (auto f = dir.openNextFile()) {
+		if (f.isDirectory()) {
+			continue;
+		}
+
+		callback(File(f));
+	}
+
+	dir.close();
+#else
+	auto dir = LittleFS.openDir(directory);
+	while (dir.next()) {
+		auto fd = dir.openFile("r");
+		if (!fd.isFile()) {
+			continue;
+		}
+
+		callback(File(fd));
+	}
+#endif
+}
+}  // namespace SlimeVR::Utils

src/FSHelper.h

@@ -0,0 +1,66 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 TheDevMinerTV
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#ifndef UTILS_FSHELPER_H
+#define UTILS_FSHELPER_H
+
+#include <LittleFS.h>
+#include <logging/Logger.h>
+
+#include <functional>
+
+namespace SlimeVR::Utils {
+
+class File {
+public:
+	File(fs::File file)
+		: m_File(file) {}
+
+	~File() {
+		if (m_File) {
+			m_File.close();
+		}
+	}
+
+	const char* name() const { return m_File.name(); }
+	size_t size() const { return m_File.size(); }
+	bool isDirectory() { return m_File.isDirectory(); }
+	bool seek(size_t pos) { return m_File.seek(pos); }
+	bool read(uint8_t* buffer, size_t size) { return m_File.read(buffer, size); }
+	bool write(const uint8_t* buffer, size_t size) {
+		return m_File.write(buffer, size);
+	}
+	void close() { return m_File.close(); }
+
+private:
+	fs::File m_File;
+};
+
+bool ensureDirectory(const char* directory);
+
+File openFile(const char* path, const char* mode);
+
+void forEachFile(const char* directory, std::function<void(File file)> callback);
+}  // namespace SlimeVR::Utils
+
+#endif

src/GlobalVars.h

@@ -1,35 +1,46 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
+#pragma once
 
-#ifndef GLOBALVARS_H
-#define GLOBALVARS_H
+#include <arduino-timer.h>
 
-#include "LEDManager.h"
-#include "status/StatusManager.h"
+#include "batterymonitor.h"
 #include "configuration/Configuration.h"
+#include "network/connection.h"
+#include "network/manager.h"
+#include "network/wifihandler.h"
+#include "network/wifiprovisioning.h"
+#include "sensors/SensorManager.h"
+#include "status/LEDManager.h"
+#include "status/StatusManager.h"
 
+extern Timer<> globalTimer;
 extern SlimeVR::LEDManager ledManager;
 extern SlimeVR::Status::StatusManager statusManager;
 extern SlimeVR::Configuration::Configuration configuration;
-
-#endif
+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/LEDManager.cpp

@@ -1,215 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
-
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
-
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
-
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-
-#include "LEDManager.h"
-#include "GlobalVars.h"
-#include "status/Status.h"
-
-namespace SlimeVR
-{
-    void LEDManager::setup()
-    {
-#if ENABLE_LEDS
-        pinMode(m_Pin, OUTPUT);
-#endif
-
-        // Do the initial pull of the state
-        update();
-    }
-
-    void LEDManager::on()
-    {
-#if ENABLE_LEDS
-        digitalWrite(m_Pin, LED__ON);
-#endif
-    }
-
-    void LEDManager::off()
-    {
-#if ENABLE_LEDS
-        digitalWrite(m_Pin, LED__OFF);
-#endif
-    }
-
-    void LEDManager::blink(unsigned long time)
-    {
-        on();
-        delay(time);
-        off();
-    }
-
-    void LEDManager::pattern(unsigned long timeon, unsigned long timeoff, int times)
-    {
-        for (int i = 0; i < times; i++)
-        {
-            blink(timeon);
-            delay(timeoff);
-        }
-    }
-
-    void LEDManager::update()
-    {
-        unsigned long time = millis();
-        unsigned long diff = time - m_LastUpdate;
-
-        // Don't tick the LEDManager *too* often
-        if (diff < 10)
-        {
-            return;
-        }
-
-        m_LastUpdate = time;
-
-        unsigned int length = 0;
-        unsigned int count = 0;
-
-        if (statusManager.hasStatus(Status::LOW_BATTERY))
-        {
-            count = LOW_BATTERY_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = LOW_BATTERY_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = LOW_BATTERY_INTERVAL;
-                break;
-            }
-        }
-        else if (statusManager.hasStatus(Status::IMU_ERROR))
-        {
-            count = IMU_ERROR_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = IMU_ERROR_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = IMU_ERROR_INTERVAL;
-                break;
-            }
-        }
-        else if (statusManager.hasStatus(Status::WIFI_CONNECTING))
-        {
-            count = WIFI_CONNECTING_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = WIFI_CONNECTING_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = WIFI_CONNECTING_INTERVAL;
-                break;
-            }
-        }
-        else if (statusManager.hasStatus(Status::SERVER_CONNECTING))
-        {
-            count = SERVER_CONNECTING_COUNT;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = SERVER_CONNECTING_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = SERVER_CONNECTING_INTERVAL;
-                break;
-            }
-        }
-        else
-        {
-#if defined(LED_INTERVAL_STANDBY) && LED_INTERVAL_STANDBY > 0
-            count = 1;
-            switch (m_CurrentStage)
-            {
-            case ON:
-            case OFF:
-                length = STANDBUY_LENGTH;
-                break;
-            case GAP:
-                length = DEFAULT_GAP;
-                break;
-            case INTERVAL:
-                length = LED_INTERVAL_STANDBY;
-                break;
-            }
-#else
-            return;
-#endif
-        }
-
-        if (m_CurrentStage == OFF || m_Timer + diff >= length)
-        {
-            m_Timer = 0;
-            // Advance stage
-            switch (m_CurrentStage)
-            {
-            case OFF:
-                on();
-                m_CurrentStage = ON;
-                m_CurrentCount = 0;
-                break;
-            case ON:
-                off();
-                m_CurrentCount++;
-                if (m_CurrentCount >= count)
-                {
-                    m_CurrentCount = 0;
-                    m_CurrentStage = INTERVAL;
-                }
-                else
-                {
-                    m_CurrentStage = GAP;
-                }
-                break;
-            case GAP:
-            case INTERVAL:
-                on();
-                m_CurrentStage = ON;
-                break;
-                on();
-                m_CurrentStage = ON;
-                break;
-            }
-        }
-        else
-        {
-            m_Timer += diff;
-        }
-    }
-}

src/LEDManager.h

@@ -1,103 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
-
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
-
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
-
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-#ifndef SLIMEVR_LEDMANAGER_H
-#define SLIMEVR_LEDMANAGER_H
-
-#include <Arduino.h>
-#include "globals.h"
-#include "logging/Logger.h"
-
-#define DEFAULT_LENGTH 300
-#define DEFAULT_GAP 500
-#define DEFAULT_INTERVAL 3000
-
-#define STANDBUY_LENGTH DEFAULT_LENGTH
-#define IMU_ERROR_LENGTH DEFAULT_LENGTH
-#define IMU_ERROR_INTERVAL 1000
-#define IMU_ERROR_COUNT 5
-#define LOW_BATTERY_LENGTH DEFAULT_LENGTH
-#define LOW_BATTERY_INTERVAL 300
-#define LOW_BATTERY_COUNT 1
-#define WIFI_CONNECTING_LENGTH DEFAULT_LENGTH
-#define WIFI_CONNECTING_INTERVAL 3000
-#define WIFI_CONNECTING_COUNT 3
-#define SERVER_CONNECTING_LENGTH DEFAULT_LENGTH
-#define SERVER_CONNECTING_INTERVAL 3000
-#define SERVER_CONNECTING_COUNT 2
-
-namespace SlimeVR
-{
-    enum LEDStage
-    {
-        OFF,
-        ON,
-        GAP,
-        INTERVAL
-    };
-
-    class LEDManager
-    {
-    public:
-        LEDManager(uint8_t pin) : m_Pin(pin) {}
-
-        void setup();
-
-        /*!
-         *  @brief Turns the LED on
-         */
-        void on();
-
-        /*!
-         *  @brief Turns the LED off
-         */
-        void off();
-
-        /*!
-         *  @brief Blink the LED for [time]ms. *Can* cause lag
-         *  @param time Amount of ms to turn the LED on
-         */
-        void blink(unsigned long time);
-
-        /*!
-         *  @brief Show a pattern on the LED. *Can* cause lag
-         *  @param timeon Amount of ms to turn the LED on
-         *  @param timeoff Amount of ms to turn the LED off
-         *  @param times Amount of times to display the pattern
-         */
-        void pattern(unsigned long timeon, unsigned long timeoff, int times);
-
-        void update();
-
-    private:
-        uint8_t m_CurrentCount = 0;
-        unsigned long m_Timer = 0;
-        LEDStage m_CurrentStage = OFF;
-        unsigned long m_LastUpdate = millis();
-
-        uint8_t m_Pin;
-
-        Logging::Logger m_Logger = Logging::Logger("LEDManager");
-    };
-}
-
-#endif

src/batterymonitor.cpp

@@ -1,135 +1,132 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 #include "batterymonitor.h"
+
 #include "GlobalVars.h"
 
-#if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
+#if ESP8266 \
+	&& (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
 ADC_MODE(ADC_VCC);
 #endif
 
-void BatteryMonitor::Setup()
-{
+void BatteryMonitor::Setup() {
 #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-    for (uint8_t i = 0x48; i < 0x4F; i++)
-    {
-        if (I2CSCAN::isI2CExist(i))
-        {
-            address = i;
-            break;
-        }
-    }
-    if (address == 0)
-    {
-        m_Logger.error("MCP3021 not found on I2C bus");
-    }
+	for (uint8_t i = 0x48; i < 0x4F; i++) {
+		if (I2CSCAN::hasDevOnBus(i)) {
+			address = i;
+			break;
+		}
+	}
+	if (address == 0) {
+		m_Logger.error("MCP3021 not found on I2C bus");
+	}
 #endif
 }
 
-void BatteryMonitor::Loop()
-{
-    #if BATTERY_MONITOR == BAT_EXTERNAL || BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-        auto now_ms = millis();
-        if (now_ms - last_battery_sample >= batterySampleRate)
-        {
-            last_battery_sample = now_ms;
-            voltage = -1;
-            #if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
-                // Find out what your max measurement is (voltage_3_3).
-                // Take the max measurement and check if it was less than 50mV 
-                // if yes output 5.0V
-                // if no output 3.3V - dropvoltage + 0.1V 
-                auto ESPmV = ESP.getVcc();
-                if (ESPmV > voltage_3_3)
-                {
-                    voltage_3_3 = ESPmV;
-                }
-                else
-                {
-                    //Calculate drop in mV
-                    ESPmV = voltage_3_3 - ESPmV;
-                    if (ESPmV < 50)
-                    {
-                        voltage = 5.0F;
-                    }
-                    else
-                    {
-                        voltage = 3.3F - ((float)ESPmV / 1000.0F) + 0.1F; //we assume 100mV drop on the linear converter
-                    }
-                }
-            #endif
-            #if BATTERY_MONITOR == BAT_EXTERNAL
-                voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * batteryADCMultiplier;
-            #endif
-            #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-                if (address > 0)
-                {
-                    Wire.beginTransmission(address);
-                    Wire.requestFrom(address, (uint8_t)2);
-                    auto MSB = Wire.read();
-                    auto LSB = Wire.read();
-                    auto status = Wire.endTransmission();
-                    if (status == 0)
-                    {
-                        float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2));
-                        v *= batteryADCMultiplier;
-                        voltage = (voltage > 0) ? min(voltage, v) : v;
-                    }
-                }
-            #endif
-            if (voltage > 0) //valid measurement
-            {
-                // Estimate battery level, 3.2V is 0%, 4.17V is 100% (1.0)
-                if (voltage > 3.975f)
-                    level = (voltage - 2.920f) * 0.8f;
-                else if (voltage > 3.678f)
-                    level = (voltage - 3.300f) * 1.25f;
-                else if (voltage > 3.489f)
-                    level = (voltage - 3.400f) * 1.7f;
-                else if (voltage > 3.360f)
-                    level = (voltage - 3.300f) * 0.8f;
-                else
-                    level = (voltage - 3.200f) * 0.3f;
+void BatteryMonitor::Loop() {
+#if BATTERY_MONITOR == BAT_EXTERNAL || BATTERY_MONITOR == BAT_INTERNAL \
+	|| BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
+	auto now_ms = millis();
+	if (now_ms - last_battery_sample >= batterySampleRate) {
+		last_battery_sample = now_ms;
+		voltage = -1;
+#if ESP8266 \
+	&& (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
+		// Find out what your max measurement is (voltage_3_3).
+		// Take the max measurement and check if it was less than 50mV
+		// if yes output 5.0V
+		// if no output 3.3V - dropvoltage + 0.1V
+		auto ESPmV = ESP.getVcc();
+		if (ESPmV > voltage_3_3) {
+			voltage_3_3 = ESPmV;
+		} else {
+			// Calculate drop in mV
+			ESPmV = voltage_3_3 - ESPmV;
+			if (ESPmV < 50) {
+				voltage = 5.0F;
+			} else {
+				voltage = 3.3F - ((float)ESPmV / 1000.0F)
+						+ 0.1F;  // we assume 100mV drop on the linear converter
+			}
+		}
+#endif
+#if ESP8266 && BATTERY_MONITOR == BAT_EXTERNAL
+		voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * ADCVoltageMax / ADCResolution
+				* ADCMultiplier;
+#endif
+#if defined(ESP32) && BATTERY_MONITOR == BAT_EXTERNAL
+		voltage
+			= ((float)analogReadMilliVolts(PIN_BATTERY_LEVEL)) / 1000 * ADCMultiplier;
+#endif
+#if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
+		if (address > 0) {
+			Wire.beginTransmission(address);
+			Wire.requestFrom(address, (uint8_t)2);
+			auto MSB = Wire.read();
+			auto LSB = Wire.read();
+			auto status = Wire.endTransmission();
+			if (status == 0) {
+				float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2));
+				v *= ADCMultiplier;
+				voltage = (voltage > 0) ? min(voltage, v) : v;
+			}
+		}
+#endif
+		if (voltage > 0)  // valid measurement
+		{
+			// Estimate battery level, 3.2V is 0%, 4.17V is 100% (1.0)
+			if (voltage > 3.975f) {
+				level = (voltage - 2.920f) * 0.8f;
+			} else if (voltage > 3.678f) {
+				level = (voltage - 3.300f) * 1.25f;
+			} else if (voltage > 3.489f) {
+				level = (voltage - 3.400f) * 1.7f;
+			} else if (voltage > 3.360f) {
+				level = (voltage - 3.300f) * 0.8f;
+			} else {
+				level = (voltage - 3.200f) * 0.3f;
+			}
 
-                level = (level - 0.05f) / 0.95f; // Cut off the last 5% (3.36V)
+			level = (level - 0.05f) / 0.95f;  // Cut off the last 5% (3.36V)
 
-                if (level > 1)
-                    level = 1;
-                else if (level < 0)
-                    level = 0;
-                Network::sendBatteryLevel(voltage, level);
-                #ifdef BATTERY_LOW_POWER_VOLTAGE
-                    if (voltage < BATTERY_LOW_POWER_VOLTAGE)
-                    {
-                        #if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP
-                            ESP.deepSleep(0);
-                        #else
-                            statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true);
-                        #endif
-                    } else {
-                        statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false);
-                    }
-                #endif
-            }
-        }
-    #endif
-}
+			if (level > 1) {
+				level = 1;
+			} else if (level < 0) {
+				level = 0;
+			}
+			networkConnection.sendBatteryLevel(voltage, level);
+#ifdef BATTERY_LOW_POWER_VOLTAGE
+			if (voltage < BATTERY_LOW_POWER_VOLTAGE) {
+#if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP
+				ESP.deepSleep(0);
+#else
+				statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true);
+#endif
+			} else {
+				statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false);
+			}
+#endif
+		}
+	}
+#endif
+}

src/batterymonitor.h

@@ -1,91 +1,95 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_BATTERYMONITOR_H_
 #define SLIMEVR_BATTERYMONITOR_H_
 
 #include <Arduino.h>
-#include "globals.h"
-#include "network/network.h"
-#include <i2cscan.h>
 #include <I2Cdev.h>
+#include <i2cscan.h>
+
+#include "globals.h"
 #include "logging/Logger.h"
 
 #if ESP8266
-    #define ADCResulution 1023.0  // ESP8266 has 12bit ADC
-    #define ADCVoltageMax 1.0     // ESP8266 input is 1.0 V = 1023.0
+#define ADCResolution 1023.0  // ESP8266 has 10bit ADC
+#define ADCVoltageMax 1.0  // ESP8266 input is 1.0 V = 1023.0
 #endif
-#if ESP32
-    #define ADCResulution 4095.0  // ESP32 has 12bit ADC
-    #define ADCVoltageMax 3.3     // ESP32 input is 3.3 V = 4095.0
-#endif
-#ifndef ADCResulution
-    #define ADCResulution 1023.0
+#ifndef ADCResolution
+#define ADCResolution 1023.0
 #endif
 #ifndef ADCVoltageMax
-    #define ADCVoltageMax 1.0
+#define ADCVoltageMax 1.0
 #endif
 
+#ifndef BATTERY_SHIELD_RESISTANCE
+#define BATTERY_SHIELD_RESISTANCE 180.0
+#endif
 #ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 100.0
+#define BATTERY_SHIELD_R1 100.0
 #endif
 #ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 220.0
+#define BATTERY_SHIELD_R2 220.0
 #endif
 
 #if BATTERY_MONITOR == BAT_EXTERNAL
-    #ifndef PIN_BATTERY_LEVEL
-        #error Internal ADC enabled without pin! Please select a pin.
-    #endif
-    // Wemos D1 Mini has an internal Voltage Divider with R1=220K and R2=100K > this means, 3.3V analogRead input voltage results in 1023.0
-    // Wemos D1 Mini with Wemos Battery Shield v1.2.0 or higher: Battery Shield with J2 closed, has an additional 130K resistor. So the resulting Voltage Divider is R1=220K+100K=320K and R2=100K > this means, 4.5V analogRead input voltage results in 1023.0
-    // ESP32 Boards may have not the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can be changed.
-    // Diagramm: 
-    //   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2] ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
-    // SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0
-  #define batteryADCMultiplier ADCVoltageMax / ADCResulution * (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) / BATTERY_SHIELD_R1
+// Wemos D1 Mini has an internal Voltage Divider with R1=100K and R2=220K > this
+// means, 3.3V analogRead input voltage results in 1023.0 Wemos D1 Mini with Wemos
+// Battery Shield v1.2.0 or higher: Battery Shield with J2 closed, has an additional
+// 130K resistor. So the resulting Voltage Divider is R1=220K+100K=320K and R2=100K >
+// this means, 4.5V analogRead input voltage results in 1023.0 ESP32 Boards may have not
+// the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can
+// be changed. Diagramm:
+//   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2]
+//   ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
+// SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0
+#define ADCMultiplier                                                   \
+	(BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) \
+		/ BATTERY_SHIELD_R1
 #elif BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-  // Default recommended resistors are 9.1k and 5.1k
-  #define batteryADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1
+// Default recommended resistors are 9.1k and 5.1k
+#define ADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1
 #endif
 
-class BatteryMonitor
-{
+class BatteryMonitor {
 public:
-    void Setup();
-    void Loop();
+	void Setup();
+	void Loop();
+
+	float getVoltage() const { return voltage; }
+	float getLevel() const { return level; }
 
 private:
-    unsigned long last_battery_sample = 0;
+	unsigned long last_battery_sample = 0;
 #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-    uint8_t address = 0;
+	uint8_t address = 0;
 #endif
 #if BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
-    uint16_t voltage_3_3 = 3000;
+	uint16_t voltage_3_3 = 3000;
 #endif
-    float voltage = -1;
-    float level = -1;
+	float voltage = -1;
+	float level = -1;
 
-    SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor");
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor");
 };
 
-#endif // SLIMEVR_BATTERYMONITOR_H_
+#endif  // SLIMEVR_BATTERYMONITOR_H_

src/boards/boards_default.h

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

src/boards/defines_helpers.cpp

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

src/boards/defines_helpers.h

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

src/boards/glove_default.h

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

src/calibration.h

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

src/configuration/CalibrationConfig.cpp

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

src/configuration/CalibrationConfig.h

@@ -1,92 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
-
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
-
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
-
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-
-#ifndef SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H
-#define SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H
-
-#include <stdint.h>
-
-namespace SlimeVR {
-    namespace Configuration {
-        struct BMI160CalibrationConfig {
-            // accelerometer offsets and correction matrix
-            float A_B[3];
-            float A_Ainv[3][3];
-
-            // raw offsets, determined from gyro at rest
-            float G_off[3];
-
-            // calibration temperature for dynamic compensation
-            float temperature;
-        };
-
-        struct MPU6050CalibrationConfig {
-            // accelerometer offsets and correction matrix
-            float A_B[3];
-
-            // raw offsets, determined from gyro at rest
-            float G_off[3];
-        };
-
-        struct MPU9250CalibrationConfig {
-            // accelerometer offsets and correction matrix
-            float A_B[3];
-            float A_Ainv[3][3];
-
-            // magnetometer offsets and correction matrix
-            float M_B[3];
-            float M_Ainv[3][3];
-
-            // raw offsets, determined from gyro at rest
-            float G_off[3];
-        };
-
-        struct ICM20948CalibrationConfig {
-            // gyroscope bias
-            int32_t G[3];
-
-            // accelerometer bias
-            int32_t A[3];
-
-            // compass bias
-            int32_t C[3];
-        };
-
-        enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948 };
-
-        const char* calibrationConfigTypeToString(CalibrationConfigType type);
-
-        struct CalibrationConfig {
-            CalibrationConfigType type;
-
-            union {
-                BMI160CalibrationConfig bmi160;
-                MPU6050CalibrationConfig mpu6050;
-                MPU9250CalibrationConfig mpu9250;
-                ICM20948CalibrationConfig icm20948;
-            } data;
-        };
-    }
-}
-
-#endif

src/configuration/Configuration.cpp

@@ -1,316 +1,489 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 
-#ifdef ESP32
-    // #define CONFIG_LITTLEFS_SPIFFS_COMPAT 1
-    // #define CONFIG_LITTLEFS_LFS_YES_TRACE
-    // #define LFS_YES_TRACE
-    #include <LITTLEFS.h>
-    #define LittleFS LITTLEFS
-#else
-    #include <LittleFS.h>
-#endif
-
 #include "Configuration.h"
+
+#include <LittleFS.h>
+
+#include <cstdint>
+#include <cstring>
+
+#include "../FSHelper.h"
 #include "consts.h"
+#include "sensors/SensorToggles.h"
 #include "utils.h"
 
-namespace SlimeVR {
-    namespace Configuration {
-        CalibrationConfig Configuration::m_EmptyCalibration = {NONE};
+#define DIR_CALIBRATIONS "/calibrations"
+#define DIR_TEMPERATURE_CALIBRATIONS "/tempcalibrations"
+#define DIR_TOGGLES_OLD "/toggles"
+#define DIR_TOGGLES "/sensortoggles"
 
-        void Configuration::setup() {
-            if (m_Loaded) {
-                return;
-            }
+namespace SlimeVR::Configuration {
+void Configuration::setup() {
+	if (m_Loaded) {
+		return;
+	}
 
-            bool status = LittleFS.begin();
-            if (!status) {
-                this->m_Logger.warn("Could not mount LittleFS, formatting");
+	bool status = LittleFS.begin();
+	if (!status) {
+		this->m_Logger.warn("Could not mount LittleFS, formatting");
 
-                status = LittleFS.format();
-                if (!status) {
-                    this->m_Logger.warn("Could not format LittleFS, aborting");
-                    return;
-                }
+		status = LittleFS.format();
+		if (!status) {
+			this->m_Logger.warn("Could not format LittleFS, aborting");
+			return;
+		}
 
-                status = LittleFS.begin();
-                if (!status) {
-                    this->m_Logger.error("Could not mount LittleFS, aborting");
-                    return;
-                }
-            }
+		status = LittleFS.begin();
+		if (!status) {
+			this->m_Logger.error("Could not mount LittleFS, aborting");
+			return;
+		}
+	}
 
-            if (LittleFS.exists("/config.bin")) {
-                m_Logger.trace("Found configuration file");
+	if (LittleFS.exists("/config.bin")) {
+		m_Logger.trace("Found configuration file");
 
-                File file = LittleFS.open("/config.bin", "r");
+		auto file = LittleFS.open("/config.bin", "r");
 
-                file.read((uint8_t*)&m_Config.version, sizeof(int32_t));
+		file.read((uint8_t*)&m_Config.version, sizeof(int32_t));
 
-                if (m_Config.version < CURRENT_CONFIGURATION_VERSION) {
-                    m_Logger.debug("Configuration is outdated: v%d < v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION);
+		if (m_Config.version < CURRENT_CONFIGURATION_VERSION) {
+			m_Logger.debug(
+				"Configuration is outdated: v%d < v%d",
+				m_Config.version,
+				CURRENT_CONFIGURATION_VERSION
+			);
 
-                    if (!runMigrations(m_Config.version)) {
-                        m_Logger.error("Failed to migrate configuration from v%d to v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION);
-                        return;
-                    }
-                } else {
-                    m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
-                }
+			if (!runMigrations(m_Config.version)) {
+				m_Logger.error(
+					"Failed to migrate configuration from v%d to v%d",
+					m_Config.version,
+					CURRENT_CONFIGURATION_VERSION
+				);
+				return;
+			}
+		} else {
+			m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
+		}
 
-                file.seek(0);
-                file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
-                file.close();
-            } else {
-                m_Logger.info("No configuration file found, creating new one");
-                m_Config.version = CURRENT_CONFIGURATION_VERSION;
-                save();
-            }
+		file.seek(0);
+		file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
+		file.close();
+	} else {
+		m_Logger.info("No configuration file found, creating new one");
+		m_Config.version = CURRENT_CONFIGURATION_VERSION;
+		save();
+	}
 
-            loadCalibrations();
+	loadSensors();
 
-            m_Loaded = true;
+	m_Loaded = true;
 
-            m_Logger.info("Loaded configuration");
+	m_Logger.info("Loaded configuration");
 
 #ifdef DEBUG_CONFIGURATION
-            print();
+	print();
 #endif
-        }
-
-        void Configuration::save() {
-            for (size_t i = 0; i < m_Calibrations.size(); i++) {
-                CalibrationConfig config = m_Calibrations[i];
-                if (config.type == CalibrationConfigType::NONE) {
-                    continue;
-                }
-
-                char path[17];
-                sprintf(path, "/calibrations/%d", i);
-
-                m_Logger.trace("Saving calibration data for %d", i);
-
-                File file = LittleFS.open(path, "w");
-                file.write((uint8_t*)&config, sizeof(CalibrationConfig));
-                file.close();
-            }
-
-            {
-                File file = LittleFS.open("/config.bin", "w");
-                file.write((uint8_t*)&m_Config, sizeof(DeviceConfig));
-                file.close();
-            }
-
-            m_Logger.debug("Saved configuration");
-        }
-
-        void Configuration::reset() {
-            LittleFS.format();
-
-            m_Calibrations.clear();
-            m_Config.version = 1;
-            save();
-
-            m_Logger.debug("Reset configuration");
-        }
-
-        int32_t Configuration::getVersion() const {
-            return m_Config.version;
-        }
-
-        size_t Configuration::getCalibrationCount() const {
-            return m_Calibrations.size();
-        }
-
-        CalibrationConfig Configuration::getCalibration(size_t sensorID) const {
-            if (sensorID >= m_Calibrations.size()) {
-                return m_EmptyCalibration;
-            }
-
-            return m_Calibrations.at(sensorID);
-        }
-
-        void Configuration::setCalibration(size_t sensorID, const CalibrationConfig& config) {
-            size_t currentCalibrations = m_Calibrations.size();
-
-            if (sensorID >= currentCalibrations) {
-                m_Calibrations.resize(sensorID + 1, m_EmptyCalibration);
-            }
-
-            m_Calibrations[sensorID] = config;
-        }
-
-        void Configuration::loadCalibrations() {
-#ifdef ESP32
-            {
-                File calibrations = LittleFS.open("/calibrations");
-                if (!calibrations) {
-                    m_Logger.warn("No calibration data found, creating new directory...");
-
-                    if (!LittleFS.mkdir("/calibrations")) {
-                        m_Logger.error("Failed to create directory: /calibrations");
-                        return;
-                    }
-
-                    calibrations = LittleFS.open("/calibrations");
-                }
-
-                if (!calibrations.isDirectory()) {
-                    calibrations.close();
-
-                    m_Logger.warn("Found file instead of directory: /calibrations");
-
-                    if (!LittleFS.remove("/calibrations")) {
-                        m_Logger.error("Failed to remove directory: /calibrations");
-                        return;
-                    }
-
-                    if (!LittleFS.mkdir("/calibrations")) {
-                        m_Logger.error("Failed to create directory: /calibrations");
-                        return;
-                    }
-
-                    calibrations = LittleFS.open("/calibrations");
-                }
-
-                m_Logger.debug("Found calibration data directory");
-
-                while (File f = calibrations.openNextFile()) {
-                    if (f.isDirectory()) {
-                        continue;
-                    }
-
-                    m_Logger.trace("Found calibration data file: %s", f.name());
-
-                    CalibrationConfig calibrationConfig;
-                    f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig));
-                    f.close();
-
-                    uint8_t sensorId = strtoul(calibrations.name(), nullptr, 10);
-                    m_Logger.debug("Found sensor calibration for %s at index %d", calibrationConfigTypeToString(calibrationConfig.type), sensorId);
-
-                    setCalibration(sensorId, calibrationConfig);
-                }
-
-                calibrations.close();
-            }
-#else
-            {
-                if (!LittleFS.exists("/calibrations")) {
-                    m_Logger.warn("No calibration data found, creating new directory...");
-
-                    if (!LittleFS.mkdir("/calibrations")) {
-                        m_Logger.error("Failed to create directory: /calibrations");
-                        return;
-                    }
-
-                    // There's no calibrations here, so we're done
-                    return;
-                }
-
-                Dir calibrations = LittleFS.openDir("/calibrations");
-                while (calibrations.next()) {
-                    File f = calibrations.openFile("r");
-                    if (!f.isFile()) {
-                        continue;
-                    }
-
-                    CalibrationConfig calibrationConfig;
-                    f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig));
-
-                    uint8_t sensorId = strtoul(calibrations.fileName().c_str(), nullptr, 10);
-                    m_Logger.debug("Found sensor calibration for %s at index %d", calibrationConfigTypeToString(calibrationConfig.type), sensorId);
-
-                    setCalibration(sensorId, calibrationConfig);
-                }
-            }
-#endif
-        }
-
-        bool Configuration::runMigrations(int32_t version) {
-            return true;
-        }
-
-        void Configuration::print() {
-            m_Logger.info("Configuration:");
-            m_Logger.info("  Version: %d", m_Config.version);
-            m_Logger.info("  %d Calibrations:", m_Calibrations.size());
-
-            for (size_t i = 0; i < m_Calibrations.size(); i++) {
-                const CalibrationConfig& c = m_Calibrations[i];
-                m_Logger.info("    - [%3d] %s", i, calibrationConfigTypeToString(c.type));
-
-                switch (c.type) {
-                case CalibrationConfigType::NONE:
-                    break;
-
-                case CalibrationConfigType::BMI160:
-                    m_Logger.info("            A_B        : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B));
-
-                    m_Logger.info("            A_Ainv     :");
-                    for (uint8_t i = 0; i < 3; i++) {
-                        m_Logger.info("                         %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_Ainv[i]));
-                    }
-
-                    m_Logger.info("            G_off      : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.G_off));
-                    m_Logger.info("            Temperature: %f", c.data.bmi160.temperature);
-
-                    break;
-
-                case CalibrationConfigType::ICM20948:
-                    m_Logger.info("            G: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.G));
-                    m_Logger.info("            A: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.A));
-                    m_Logger.info("            C: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.C));
-
-                    break;
-
-                case CalibrationConfigType::MPU9250:
-                    m_Logger.info("            A_B   : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B));
-
-                    m_Logger.info("            A_Ainv:");
-                    for (uint8_t i = 0; i < 3; i++) {
-                        m_Logger.info("                    %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_Ainv[i]));
-                    }
-
-                    m_Logger.info("            M_B   : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B));
-
-                    m_Logger.info("            M_Ainv:");
-                    for (uint8_t i = 0; i < 3; i++) {
-                        m_Logger.info("                    %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_Ainv[i]));
-                    }
-
-                    m_Logger.info("            G_off  : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off));
-
-                    break;
-
-                case CalibrationConfigType::MPU6050:
-                    m_Logger.info("            A_B  : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B));
-                    m_Logger.info("            G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off));
-
-                    break;
-                }
-            }
-        }
-    }
 }
+
+void Configuration::save() {
+	for (size_t i = 0; i < m_Sensors.size(); i++) {
+		SensorConfig config = m_Sensors[i];
+		if (config.type == SensorConfigType::NONE) {
+			continue;
+		}
+
+		char path[17];
+		sprintf(path, DIR_CALIBRATIONS "/%zu", i);
+
+		m_Logger.trace("Saving sensor config data for %d", i);
+
+		File file = LittleFS.open(path, "w");
+		file.write((uint8_t*)&config, sizeof(SensorConfig));
+		file.close();
+
+		if (i < m_SensorToggles.size()) {
+			sprintf(path, DIR_TOGGLES "/%zu", i);
+
+			m_Logger.trace("Saving sensor toggle state for %d", i);
+
+			file = LittleFS.open(path, "w");
+			auto toggleValues = m_SensorToggles[i].getValues();
+			file.write((uint8_t*)&toggleValues, sizeof(SensorToggleValues));
+			file.close();
+		} else {
+			m_Logger.trace(
+				"Skipping saving toggles for sensor %d, no toggles present",
+				i
+			);
+		}
+	}
+
+	{
+		File file = LittleFS.open("/config.bin", "w");
+		file.write((uint8_t*)&m_Config, sizeof(DeviceConfig));
+		file.close();
+	}
+
+	// Clean up old toggles directory
+	if (LittleFS.exists(DIR_TOGGLES_OLD)) {
+		char path[17] = DIR_TOGGLES_OLD;
+		char* end = path + strlen(DIR_TOGGLES_OLD);
+		Utils::forEachFile(DIR_TOGGLES_OLD, [&](SlimeVR::Utils::File file) {
+			sprintf(end, "/%s", file.name());
+			LittleFS.remove(path);
+			file.close();
+		});
+		LittleFS.rmdir(DIR_TOGGLES_OLD);
+	}
+
+	m_Logger.debug("Saved configuration");
+}
+
+void Configuration::reset() {
+	LittleFS.format();
+
+	m_Sensors.clear();
+	m_SensorToggles.clear();
+	m_Config.version = 1;
+	save();
+
+	m_Logger.debug("Reset configuration");
+}
+
+int32_t Configuration::getVersion() const { return m_Config.version; }
+
+size_t Configuration::getSensorCount() const { return m_Sensors.size(); }
+
+SensorConfig Configuration::getSensor(size_t sensorID) const {
+	if (sensorID >= m_Sensors.size()) {
+		return {};
+	}
+
+	return m_Sensors.at(sensorID);
+}
+
+void Configuration::setSensor(size_t sensorID, const SensorConfig& config) {
+	size_t currentSensors = m_Sensors.size();
+
+	if (sensorID >= currentSensors) {
+		m_Sensors.resize(sensorID + 1);
+	}
+
+	m_Sensors[sensorID] = config;
+}
+
+SensorToggleState Configuration::getSensorToggles(size_t sensorId) const {
+	if (sensorId >= m_SensorToggles.size()) {
+		return {};
+	}
+
+	return m_SensorToggles.at(sensorId);
+}
+
+void Configuration::setSensorToggles(size_t sensorId, SensorToggleState state) {
+	size_t currentSensors = m_SensorToggles.size();
+
+	if (sensorId >= currentSensors) {
+		m_SensorToggles.resize(sensorId + 1);
+	}
+
+	m_SensorToggles[sensorId] = state;
+}
+
+void Configuration::eraseSensors() {
+	m_Sensors.clear();
+
+	SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
+		char path[17];
+		sprintf(path, DIR_CALIBRATIONS "/%s", f.name());
+
+		f.close();
+
+		LittleFS.remove(path);
+	});
+
+	save();
+}
+
+void Configuration::loadSensors() {
+	SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
+		SensorConfig sensorConfig;
+		f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig));
+
+		uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+		m_Logger.debug(
+			"Found sensor calibration for %s at index %d",
+			calibrationConfigTypeToString(sensorConfig.type),
+			sensorId
+		);
+
+		if (sensorConfig.type == SensorConfigType::BNO0XX) {
+			SensorToggleState toggles;
+			toggles.setToggle(
+				SensorToggles::MagEnabled,
+				sensorConfig.data.bno0XX.magEnabled
+			);
+			setSensorToggles(sensorId, toggles);
+		}
+
+		setSensor(sensorId, sensorConfig);
+	});
+
+	if (LittleFS.exists(DIR_TOGGLES_OLD)) {
+		SlimeVR::Utils::forEachFile(DIR_TOGGLES_OLD, [&](SlimeVR::Utils::File f) {
+			SensorToggleValues values;
+			// Migration for pre 0.7.0 togglestate, the values started at offset 20 and
+			// there were 3 of them
+			f.seek(20);
+			f.read(reinterpret_cast<uint8_t*>(&values), 3);
+
+			uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+			m_Logger.debug("Found sensor toggle state at index %d", sensorId);
+
+			setSensorToggles(sensorId, SensorToggleState{values});
+		});
+	}
+
+	SlimeVR::Utils::forEachFile(DIR_TOGGLES, [&](SlimeVR::Utils::File f) {
+		if (f.size() > sizeof(SensorToggleValues)) {
+			return;
+		}
+		SensorToggleValues values;
+		// With the magic of C++ default initialization, the rest of the values should
+		// be their default after reading
+		f.read(reinterpret_cast<uint8_t*>(&values), f.size());
+
+		uint8_t sensorId = strtoul(f.name(), nullptr, 10);
+		m_Logger.debug("Found sensor toggle state at index %d", sensorId);
+
+		setSensorToggles(sensorId, SensorToggleState{values});
+	});
+}
+
+bool Configuration::loadTemperatureCalibration(
+	uint8_t sensorId,
+	GyroTemperatureCalibrationConfig& config
+) {
+	if (!SlimeVR::Utils::ensureDirectory(DIR_TEMPERATURE_CALIBRATIONS)) {
+		return false;
+	}
+
+	char path[32];
+	sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
+
+	if (!LittleFS.exists(path)) {
+		return false;
+	}
+
+	auto f = SlimeVR::Utils::openFile(path, "r");
+	if (f.isDirectory()) {
+		return false;
+	}
+
+	if (f.size() != sizeof(GyroTemperatureCalibrationConfig)) {
+		m_Logger.debug(
+			"Found incompatible sensor temperature calibration (size mismatch) "
+			"sensorId:%d, skipping",
+			sensorId
+		);
+		return false;
+	}
+
+	SensorConfigType storedConfigType;
+	f.read((uint8_t*)&storedConfigType, sizeof(SensorConfigType));
+
+	if (storedConfigType != config.type) {
+		m_Logger.debug(
+			"Found incompatible sensor temperature calibration (expected %s, "
+			"found %s) sensorId:%d, skipping",
+			calibrationConfigTypeToString(config.type),
+			calibrationConfigTypeToString(storedConfigType),
+			sensorId
+		);
+		return false;
+	}
+
+	f.seek(0);
+	f.read((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
+	m_Logger.debug(
+		"Found sensor temperature calibration for %s sensorId:%d",
+		calibrationConfigTypeToString(config.type),
+		sensorId
+	);
+	return true;
+}
+
+bool Configuration::saveTemperatureCalibration(
+	uint8_t sensorId,
+	const GyroTemperatureCalibrationConfig& config
+) {
+	if (config.type == SensorConfigType::NONE) {
+		return false;
+	}
+
+	char path[32];
+	sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
+
+	m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId);
+
+	File file = LittleFS.open(path, "w");
+	file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
+	file.close();
+
+	m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId);
+	return true;
+}
+
+bool Configuration::runMigrations(int32_t version) { return true; }
+
+void Configuration::print() {
+	m_Logger.info("Configuration:");
+	m_Logger.info("  Version: %d", m_Config.version);
+	m_Logger.info("  %d Sensors:", m_Sensors.size());
+
+	for (size_t i = 0; i < m_Sensors.size(); i++) {
+		const SensorConfig& c = m_Sensors[i];
+		m_Logger.info("    - [%3d] %s", i, calibrationConfigTypeToString(c.type));
+
+		switch (c.type) {
+			case SensorConfigType::NONE:
+				break;
+
+			case SensorConfigType::BMI160:
+				m_Logger.info(
+					"            A_B        : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B)
+				);
+
+				m_Logger.info("            A_Ainv     :");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                         %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.bmi160.A_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            G_off      : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.bmi160.G_off)
+				);
+				m_Logger.info("            Temperature: %f", c.data.bmi160.temperature);
+
+				break;
+
+			case SensorConfigType::SFUSION:
+				m_Logger.info(
+					"            A_B        : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B)
+				);
+
+				m_Logger.info("            A_Ainv     :");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                         %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.sfusion.A_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            G_off      : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.sfusion.G_off)
+				);
+				m_Logger.info(
+					"            Temperature: %f",
+					c.data.sfusion.temperature
+				);
+				break;
+
+			case SensorConfigType::ICM20948:
+				m_Logger.info(
+					"            G: %d, %d, %d",
+					UNPACK_VECTOR_ARRAY(c.data.icm20948.G)
+				);
+				m_Logger.info(
+					"            A: %d, %d, %d",
+					UNPACK_VECTOR_ARRAY(c.data.icm20948.A)
+				);
+				m_Logger.info(
+					"            C: %d, %d, %d",
+					UNPACK_VECTOR_ARRAY(c.data.icm20948.C)
+				);
+
+				break;
+
+			case SensorConfigType::MPU9250:
+				m_Logger.info(
+					"            A_B   : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B)
+				);
+
+				m_Logger.info("            A_Ainv:");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                    %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            M_B   : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B)
+				);
+
+				m_Logger.info("            M_Ainv:");
+				for (uint8_t i = 0; i < 3; i++) {
+					m_Logger.info(
+						"                    %f, %f, %f",
+						UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_Ainv[i])
+					);
+				}
+
+				m_Logger.info(
+					"            G_off  : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off)
+				);
+
+				break;
+
+			case SensorConfigType::MPU6050:
+				m_Logger.info(
+					"            A_B  : %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)
+				);
+				m_Logger.info(
+					"            G_off: %f, %f, %f",
+					UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)
+				);
+
+				break;
+
+			case SensorConfigType::BNO0XX:
+				m_Logger.info("            magEnabled: %d", c.data.bno0XX.magEnabled);
+
+				break;
+		}
+	}
+}
+}  // namespace SlimeVR::Configuration

src/configuration/Configuration.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 
 #ifndef SLIMEVR_CONFIGURATION_CONFIGURATION_H
@@ -26,40 +26,51 @@
 
 #include <vector>
 
+#include "../motionprocessing/GyroTemperatureCalibrator.h"
+#include "../sensors/SensorToggles.h"
 #include "DeviceConfig.h"
 #include "logging/Logger.h"
 
-namespace SlimeVR {
-    namespace Configuration {
-        class Configuration {
-        public:
-            void setup();
+namespace SlimeVR::Configuration {
+class Configuration {
+public:
+	void setup();
 
-            void save();
-            void reset();
+	void save();
+	void reset();
 
-            void print();
+	void print();
 
-            int32_t getVersion() const;
+	int32_t getVersion() const;
 
-            size_t getCalibrationCount() const;
-            CalibrationConfig getCalibration(size_t sensorID) const;
-            void setCalibration(size_t sensorID, const CalibrationConfig& config);
+	size_t getSensorCount() const;
+	SensorConfig getSensor(size_t sensorID) const;
+	void setSensor(size_t sensorID, const SensorConfig& config);
+	SensorToggleState getSensorToggles(size_t sensorId) const;
+	void setSensorToggles(size_t sensorId, SensorToggleState state);
+	void eraseSensors();
 
-        private:
-            void loadCalibrations();
-            bool runMigrations(int32_t version);
+	bool loadTemperatureCalibration(
+		uint8_t sensorId,
+		GyroTemperatureCalibrationConfig& config
+	);
+	bool saveTemperatureCalibration(
+		uint8_t sensorId,
+		const GyroTemperatureCalibrationConfig& config
+	);
 
-            bool m_Loaded = false;
+private:
+	void loadSensors();
+	bool runMigrations(int32_t version);
 
-            DeviceConfig m_Config{};
-            std::vector<CalibrationConfig> m_Calibrations;
+	bool m_Loaded = false;
 
-            Logging::Logger m_Logger = Logging::Logger("Configuration");
+	DeviceConfig m_Config{};
+	std::vector<SensorConfig> m_Sensors;
+	std::vector<SensorToggleState> m_SensorToggles;
 
-            static CalibrationConfig m_EmptyCalibration;
-        };
-    }
-}
+	Logging::Logger m_Logger = Logging::Logger("Configuration");
+};
+}  // namespace SlimeVR::Configuration
 
 #endif

src/configuration/DeviceConfig.h

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

src/configuration/SensorConfig.cpp

@@ -0,0 +1,62 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "SensorConfig.h"
+
+namespace SlimeVR::Configuration {
+const char* calibrationConfigTypeToString(SensorConfigType type) {
+	switch (type) {
+		case SensorConfigType::NONE:
+			return "NONE";
+		case SensorConfigType::BMI160:
+			return "BMI160";
+		case SensorConfigType::MPU6050:
+			return "MPU6050";
+		case SensorConfigType::MPU9250:
+			return "MPU9250";
+		case SensorConfigType::ICM20948:
+			return "ICM20948";
+		case SensorConfigType::SFUSION:
+			return "SoftFusion (common)";
+		case SensorConfigType::BNO0XX:
+			return "BNO0XX";
+		case SensorConfigType::RUNTIME_CALIBRATION:
+			return "SoftFusion (runtime calibration)";
+		default:
+			return "UNKNOWN";
+	}
+}
+
+bool SensorConfigBits::operator==(const SensorConfigBits& rhs) const {
+	return magEnabled == rhs.magEnabled && magSupported == rhs.magSupported
+		&& calibrationEnabled == rhs.calibrationEnabled
+		&& calibrationSupported == rhs.calibrationSupported
+		&& tempGradientCalibrationEnabled == rhs.tempGradientCalibrationEnabled
+		&& tempGradientCalibrationSupported == rhs.tempGradientCalibrationSupported;
+}
+
+bool SensorConfigBits::operator!=(const SensorConfigBits& rhs) const {
+	return !(*this == rhs);
+}
+
+}  // namespace SlimeVR::Configuration

src/configuration/SensorConfig.h

@@ -0,0 +1,200 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#ifndef SLIMEVR_CONFIGURATION_SENSORCONFIG_H
+#define SLIMEVR_CONFIGURATION_SENSORCONFIG_H
+
+#include <stdint.h>
+
+#include "consts.h"
+
+namespace SlimeVR::Configuration {
+struct BMI160SensorConfig {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+
+	// calibration temperature for dynamic compensation
+	float temperature;
+};
+
+struct SoftFusionSensorConfig {
+	SensorTypeID ImuType;
+	uint16_t MotionlessDataLen;
+
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+
+	// calibration temperature for dynamic compensation
+	float temperature;
+
+	// real measured sensor sampling rate
+	float A_Ts;
+	float G_Ts;
+	float M_Ts;
+
+	// gyro sensitivity multiplier
+	float G_Sens[3];
+
+	uint8_t MotionlessData[60];
+
+	// temperature sampling rate (placed at the end to not break existing configs)
+	float T_Ts;
+};
+
+struct RuntimeCalibrationSensorConfig {
+	SensorTypeID ImuType;
+	uint16_t MotionlessDataLen;
+
+	bool sensorTimestepsCalibrated;
+	float A_Ts;
+	float G_Ts;
+	float M_Ts;
+	float T_Ts;
+
+	bool motionlessCalibrated;
+	uint8_t MotionlessData[60];
+
+	uint8_t gyroPointsCalibrated;
+	float gyroMeasurementTemperature1;
+	float G_off1[3];
+	float gyroMeasurementTemperature2;
+	float G_off2[3];
+
+	bool accelCalibrated[3];
+	float A_off[3];
+};
+
+struct MPU6050SensorConfig {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+};
+
+struct MPU9250SensorConfig {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+};
+
+struct ICM20948SensorConfig {
+	// gyroscope bias
+	int32_t G[3];
+
+	// accelerometer bias
+	int32_t A[3];
+
+	// compass bias
+	int32_t C[3];
+};
+
+struct ICM42688SensorConfig {
+	// accelerometer offsets and correction matrix
+	float A_B[3];
+	float A_Ainv[3][3];
+
+	// magnetometer offsets and correction matrix
+	float M_B[3];
+	float M_Ainv[3][3];
+
+	// raw offsets, determined from gyro at rest
+	float G_off[3];
+};
+
+struct BNO0XXSensorConfig {
+	bool magEnabled;
+};
+
+enum class SensorConfigType {
+	NONE,
+	BMI160,
+	MPU6050,
+	MPU9250,
+	ICM20948,
+	SFUSION,
+	BNO0XX,
+	RUNTIME_CALIBRATION,
+};
+
+const char* calibrationConfigTypeToString(SensorConfigType type);
+
+struct SensorConfig {
+	SensorConfigType type;
+
+	union {
+		BMI160SensorConfig bmi160;
+		SoftFusionSensorConfig sfusion;
+		MPU6050SensorConfig mpu6050;
+		MPU9250SensorConfig mpu9250;
+		ICM20948SensorConfig icm20948;
+		BNO0XXSensorConfig bno0XX;
+		RuntimeCalibrationSensorConfig runtimeCalibration;
+	} data;
+};
+
+struct SensorConfigBits {
+	bool magEnabled : 1;
+	bool magSupported : 1;
+	bool calibrationEnabled : 1;
+	bool calibrationSupported : 1;
+	bool tempGradientCalibrationEnabled : 1;
+	bool tempGradientCalibrationSupported : 1;
+
+	// Remove if the above fields exceed a byte, necessary to make the struct 16
+	// bit
+	uint8_t padding;
+
+	bool operator==(const SensorConfigBits& rhs) const;
+	bool operator!=(const SensorConfigBits& rhs) const;
+};
+
+// If this fails, you forgot to do the above
+static_assert(sizeof(SensorConfigBits) == 2);
+
+}  // namespace SlimeVR::Configuration
+
+#endif

src/consts.h

@@ -1,48 +1,105 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_CONSTS_H_
 #define SLIMEVR_CONSTS_H_
 
 // List of constants used in other places
-#define IMU_MPU9250 1
-#define IMU_MPU6500 2
-#define IMU_BNO080 3
-#define IMU_BNO085 4
-#define IMU_BNO055 5
-#define IMU_MPU6050 6
-#define IMU_BNO086 7
-#define IMU_BMI160 8
-#define IMU_ICM20948 9
 
-#define BOARD_SLIMEVR_LEGACY 1
-#define BOARD_SLIMEVR_DEV 2
+#include <cstdint>
+enum class SensorTypeID : uint8_t {
+	Unknown = 0,
+	MPU9250,
+	MPU6500,
+	BNO080,
+	BNO085,
+	BNO055,
+	MPU6050,
+	BNO086,
+	BMI160,
+	ICM20948,
+	ICM42688,
+	BMI270,
+	LSM6DS3TRC,
+	LSM6DSV,
+	LSM6DSO,
+	LSM6DSR,
+	ICM45686,
+	ICM45605,
+	ADC_RESISTANCE,
+	Empty = 255
+};
+
+#define IMU_AUTO SensorAuto
+#define IMU_UNKNOWN ErroneousSensor
+#define IMU_MPU9250 MPU9250Sensor
+#define IMU_MPU6500 MPU6050Sensor
+#define IMU_BNO080 BNO080Sensor
+#define IMU_BNO085 BNO085Sensor
+#define IMU_BNO055 BNO055Sensor
+#define IMU_MPU6050 MPU6050Sensor
+#define IMU_BNO086 BNO086Sensor
+#define IMU_BMI160 SoftFusionBMI160
+#define IMU_ICM20948 ICM20948Sensor
+#define IMU_ICM42688 SoftFusionICM42688
+#define IMU_BMI270 SoftFusionBMI270
+#define IMU_LSM6DS3TRC SoftFusionLSM6DS3TRC
+#define IMU_LSM6DSV SoftFusionLSM6DSV
+#define IMU_LSM6DSO SoftFusionLSM6DSO
+#define IMU_LSM6DSR SoftFusionLSM6DSR
+#define IMU_MPU6050_SF SoftFusionMPU6050
+#define IMU_ICM45686 SoftFusionICM45686
+#define IMU_ICM45605 SoftFusionICM45605
+
+#define IMU_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
+
+#define BOARD_UNKNOWN 0
+#define BOARD_SLIMEVR_LEGACY 1  // More ancient development version of SlimeVR
+#define BOARD_SLIMEVR_DEV 2  // Ancient development version of SlimeVR
 #define BOARD_NODEMCU 3
 #define BOARD_CUSTOM 4
 #define BOARD_WROOM32 5
 #define BOARD_WEMOSD1MINI 6
 #define BOARD_TTGO_TBASE 7
 #define BOARD_ESP01 8
-#define BOARD_SLIMEVR 9
+#define BOARD_SLIMEVR 9  // SlimeVR v1.0 & v1.1
+#define BOARD_LOLIN_C3_MINI 10
+#define BOARD_BEETLE32C3 11
+#define BOARD_ESP32C3DEVKITM1 12
+#define BOARD_OWOTRACK 13  // Only used by owoTrack mobile app
+#define BOARD_WRANGLER 14  // Only used by wrangler app
+#define BOARD_MOCOPI 15  // Used by mocopi/moslime
+#define BOARD_WEMOSWROOM02 16
+#define BOARD_XIAO_ESP32C3 17
+#define BOARD_HARITORA 18  // Used by Haritora/SlimeTora
+#define BOARD_ESP32C6DEVKITC1 19
+#define BOARD_GLOVE_IMU_SLIMEVR_DEV 20  // IMU Glove
+#define BOARD_GESTURES 21  // Used by Gestures
+#define BOARD_SLIMEVR_V1_2 22  // SlimeVR v1.2
+#define BOARD_ESP32S3_SUPERMINI 23
+#define BOARD_GENERIC_NRF 24
+#define BOARD_SLIMEVR_BUTTERFLY_DEV 25
+#define BOARD_SLIMEVR_BUTTERFLY 26
+#define BOARD_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
 
 #define BAT_EXTERNAL 1
 #define BAT_INTERNAL 2
@@ -51,25 +108,79 @@
 
 #define LED_OFF 255
 
-#define POWER_SAVING_LEGACY 0 // No sleeping, but PS enabled
-#define POWER_SAVING_NONE 1 // No sleeping, no PS => for connection issues
-#define POWER_SAVING_MINIMUM 2 // Sleeping and PS => default
-#define POWER_SAVING_MODERATE 3 // Sleeping and better PS => might miss broadcasts, use at own risk
-#define POWER_SAVING_MAXIMUM 4 // Actual CPU sleeping, currently has issues with disconnecting
+#define POWER_SAVING_LEGACY 0  // No sleeping, but PS enabled
+#define POWER_SAVING_NONE 1  // No sleeping, no PS => for connection issues
+#define POWER_SAVING_MINIMUM 2  // Sleeping and PS => default
+#define POWER_SAVING_MODERATE \
+	3  // Sleeping and better PS => might miss broadcasts, use at own risk
+#define POWER_SAVING_MAXIMUM \
+	4  // Actual CPU sleeping, currently has issues with disconnecting
 
-#define DEG_0 0.f
-#define DEG_90 -PI / 2
-#define DEG_180 PI
-#define DEG_270 PI / 2
+// Send rotation/acceleration data as separate frames.
+// PPS: 1470 @ 5+1, 1960 @ 5+3
+#define PACKET_BUNDLING_DISABLED 0
+// Less packets. Pack data per sensor and send asap.
+// Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3
+// setups. PPS: 680 @ 5+1, 740 @ 5+3
+#define PACKET_BUNDLING_LOWLATENCY 1
+// Even less packets, if more than 1 sensor - wait for data from all sensors or until
+// timeout, then send. Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for
+// 5+1, by ~15% for 5+3 setups. PPS: 650 @ 5+1, 650 @ 5+3
+#define PACKET_BUNDLING_BUFFERED 2
+
+// Get radian for a given angle from 0° to 360° (2*PI*r, solve for r given an angle,
+// range -180° to 180°)
+#define DEG_X(deg) ((((deg) < 180.0f ? 0 : 360.0f) - (deg)) * PI / 180.0f)
+
+#define DEG_0 DEG_X(0.0f)
+#define DEG_90 DEG_X(90.0f)
+#define DEG_180 DEG_X(180.0f)
+#define DEG_270 DEG_X(270.0f)
+
+#define CONST_EARTH_GRAVITY 9.80665
+
+#define ACCEL_CALIBRATION_METHOD_SKIP 0
+#define ACCEL_CALIBRATION_METHOD_ROTATION 1
+#define ACCEL_CALIBRATION_METHOD_6POINT 2
+
+#define BMI160_MAG_TYPE_HMC 1
+#define BMI160_MAG_TYPE_QMC 2
+
+#define MCU_UNKNOWN 0
+#define MCU_ESP8266 1
+#define MCU_ESP32 2
+#define MCU_OWOTRACK_ANDROID 3  // Only used by owoTrack mobile app
+#define MCU_WRANGLER 4  // Only used by wrangler app
+#define MCU_OWOTRACK_IOS 5  // Only used by owoTrack mobile app
+#define MCU_ESP32_C3 6
+#define MCU_MOCOPI 7  // Used by mocopi/moslime
+#define MCU_HARITORA 8  // Used by Haritora/SlimeTora
+#define MCU_NRF52 9
+#define MCU_NRF54L 10
+#define MCU_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
+
+enum class SensorDataType : uint8_t {
+	SENSOR_DATATYPE_ROTATION = 0,
+	SENSOR_DATATYPE_FLEX_RESISTANCE,
+	SENSOR_DATATYPE_FLEX_ANGLE
+};
+
+enum class TrackerType : uint8_t {
+	TRACKER_TYPE_SVR_ROTATION = 0,
+	TRACKER_TYPE_SVR_GLOVE_LEFT,
+	TRACKER_TYPE_SVR_GLOVE_RIGHT
+};
 
 #ifdef ESP8266
-  #define HARDWARE_MCU 1
+#define HARDWARE_MCU MCU_ESP8266
 #elif defined(ESP32)
-  #define HARDWARE_MCU 2
+#define HARDWARE_MCU MCU_ESP32
 #else
-  #define HARDWARE_MCU 0
+#define HARDWARE_MCU MCU_UNKNOWN
 #endif
 
 #define CURRENT_CONFIGURATION_VERSION 1
 
-#endif // SLIMEVR_CONSTS_H_
+#include "sensors/sensorposition.h"
+
+#endif  // SLIMEVR_CONSTS_H_

src/credentials.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_CREDENTIALS_H_
 #define SLIMEVR_CREDENTIALS_H_
@@ -30,6 +30,7 @@
 // firmware. We don't have any hardware buttons for the user to confirm
 // OTA update, so this is the best way we have.
 // OTA is allowed only for the first 60 seconds after device startup.
-const char *otaPassword = "SlimeVR-OTA"; // YOUR OTA PASSWORD HERE, LEAVE EMPTY TO DISABLE OTA UPDATES
+const char* otaPassword
+	= "SlimeVR-OTA";  // YOUR OTA PASSWORD HERE, LEAVE EMPTY TO DISABLE OTA UPDATES
 
-#endif // SLIMEVR_CREDENTIALS_H_
+#endif  // SLIMEVR_CREDENTIALS_H_

src/debug.h

@@ -1,52 +1,60 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_DEBUG_H_
 #define SLIMEVR_DEBUG_H_
 #include "consts.h"
 #include "logging/Level.h"
 
-#define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration
-#define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs
-#define BNO_USE_MAGNETOMETER_CORRECTION false // Set to true to enable magnetometer correction for BNO08x IMUs. Only works with USE_6_AXIS set to true.
-#define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO08x currently)
-#define LOAD_BIAS 1 // Loads the bias values from NVS on start (ESP32 Only)
-#define SAVE_BIAS 1 // Periodically saves bias calibration data to NVS (ESP32 Only)
-#define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only)
-#define ENABLE_TAP false // monitor accel for (triple) tap events and send them. Uses more cpu, disable if problems. Server does nothing with value so disabled atm
+#define IMU_MPU6050_RUNTIME_CALIBRATION  // Comment to revert to
+										 // startup/traditional-calibration
+#define BNO_USE_ARVR_STABILIZATION \
+	true  // Set to false to disable stabilization for BNO085+ IMUs
+#define USE_6_AXIS \
+	true  // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently)
+#define LOAD_BIAS true  // Loads the bias values from NVS on start
+#define SAVE_BIAS true  // Periodically saves bias calibration data to NVS
+#define BIAS_DEBUG false  // Printing BIAS Variables to serial (ICM20948 only)
+#define ENABLE_TAP \
+	false  // monitor accel for (triple) tap events and send them. Uses more cpu,
+		   // disable if problems. Server does nothing with value so disabled atm
+#define SEND_ACCELERATION true  // send linear acceleration to the server
 
-//Debug information
+#define EXT_SERIAL_COMMANDS false  // Set to true to enable extra serial debug commands
+
+// Debug information
 
 #define LOG_LEVEL LOG_LEVEL_DEBUG
 
 #if LOG_LEVEL == LOG_LEVEL_TRACE
-    #define DEBUG_SENSOR
-    #define DEBUG_NETWORK
-    #define DEBUG_CONFIGURATION
+#define DEBUG_SENSOR
+#define DEBUG_NETWORK
+#define DEBUG_CONFIGURATION
 #endif
 
-#define serialDebug false // Set to true to get Serial output for debugging
+#define serialDebug false  // Set to true to get Serial output for debugging
 #define serialBaudRate 115200
 #define LED_INTERVAL_STANDBY 10000
+#define PRINT_STATE_EVERY_MS 60000
 
 // Determines how often we sample and send data
 #define samplingRateInMillis 10
@@ -54,16 +62,21 @@
 // Sleeping options
 #define POWERSAVING_MODE POWER_SAVING_LEGACY  // Minimum causes sporadic data pauses
 #if POWERSAVING_MODE >= POWER_SAVING_MINIMUM
-    #define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000)
+#define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000)
 #endif
 
+// Packet bundling/aggregation
+#define PACKET_BUNDLING PACKET_BUNDLING_BUFFERED
+// Extra tunable for PACKET_BUNDLING_BUFFERED (10000us = 10ms timeout, 100hz target)
+#define PACKET_BUNDLING_BUFFER_SIZE_MICROS 10000
+
 // Setup for the Magnetometer
 #define useFullCalibrationMatrix true
 
 // Battery configuration
 #define batterySampleRate 10000
 #define BATTERY_LOW_VOLTAGE_DEEP_SLEEP false
-#define BATTERY_LOW_POWER_VOLTAGE 3.3f // Voltage to raise error
+#define BATTERY_LOW_POWER_VOLTAGE 3.3f  // Voltage to raise error
 
 // Send updates over network only when changes are substantial
 // If "false" updates are sent at the sensor update rate (usually 100 TPS)
@@ -73,11 +86,34 @@
 
 #define I2C_SPEED 400000
 
+#define COMPLIANCE_MODE true
+#define USE_ATTENUATION COMPLIANCE_MODE&& ESP8266
+#define ATTENUATION_N 10.0 / 4.0
+#define ATTENUATION_G 14.0 / 4.0
+#define ATTENUATION_B 40.0 / 4.0
+
 // Send inspection packets over the network to a profiler
 // Not recommended for production
 #define ENABLE_INSPECTION false
 
-#define FIRMWARE_BUILD_NUMBER 11
-#define FIRMWARE_VERSION "0.2.2"
+#define PROTOCOL_VERSION 22
 
-#endif // SLIMEVR_DEBUG_H_
+#ifndef FIRMWARE_VERSION
+#define FIRMWARE_VERSION "UNKNOWN"
+#endif
+
+#ifndef USE_RUNTIME_CALIBRATION
+#define USE_RUNTIME_CALIBRATION true
+#endif
+
+#define DEBUG_MEASURE_SENSOR_TIME_TAKEN false
+
+#ifndef DEBUG_MEASURE_SENSOR_TIME_TAKEN
+#define DEBUG_MEASURE_SENSOR_TIME_TAKEN false
+#endif
+
+#ifndef USE_OTA_TIMEOUT
+#define USE_OTA_TIMEOUT false
+#endif
+
+#endif  // SLIMEVR_DEBUG_H_

src/debugging/TimeTaken.cpp

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

src/debugging/TimeTaken.h

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

src/defines.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 // ================================================
 // See docs for configuration options and examples:
@@ -26,88 +26,53 @@
 // ================================================
 
 // Set parameters of IMU and board used
-#define IMU IMU_BNO085
-#define SECOND_IMU IMU
-#define BOARD BOARD_SLIMEVR
-#define IMU_ROTATION DEG_90
+#ifndef IMU
+#define IMU IMU_AUTO
+#endif
+#ifndef SECOND_IMU
+#define SECOND_IMU IMU_AUTO
+#endif
+#ifndef BOARD
+#define BOARD BOARD_SLIMEVR_V1_2
+#endif
+#ifndef IMU_ROTATION
+#define IMU_ROTATION DEG_270
+#endif
+#ifndef SECOND_IMU_ROTATION
 #define SECOND_IMU_ROTATION DEG_270
+#endif
 
+#ifndef PRIMARY_IMU_OPTIONAL
+#define PRIMARY_IMU_OPTIONAL false
+#endif
+#ifndef SECONDARY_IMU_OPTIONAL
+#define SECONDARY_IMU_OPTIONAL true
+#endif
+
+// Set I2C address here or directly in IMU_DESC_ENTRY for each IMU used
+// If not set, default address is used based on the IMU and Sensor ID
+// #define PRIMARY_IMU_ADDRESS_ONE 0x4a
+// #define SECONDARY_IMU_ADDRESS_TWO 0x4b
+
+#ifndef BATTERY_MONITOR
 // Battery monitoring options (comment to disable):
-//   BAT_EXTERNAL for ADC pin, 
-//   BAT_INTERNAL for internal - can detect only low battery, 
+//   BAT_EXTERNAL for ADC pin,
+//   BAT_INTERNAL for internal - can detect only low battery,
 //   BAT_MCP3021 for external ADC connected over I2C
 #define BATTERY_MONITOR BAT_EXTERNAL
-
-// BAT_EXTERNAL definition
-// D1 Mini boards with ESP8266 have internal resistors. For these boards you only have to adjust BATTERY_SHIELD_RESISTANCE.
-// For other boards you can now adjust the other resistor values.
-// The diagram looks like this:
-//   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2] ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
-#define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in external resistor value in kOhm
-// #define BATTERY_SHIELD_R1 100 // Board voltage divider resistor Ain to GND in kOhm
-// #define BATTERY_SHIELD_R2 220 // Board voltage divider resistor Ain to INPUT_BOARD in kOhm
-
-// LED configuration:
-// Configuration Priority 1 = Highest:
-// 1. LED_PIN
-// 2. LED_BUILTIN
-//
-//   LED_PIN
-//     - Number or Symbol (D1,..) of the Output
-//     - To turn off the LED, set LED_PIN to LED_OFF
-//   LED_INVERTED 
-//     - false for output 3.3V on high
-//     - true for pull down to GND on high
-
-// Board-specific configurations
-#if BOARD == BOARD_SLIMEVR
-  #define PIN_IMU_SDA 14
-  #define PIN_IMU_SCL 12
-  #define PIN_IMU_INT 16
-  #define PIN_IMU_INT_2 13
-  #define PIN_BATTERY_LEVEL 17
-  #define LED_PIN 2
-  #define LED_INVERTED true
-#elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV
-  #define PIN_IMU_SDA 4
-  #define PIN_IMU_SCL 5
-  #define PIN_IMU_INT 10
-  #define PIN_IMU_INT_2 13
-  #define PIN_BATTERY_LEVEL 17
-  #define LED_PIN 2
-  #define LED_INVERTED true
-#elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI
-  #define PIN_IMU_SDA D2
-  #define PIN_IMU_SCL D1
-  #define PIN_IMU_INT D5
-  #define PIN_IMU_INT_2 D6
-  #define PIN_BATTERY_LEVEL A0
-//  #define LED_PIN 2
-//  #define LED_INVERTED true
-#elif BOARD == BOARD_ESP01
-  #define PIN_IMU_SDA 2
-  #define PIN_IMU_SCL 0
-  #define PIN_IMU_INT 255
-  #define PIN_IMU_INT_2 255
-  #define PIN_BATTERY_LEVEL 255
-  #define LED_PIN LED_OFF
-  #define LED_INVERTED false
-#elif BOARD == BOARD_TTGO_TBASE
-  #define PIN_IMU_SDA 5
-  #define PIN_IMU_SCL 4
-  #define PIN_IMU_INT 14
-  #define PIN_IMU_INT_2 13
-  #define PIN_BATTERY_LEVEL A0
-//  #define LED_PIN 2
-//  #define LED_INVERTED false
-#elif BOARD == BOARD_CUSTOM
-  // Define pins by the examples above
-#elif BOARD == BOARD_WROOM32
-  #define PIN_IMU_SDA 21
-  #define PIN_IMU_SCL 22
-  #define PIN_IMU_INT 23
-  #define PIN_IMU_INT_2 25
-  #define PIN_BATTERY_LEVEL 36
-//  #define LED_PIN 2
-//  #define LED_INVERTED false
 #endif
+
+// --- OVERRIDES FOR DEFAULT PINS
+
+// #define PIN_IMU_SDA 14
+// #define PIN_IMU_SCL 12
+// #define PIN_IMU_INT 16
+// #define PIN_IMU_INT_2 13
+// #define PIN_BATTERY_LEVEL 17
+// #define LED_PIN 2
+// #define LED_INVERTED true
+// #define BATTERY_SHIELD_RESISTANCE 0
+// #define BATTERY_SHIELD_R1 10
+// #define BATTERY_SHIELD_R2 40.2
+
+// ------------------------------

src/globals.h

@@ -1,33 +1,37 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
-#ifndef SLIMEVR_GLOBALS_H_
-#define SLIMEVR_GLOBALS_H_
+#pragma once
 
 #include <Arduino.h>
+
 #include "consts.h"
 #include "debug.h"
 #include "defines.h"
 
+// clang-format off
+#include "boards/boards_default.h"
+// clang-format on
+
 #ifndef SECOND_IMU
 #define SECOND_IMU IMU
 #endif
@@ -40,37 +44,35 @@
 #define BATTERY_MONITOR BAT_INTERNAL
 #endif
 
-// If LED_PIN is not defined in "defines.h" take the default pin from "pins_arduino.h" framework.
-// If there is no pin defined for the board, use LED_PIN 255 and disable LED
-#if defined(LED_PIN)
-    // LED_PIN is defined
-    #if (LED_PIN < 0) || (LED_PIN >= LED_OFF)
-        #define ENABLE_LEDS false    
-    #else
-        #define ENABLE_LEDS true
-    #endif
-#else
-    // LED_PIN is not defined
-    #if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0)
-        #define LED_PIN LED_BUILTIN
-        #define ENABLE_LEDS true
-    #else
-        #define LED_PIN LED_OFF
-        #define ENABLE_LEDS false
-    #endif
+#ifndef SENSOR_INFO_LIST
+#define SENSOR_INFO_LIST
 #endif
 
-#if !defined(LED_INVERTED)
-    // default is inverted for SlimeVR / ESP-12E
-    #define LED_INVERTED true
+// Experimental features
+#ifndef EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION
+#define EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION true
 #endif
 
-#if LED_INVERTED
-#define LED__ON LOW
-#define LED__OFF HIGH
-#else
-#define LED__ON HIGH
-#define LED__OFF LOW
+#ifndef IMU_USE_EXTERNAL_CLOCK
+#define IMU_USE_EXTERNAL_CLOCK true  // Use external clock for IMU (ICM-45686 only)
 #endif
 
-#endif // SLIMEVR_GLOBALS_H_
+#ifndef VENDOR_NAME
+#define VENDOR_NAME "Unknown"
+#endif
+
+#ifndef VENDOR_URL
+#define VENDOR_URL ""
+#endif
+
+#ifndef PRODUCT_NAME
+#define PRODUCT_NAME "DIY SlimeVR Tracker"
+#endif
+
+#ifndef UPDATE_ADDRESS
+#define UPDATE_ADDRESS ""
+#endif
+
+#ifndef UPDATE_NAME
+#define UPDATE_NAME ""
+#endif

src/logging/Level.cpp

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

src/logging/Level.h

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

src/logging/Logger.cpp

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

src/logging/Logger.h

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

src/main.cpp

@@ -1,123 +1,196 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 
-#include "Wire.h"
-#include "ota.h"
-#include "sensors/SensorManager.h"
-#include "configuration/Configuration.h"
-#include "network/network.h"
-#include "globals.h"
-#include "credentials.h"
 #include <i2cscan.h>
-#include "serial/serialcommands.h"
-#include "LEDManager.h"
-#include "status/StatusManager.h"
-#include "batterymonitor.h"
-#include "logging/Logger.h"
 
+#include "GlobalVars.h"
+#include "Wire.h"
+#include "batterymonitor.h"
+#include "credentials.h"
+#include "debugging/TimeTaken.h"
+#include "globals.h"
+#include "logging/Logger.h"
+#include "ota.h"
+#include "serial/serialcommands.h"
+#include "status/TPSCounter.h"
+
+Timer<> globalTimer;
 SlimeVR::Logging::Logger logger("SlimeVR");
 SlimeVR::Sensors::SensorManager sensorManager;
-SlimeVR::LEDManager ledManager(LED_PIN);
+SlimeVR::LEDManager ledManager;
 SlimeVR::Status::StatusManager statusManager;
 SlimeVR::Configuration::Configuration configuration;
+SlimeVR::Network::Manager networkManager;
+SlimeVR::Network::Connection networkConnection;
+SlimeVR::WiFiNetwork wifiNetwork;
+SlimeVR::WifiProvisioning wifiProvisioning;
+
+#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
+SlimeVR::Debugging::TimeTakenMeasurer sensorMeasurer{"Sensors"};
+#endif
 
 int sensorToCalibrate = -1;
 bool blinking = false;
 unsigned long blinkStart = 0;
 unsigned long loopTime = 0;
+unsigned long lastStatePrint = 0;
 bool secondImuActive = false;
 BatteryMonitor battery;
+TPSCounter tpsCounter;
 
-void setup()
-{
-    Serial.begin(serialBaudRate);
-    Serial.println();
-    Serial.println();
-    Serial.println();
+void setup() {
+	Serial.begin(serialBaudRate);
+	globalTimer = timer_create_default();
 
-    logger.info("SlimeVR v" FIRMWARE_VERSION " starting up...");
+	Serial.println();
+	Serial.println();
+	Serial.println();
 
-    //wifi_set_sleep_type(NONE_SLEEP_T);
+	logger.info("SlimeVR v" FIRMWARE_VERSION " starting up...");
 
-    statusManager.setStatus(SlimeVR::Status::LOADING, true);
+	char vendorBuffer[512];
+	size_t writtenLength;
 
-    ledManager.setup();
-    configuration.setup();
+	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
+		);
+	}
 
-    SerialCommands::setUp();
+	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);
 
-#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250
-    I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down
-    // Do it only for MPU, cause reaction of BNO to this is not investigated yet
+	statusManager.setStatus(SlimeVR::Status::LOADING, true);
+
+	ledManager.setup();
+	configuration.setup();
+
+	SerialCommands::setUp();
+	// Make sure the bus isn't stuck when resetting ESP without powering it down
+	// Fixes I2C issues for certain IMUs. Previously this feature was enabled for
+	// selected IMUs, now it's enabled for all. If some IMU turned out to be broken by
+	// this, check needs to be re-added.
+	auto clearResult = I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL);
+	if (clearResult != 0) {
+		logger.warn("Can't clear I2C bus, error %d", clearResult);
+	}
+
+	// join I2C bus
+
+#ifdef ESP32
+	// For some unknown reason the I2C seem to be open on ESP32-C3 by default. Let's
+	// just close it before opening it again. (The ESP32-C3 only has 1 I2C.)
+	Wire.end();
 #endif
-    // join I2C bus
-    Wire.begin(PIN_IMU_SDA, PIN_IMU_SCL);
+
+	// using `static_cast` here seems to be better, because there are 2 similar function
+	// signatures
+	Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+
 #ifdef ESP8266
-    Wire.setClockStretchLimit(150000L); // Default stretch limit 150mS
+	Wire.setClockStretchLimit(150000L);  // Default stretch limit 150mS
 #endif
-    Wire.setClock(I2C_SPEED);
+#ifdef ESP32  // Counterpart on ESP32 to ClockStretchLimit
+	Wire.setTimeOut(150);
+#endif
+	Wire.setClock(I2C_SPEED);
 
-    // Wait for IMU to boot
-    delay(500);
-    
-    sensorManager.setup();
-    
-    Network::setUp();
-    OTA::otaSetup(otaPassword);
-    battery.Setup();
+	// Wait for IMU to boot
+	delay(500);
 
-    statusManager.setStatus(SlimeVR::Status::LOADING, false);
+	sensorManager.setup();
 
-    sensorManager.postSetup();
+	networkManager.setup();
+	OTA::otaSetup(otaPassword);
+	battery.Setup();
 
-    loopTime = micros();
+	statusManager.setStatus(SlimeVR::Status::LOADING, false);
+
+	sensorManager.postSetup();
+
+	loopTime = micros();
+	tpsCounter.reset();
 }
 
-void loop()
-{
-    SerialCommands::update();
-    OTA::otaUpdate();
-    Network::update(sensorManager.getFirst(), sensorManager.getSecond());
-    sensorManager.update();
-    battery.Loop();
-    ledManager.update();
+void loop() {
+	tpsCounter.update();
+	globalTimer.tick();
+	SerialCommands::update();
+	OTA::otaUpdate();
+	networkManager.update();
 
+#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
+	sensorMeasurer.before();
+#endif
+	sensorManager.update();
+#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
+	sensorMeasurer.after();
+#endif
+
+	battery.Loop();
+	ledManager.update();
+	I2CSCAN::update();
 #ifdef TARGET_LOOPTIME_MICROS
-    long elapsed = (micros() - loopTime);
-    if (elapsed < TARGET_LOOPTIME_MICROS)
-    {
-        long sleepus = TARGET_LOOPTIME_MICROS - elapsed - 100;//µs to sleep
-        long sleepms = sleepus / 1000;//ms to sleep
-        if(sleepms > 0) // if >= 1 ms
-        {
-            delay(sleepms); // sleep ms = save power
-            sleepus -= sleepms * 1000;
-        }
-        if (sleepus > 100)
-        {
-            delayMicroseconds(sleepus);
-        }
-    }
-    loopTime = micros();
+	long elapsed = (micros() - loopTime);
+	if (elapsed < TARGET_LOOPTIME_MICROS) {
+		long sleepus = TARGET_LOOPTIME_MICROS - elapsed - 100;  // µs to sleep
+		long sleepms = sleepus / 1000;  // ms to sleep
+		if (sleepms > 0)  // if >= 1 ms
+		{
+			delay(sleepms);  // sleep ms = save power
+			sleepus -= sleepms * 1000;
+		}
+		if (sleepus > 100) {
+			delayMicroseconds(sleepus);
+		}
+	}
+	loopTime = micros();
+#endif
+#if defined(PRINT_STATE_EVERY_MS) && PRINT_STATE_EVERY_MS > 0
+	unsigned long now = millis();
+	if (lastStatePrint + PRINT_STATE_EVERY_MS < now) {
+		lastStatePrint = now;
+		SerialCommands::printState();
+	}
 #endif
 }

src/motionprocessing/GyroTemperatureCalibrator.cpp

@@ -0,0 +1,244 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "GyroTemperatureCalibrator.h"
+
+#include "GlobalVars.h"
+
+void GyroTemperatureCalibrator::resetCurrentTemperatureState() {
+	if (!state.numSamples) {
+		return;
+	}
+	state.numSamples = 0;
+	state.tSum = 0;
+	state.xSum = 0;
+	state.ySum = 0;
+	state.zSum = 0;
+}
+
+// must be called for every raw gyro sample
+void GyroTemperatureCalibrator::updateGyroTemperatureCalibration(
+	const float temperature,
+	const bool restDetected,
+	int16_t x,
+	int16_t y,
+	int16_t z
+) {
+	if (!restDetected) {
+		return resetCurrentTemperatureState();
+	}
+
+	if (temperature < TEMP_CALIBRATION_MIN && !calibrationRunning) {
+		calibrationRunning = true;
+		configSaved = false;
+		config.reset();
+	}
+	if (temperature > TEMP_CALIBRATION_MAX && calibrationRunning) {
+		auto coeffs = poly.computeCoefficients();
+		for (uint32_t i = 0; i < poly.numCoefficients; i++) {
+			config.cx[i] = coeffs[0][i];
+			config.cy[i] = coeffs[1][i];
+			config.cz[i] = coeffs[2][i];
+		}
+		config.hasCoeffs = true;
+		bst = 0.0f;
+		bsx = 0;
+		bsy = 0;
+		bsz = 0;
+		bn = 0;
+		lastTemp = 0;
+		calibrationRunning = false;
+		if (!configSaveFailed && !configSaved) {
+			saveConfig();
+		}
+	}
+	if (calibrationRunning) {
+		if (fabs(lastTemp - temperature) > 0.03f) {
+			const double avgt = (double)bst / bn;
+			const double avgValues[] = {
+				(double)bsx / bn,
+				(double)bsy / bn,
+				(double)bsz / bn,
+			};
+			if (bn > 0) {
+				poly.update(avgt, avgValues);
+			}
+			bst = 0.0f;
+			bsx = 0;
+			bsy = 0;
+			bsz = 0;
+			bn = 0;
+			lastTemp = temperature;
+		} else {
+			bst += temperature;
+			bsx += x;
+			bsy += y;
+			bsz += z;
+			bn++;
+		}
+	}
+
+	const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(temperature);
+
+	if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) {
+		return;
+	}
+
+	bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
+	if (currentTempAlreadyCalibrated) {
+		return;
+	}
+
+	if (state.temperatureCurrentIdx != idx) {
+		state.temperatureCurrentIdx = idx;
+		resetCurrentTemperatureState();
+	}
+
+	float temperatureStepBoundsMin
+		= TEMP_CALIBRATION_IDX_TO_TEMP(idx) - TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP;
+	float temperatureStepBoundsMax
+		= TEMP_CALIBRATION_IDX_TO_TEMP(idx) + TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP;
+	bool isTemperatureOutOfDeviationRange = temperature < temperatureStepBoundsMin
+										 || temperature > temperatureStepBoundsMax;
+	if (isTemperatureOutOfDeviationRange) {
+		return resetCurrentTemperatureState();
+	}
+
+	state.numSamples++;
+	state.tSum += temperature;
+	state.xSum += x;
+	state.ySum += y;
+	state.zSum += z;
+	if (state.numSamples > samplesPerStep) {
+		bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
+		if (!currentTempAlreadyCalibrated) {
+			config.samplesTotal++;
+		}
+		config.samples[idx].t = state.tSum / state.numSamples;
+		config.samples[idx].x = ((double)state.xSum / state.numSamples);
+		config.samples[idx].y = ((double)state.ySum / state.numSamples);
+		config.samples[idx].z = ((double)state.zSum / state.numSamples);
+
+		config.minTemperatureRange
+			= min(config.samples[idx].t, config.minTemperatureRange);
+		config.maxTemperatureRange
+			= max(config.samples[idx].t, config.maxTemperatureRange);
+		config.minCalibratedIdx
+			= TEMP_CALIBRATION_TEMP_TO_IDX(config.minTemperatureRange);
+		config.maxCalibratedIdx
+			= TEMP_CALIBRATION_TEMP_TO_IDX(config.maxTemperatureRange);
+		resetCurrentTemperatureState();
+	}
+}
+
+bool GyroTemperatureCalibrator::approximateOffset(
+	const float temperature,
+	float GOxyz[3]
+) {
+	if (!config.hasData()) {
+		return false;
+	}
+	if (config.hasCoeffs) {
+		if (lastApproximatedTemperature != 0.0f
+			&& temperature == lastApproximatedTemperature) {
+			GOxyz[0] = lastApproximatedOffsets[0];
+			GOxyz[1] = lastApproximatedOffsets[1];
+			GOxyz[2] = lastApproximatedOffsets[2];
+		} else {
+			float offsets[3] = {config.cx[3], config.cy[3], config.cz[3]};
+			for (int32_t i = 2; i >= 0; i--) {
+				offsets[0] = offsets[0] * temperature + config.cx[i];
+				offsets[1] = offsets[1] * temperature + config.cy[i];
+				offsets[2] = offsets[2] * temperature + config.cz[i];
+			}
+			lastApproximatedTemperature = temperature;
+			lastApproximatedOffsets[0] = GOxyz[0] = offsets[0];
+			lastApproximatedOffsets[1] = GOxyz[1] = offsets[1];
+			lastApproximatedOffsets[2] = GOxyz[2] = offsets[2];
+		}
+		return true;
+	}
+
+	const float constrainedTemperature = constrain(
+		temperature,
+		config.minTemperatureRange,
+		config.maxTemperatureRange
+	);
+
+	const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature);
+
+	if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) {
+		return false;
+	}
+
+	bool isCurrentTempCalibrated = config.samples[idx].t != 0.0f;
+	if (isCurrentTempCalibrated) {
+		GOxyz[0] = config.samples[idx].x;
+		GOxyz[1] = config.samples[idx].y;
+		GOxyz[2] = config.samples[idx].z;
+		return true;
+	}
+
+	return false;
+}
+
+bool GyroTemperatureCalibrator::loadConfig(float newSensitivity) {
+	bool ok = configuration.loadTemperatureCalibration(sensorId, config);
+	if (ok) {
+		config.rescaleSamples(newSensitivity);
+		if (config.fullyCalibrated()) {
+			configSaved = true;
+		}
+	} else {
+		m_Logger.warn(
+			"No temperature calibration data found for sensor %d, ignoring...",
+			sensorId
+		);
+		// m_Logger.info("Temperature calibration is advised");
+		m_Logger.info(
+			"Temperature calibration is a work-in-progress feature; any changes to its "
+			"parameters or updates will render the saved temperature curve invalid and "
+			"unloadable."
+		);
+	}
+	return configSaved;
+}
+
+bool GyroTemperatureCalibrator::saveConfig() {
+	if (configuration.saveTemperatureCalibration(sensorId, config)) {
+		m_Logger.info(
+			"Saved temperature calibration config (%0.1f%%) for sensorId:%i",
+			config.getCalibrationDonePercent(),
+			sensorId
+		);
+		if (config.fullyCalibrated()) {
+			configSaved = true;
+		} else {
+			m_Logger.info("Calibration will resume from this checkpoint after reboot");
+		}
+	} else {
+		configSaveFailed = true;
+		m_Logger.error("Something went wrong");
+	}
+	return configSaved;
+}

src/motionprocessing/GyroTemperatureCalibrator.h

@@ -0,0 +1,239 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#ifndef GYRO_TEMPERATURE_CALIBRATOR_H
+#define GYRO_TEMPERATURE_CALIBRATOR_H
+
+#include <Arduino.h>
+#include <stdint.h>
+
+#include "../configuration/SensorConfig.h"
+#include "../logging/Logger.h"
+#include "OnlinePolyfit.h"
+#include "debug.h"
+
+// Degrees C
+// default: 15.0f
+#define TEMP_CALIBRATION_MIN 15.0f
+
+// Degrees C
+// default: 45.0f
+#define TEMP_CALIBRATION_MAX 45.0f
+
+// Snap calibration to every 1/2 of degree: 20.00, 20.50, 21.00, etc
+// default: 0.5f
+#define TEMP_CALIBRATION_STEP 0.5f
+
+// Record debug samples if current temperature is off by no more than this value;
+// if snapping point is 20.00 - samples will be recorded in range of 19.80 - 20.20
+// default: 0.2f
+#define TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP 0.2f
+
+// How long to average gyro samples for before saving a data point
+// default: 0.2f
+#define TEMP_CALIBRATION_SECONDS_PER_STEP 0.2f
+
+#if IMU == IMU_ICM20948
+// 16 bit 333 lsb/K, ~0.00508 degrees per bit
+// already calibrated by DMP?
+#elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050
+// 16 bit 340 lsb/K, ~0.00518 degrees per bit
+#elif IMU == IMU_MPU9250
+// 16 bit 333 lsb/K, ~0.00508 degrees per bit
+#elif IMU == IMU_BMI160
+// 16 bit 128 lsb/K, ~0.00195 degrees per bit
+#endif
+
+constexpr uint16_t TEMP_CALIBRATION_BUFFER_SIZE
+	= (uint16_t)((TEMP_CALIBRATION_MAX - TEMP_CALIBRATION_MIN)
+				 * (1 / TEMP_CALIBRATION_STEP));
+
+#define TEMP_CALIBRATION_TEMP_TO_IDX(temperature)                                  \
+	(uint16_t)(                                                                    \
+		(temperature + TEMP_CALIBRATION_STEP / 2.0f) * (1 / TEMP_CALIBRATION_STEP) \
+		- TEMP_CALIBRATION_MIN * (1 / TEMP_CALIBRATION_STEP)                       \
+	)
+
+#define TEMP_CALIBRATION_IDX_TO_TEMP(idx) \
+	(float)(((float)idx / (1.0f / TEMP_CALIBRATION_STEP)) + TEMP_CALIBRATION_MIN)
+
+struct GyroTemperatureCalibrationState {
+	uint16_t temperatureCurrentIdx;
+	uint32_t numSamples;
+	float tSum;
+	int32_t xSum;
+	int32_t ySum;
+	int32_t zSum;
+
+	GyroTemperatureCalibrationState()
+		: temperatureCurrentIdx(-1)
+		, numSamples(0)
+		, tSum(0.0f)
+		, xSum(0)
+		, ySum(0)
+		, zSum(0){};
+};
+
+struct GyroTemperatureOffsetSample {
+	float t;
+	float x;
+	float y;
+	float z;
+
+	GyroTemperatureOffsetSample()
+		: t(0.0f)
+		, x(0)
+		, y(0)
+		, z(0) {}
+};
+
+struct GyroTemperatureCalibrationConfig {
+	SlimeVR::Configuration::SensorConfigType type;
+
+	float sensitivityLSB;
+	float minTemperatureRange;
+	float maxTemperatureRange;
+	uint16_t minCalibratedIdx = 0;
+	uint16_t maxCalibratedIdx = 0;
+	GyroTemperatureOffsetSample samples[TEMP_CALIBRATION_BUFFER_SIZE];
+	uint16_t samplesTotal = 0;
+	float cx[4] = {0.0};
+	float cy[4] = {0.0};
+	float cz[4] = {0.0};
+	bool hasCoeffs = false;
+
+	GyroTemperatureCalibrationConfig(
+		SlimeVR::Configuration::SensorConfigType _type,
+		float _sensitivityLSB
+	)
+		: type(_type)
+		, sensitivityLSB(_sensitivityLSB)
+		, minTemperatureRange(1000)
+		, maxTemperatureRange(-1000) {}
+
+	bool hasData() { return minTemperatureRange != 1000; }
+
+	bool fullyCalibrated() {
+		return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs;
+	}
+
+	float getCalibrationDonePercent() {
+		return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f;
+	}
+
+	void rescaleSamples(float newSensitivityLSB) {
+		if (sensitivityLSB == newSensitivityLSB) {
+			return;
+		}
+		float mul = newSensitivityLSB / sensitivityLSB;
+		for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) {
+			if (samples[i].t == 0) {
+				continue;
+			}
+			samples[i].x *= mul;
+			samples[i].y *= mul;
+			samples[i].z *= mul;
+		}
+		sensitivityLSB = newSensitivityLSB;
+	}
+
+	void reset() {
+		minTemperatureRange = 1000;
+		maxTemperatureRange = -1000;
+		samplesTotal = 0;
+		for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) {
+			samples[i].t = 0;
+			samples[i].x = 0;
+			samples[i].y = 0;
+			samples[i].z = 0;
+		}
+		hasCoeffs = false;
+	}
+};
+
+class GyroTemperatureCalibrator {
+public:
+	uint8_t sensorId;
+	GyroTemperatureCalibrationConfig config;
+
+	// set when config is fully calibrated is saved OR on startup when loaded config is
+	// fully calibrated; left unset when sending saving command over serial so it can
+	// continue calibration and autosave later
+	bool configSaved = false;
+	bool configSaveFailed = false;
+
+	GyroTemperatureCalibrator(
+		SlimeVR::Configuration::SensorConfigType _configType,
+		uint8_t _sensorId,
+		float sensitivity,
+		uint32_t _samplesPerStep
+	)
+		: sensorId(_sensorId)
+		, config(_configType, sensitivity)
+		, samplesPerStep(_samplesPerStep)
+		, m_Logger(SlimeVR::Logging::Logger("GyroTemperatureCalibrator")) {
+		char buf[4];
+		sprintf(buf, "%u", _sensorId);
+		m_Logger.setTag(buf);
+	}
+
+	void updateGyroTemperatureCalibration(
+		const float temperature,
+		const bool restDetected,
+		int16_t x,
+		int16_t y,
+		int16_t z
+	);
+	bool approximateOffset(const float temperature, float GOxyz[3]);
+	bool loadConfig(float newSensitivity);
+	bool saveConfig();
+
+	void reset() {
+		config.reset();
+		configSaved = false;
+		configSaveFailed = false;
+	}
+
+	bool isCalibrating() { return calibrationRunning; }
+
+private:
+	GyroTemperatureCalibrationState state;
+	uint32_t samplesPerStep;
+	SlimeVR::Logging::Logger m_Logger;
+
+	float lastApproximatedTemperature = 0.0f;
+	float lastApproximatedOffsets[3];
+
+	bool calibrationRunning = false;
+	OnlineVectorPolyfit<3, 3, (uint64_t)1e9> poly;
+	float bst = 0.0f;
+	int32_t bsx = 0;
+	int32_t bsy = 0;
+	int32_t bsz = 0;
+	int32_t bn = 0;
+	float lastTemp = 0;
+
+	void resetCurrentTemperatureState();
+};
+
+#endif

src/motionprocessing/OnlinePolyfit.h

@@ -0,0 +1,130 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#include <Arduino.h>
+
+#ifndef ONLINE_POLYFIT_H
+#define ONLINE_POLYFIT_H
+
+template <uint32_t degree, uint32_t dimensions, uint64_t forgettingFactorNumSamples>
+class OnlineVectorPolyfit {
+public:
+	constexpr static int32_t numDimensions = dimensions;
+	constexpr static int32_t numCoefficients = degree + 1;
+	constexpr static double forgettingFactor
+		= std::exp(-1.0 / forgettingFactorNumSamples);
+
+	OnlineVectorPolyfit() { reset(); }
+
+	void reset() {
+		std::fill(Rb[0], Rb[0] + rows * cols, 0.0);
+		std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f);
+	}
+
+	// Recursive least squares update using QR decomposition by Givens transformations
+	void update(double xValue, const double yValues[numDimensions]) {
+		double xin[cols];
+		xin[0] = 1;
+		for (int32_t i = 1; i < cols - numDimensions; i++) {
+			xin[i] = xin[i - 1] * xValue;
+		}
+		for (int32_t i = 0; i < numDimensions; i++) {
+			xin[cols - numDimensions + i] = yValues[i];
+		}
+
+		// degree = 3, dimensions = 3, yValues = [x, y, z]
+		// B I I I Ix Iy Iz       R R R R bx by bz
+		// . B I I Ix Iy Iz  ===  . R R R bx by bz
+		// . . B I Ix Iy Iz  ===  . . R R bx by bz
+		// . . . B Ix Iy Iz       . . . R bx by bz
+
+		// https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf
+
+		for (int32_t y = 0; y < rows; y++) {
+			double c = 1, s = 0;
+			if (xin[y] != 0.0) {
+				Rb[y][y] *= forgettingFactor;
+				const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]);
+				c = Rb[y][y] * (1.0 / norm);
+				s = xin[y] * (1.0 / norm);
+				Rb[y][y] = norm;
+			}
+			for (int32_t x = y + 1; x < cols; x++) {
+				Rb[y][x] *= forgettingFactor;
+				const double xout = (c * xin[x] - s * Rb[y][x]);
+				Rb[y][x] = (s * xin[x] + c * Rb[y][x]);
+				xin[x] = xout;
+			}
+		}
+	}
+
+	// Back solves upper triangular system
+	// Returns float[numDimensions][numCoefficients] coefficients from lowest to highest
+	// power
+	const auto computeCoefficients() {
+		// https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution
+		for (int32_t d = 0; d < numDimensions; d++) {
+			for (int32_t y = rows - 1; y >= 0; y--) {
+				const int32_t bColumn = cols - numDimensions + d;
+				coeffs[d][y] = Rb[y][bColumn];
+				if (Rb[y][y] == 0.0) {
+					continue;
+				}
+				for (int32_t x = y + 1; x < rows; x++) {
+					coeffs[d][y] -= coeffs[d][x] * Rb[y][x];
+				}
+				coeffs[d][y] /= Rb[y][y];
+			}
+		}
+		return coeffs;
+	}
+
+	float predict(int32_t d, float x) {
+		if (d >= numDimensions) {
+			return 0.0;
+		}
+		// https://en.wikipedia.org/wiki/Horner%27s_method
+		float y = coeffs[d][numCoefficients - 1];
+		for (int32_t i = numCoefficients - 2; i >= 0; i--) {
+			y = y * x + coeffs[d][i];
+		}
+		return y;
+	}
+
+	std::pair<float, float> tangentAt(float x) {
+		float intercept = coeffs[0];
+		float slope = coeffs[1];
+		for (uint32_t i = 2; i < degree + 1; i++) {
+			intercept -= coeffs[i] * (i - 1) * pow(x, i);
+			slope += coeffs[i] * i * pow(x, i - 1);
+		}
+		return std::make_pair(slope, intercept);
+	}
+
+private:
+	constexpr static int32_t rows = numCoefficients;
+	constexpr static int32_t cols = numCoefficients + 3;
+	double Rb[rows][cols];
+	float coeffs[numDimensions][rows];
+};
+
+#endif

src/motionprocessing/RestDetection.h

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

src/motionprocessing/types.h

@@ -0,0 +1,11 @@
+#ifndef MOTIONPROCESSING_TYPES_H
+#define MOTIONPROCESSING_TYPES_H
+
+#if SENSORS_DOUBLE_PRECISION
+typedef double sensor_real_t;
+#else
+typedef float sensor_real_t;
+#define VQF_SINGLE_PRECISION
+#endif
+
+#endif

src/network/connection.cpp

@@ -0,0 +1,795 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2023 SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	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 "connection.h"
+
+#include <string_view>
+
+#include "GlobalVars.h"
+#include "logging/Logger.h"
+#include "packets.h"
+
+#define TIMEOUT 3000UL
+
+template <typename T>
+uint8_t* convert_to_chars(T src, uint8_t* target) {
+	auto* rawBytes = reinterpret_cast<uint8_t*>(&src);
+	std::memcpy(target, rawBytes, sizeof(T));
+	std::reverse(target, target + sizeof(T));
+	return target;
+}
+
+namespace SlimeVR::Network {
+
+bool Connection::beginPacket() {
+	if (m_IsBundle) {
+		m_BundlePacketPosition = 0;
+		return true;
+	}
+
+	int r = m_UDP.beginPacket(m_ServerHost, m_ServerPort);
+	if (r == 0) {
+		// This *technically* should *never* fail, since the underlying UDP
+		// library just returns 1.
+
+		m_Logger.warn("UDP beginPacket() failed");
+	}
+
+	return r > 0;
+}
+
+bool Connection::endPacket() {
+	if (m_IsBundle) {
+		uint32_t innerPacketSize = m_BundlePacketPosition;
+
+		MUST_TRANSFER_BOOL((innerPacketSize > 0));
+
+		m_IsBundle = false;
+
+		if (m_BundlePacketInnerCount == 0) {
+			sendPacketType(SendPacketType::Bundle);
+			sendPacketNumber();
+		}
+		sendShort(innerPacketSize);
+		sendBytes(m_Packet, innerPacketSize);
+
+		m_BundlePacketInnerCount++;
+		m_IsBundle = true;
+		return true;
+	}
+
+	int r = m_UDP.endPacket();
+	if (r == 0) {
+		// This is usually just `ERR_ABRT` but the UDP client doesn't expose
+		// the full error code to us, so we just have to live with it.
+
+		// m_Logger.warn("UDP endPacket() failed");
+	}
+
+	return r > 0;
+}
+
+bool Connection::beginBundle() {
+	MUST_TRANSFER_BOOL(m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT));
+	MUST_TRANSFER_BOOL(m_Connected);
+	MUST_TRANSFER_BOOL(!m_IsBundle);
+	MUST_TRANSFER_BOOL(beginPacket());
+
+	m_IsBundle = true;
+	m_BundlePacketInnerCount = 0;
+	return true;
+}
+
+bool Connection::endBundle() {
+	MUST_TRANSFER_BOOL(m_IsBundle);
+
+	m_IsBundle = false;
+
+	MUST_TRANSFER_BOOL((m_BundlePacketInnerCount > 0));
+
+	return endPacket();
+}
+
+size_t Connection::write(const uint8_t* buffer, size_t size) {
+	if (m_IsBundle) {
+		if (m_BundlePacketPosition + size > sizeof(m_Packet)) {
+			return 0;
+		}
+		memcpy(m_Packet + m_BundlePacketPosition, buffer, size);
+		m_BundlePacketPosition += size;
+		return size;
+	}
+	return m_UDP.write(buffer, size);
+}
+
+size_t Connection::write(uint8_t byte) { return write(&byte, 1); }
+
+bool Connection::sendFloat(float f) {
+	convert_to_chars(f, m_Buf);
+
+	return write(m_Buf, sizeof(f)) != 0;
+}
+
+bool Connection::sendByte(uint8_t c) { return write(&c, 1) != 0; }
+
+bool Connection::sendShort(uint16_t i) {
+	convert_to_chars(i, m_Buf);
+
+	return write(m_Buf, sizeof(i)) != 0;
+}
+
+bool Connection::sendInt(uint32_t i) {
+	convert_to_chars(i, m_Buf);
+
+	return write(m_Buf, sizeof(i)) != 0;
+}
+
+bool Connection::sendLong(uint64_t l) {
+	convert_to_chars(l, m_Buf);
+
+	return write(m_Buf, sizeof(l)) != 0;
+}
+
+bool Connection::sendBytes(const uint8_t* c, size_t length) {
+	return write(c, length) != 0;
+}
+
+bool Connection::sendPacketNumber() {
+	if (m_IsBundle) {
+		return true;
+	}
+
+	uint64_t pn = m_PacketNumber++;
+
+	return sendLong(pn);
+}
+
+bool Connection::sendShortString(const char* str) {
+	size_t size = strlen(str);
+
+	assert(size <= 255);
+
+	MUST_TRANSFER_BOOL(sendByte(static_cast<uint8_t>(size)));
+	if (size > 0) {
+		MUST_TRANSFER_BOOL(sendBytes((const uint8_t*)str, size));
+	}
+
+	return true;
+}
+
+bool Connection::sendPacketType(SendPacketType type) {
+	MUST_TRANSFER_BOOL(sendByte(0));
+	MUST_TRANSFER_BOOL(sendByte(0));
+	MUST_TRANSFER_BOOL(sendByte(0));
+
+	return sendByte(static_cast<uint8_t>(type));
+}
+
+bool Connection::sendLongString(const char* str) {
+	int size = strlen(str);
+
+	MUST_TRANSFER_BOOL(sendInt(size));
+
+	return sendBytes((const uint8_t*)str, size);
+}
+
+int Connection::getWriteError() { return m_UDP.getWriteError(); }
+
+// PACKET_HEARTBEAT 0
+void Connection::sendHeartbeat() {
+	MUST(m_Connected);
+	MUST(sendPacketCallback(SendPacketType::HeartBeat, []() { return true; }));
+}
+
+// PACKET_ACCEL 4
+void Connection::sendSensorAcceleration(uint8_t sensorId, Vector3 vector) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::Accel,
+		AccelPacket{
+			.x = vector.x,
+			.y = vector.y,
+			.z = vector.z,
+			.sensorId = sensorId,
+		}
+	));
+}
+
+// PACKET_BATTERY_LEVEL 12
+void Connection::sendBatteryLevel(float batteryVoltage, float batteryPercentage) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::BatteryLevel,
+		BatteryLevelPacket{
+			.batteryVoltage = batteryVoltage,
+			.batteryPercentage = batteryPercentage,
+		}
+	));
+}
+
+// PACKET_TAP 13
+void Connection::sendSensorTap(uint8_t sensorId, uint8_t value) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::Tap,
+		TapPacket{
+			.sensorId = sensorId,
+			.value = value,
+		}
+	));
+}
+
+// PACKET_ERROR 14
+void Connection::sendSensorError(uint8_t sensorId, uint8_t error) {
+	MUST(m_Connected);
+	sendPacket(
+		SendPacketType::Error,
+		ErrorPacket{
+			.sensorId = sensorId,
+			.error = error,
+		}
+	);
+}
+
+// PACKET_SENSOR_INFO 15
+void Connection::sendSensorInfo(Sensor& sensor) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::SensorInfo,
+		SensorInfoPacket{
+			.sensorId = sensor.getSensorId(),
+			.sensorState = sensor.getSensorState(),
+			.sensorType = sensor.getSensorType(),
+			.sensorConfigData = sensor.getSensorConfigData(),
+			.hasCompletedRestCalibration = sensor.hasCompletedRestCalibration(),
+			.sensorPosition = sensor.getSensorPosition(),
+			.sensorDataType = sensor.getDataType(),
+
+			.tpsCounterAveragedTps = sensor.m_tpsCounter.getAveragedTPS(),
+			.dataCounterAveragedTps = sensor.m_dataCounter.getAveragedTPS(),
+		}
+	));
+}
+
+// PACKET_ROTATION_DATA 17
+void Connection::sendRotationData(
+	uint8_t sensorId,
+	Quat* const quaternion,
+	uint8_t dataType,
+	uint8_t accuracyInfo
+) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::RotationData,
+		RotationDataPacket{
+			.sensorId = sensorId,
+			.dataType = dataType,
+			.x = quaternion->x,
+			.y = quaternion->y,
+			.z = quaternion->z,
+			.w = quaternion->w,
+			.accuracyInfo = accuracyInfo,
+		}
+	));
+}
+
+// PACKET_MAGNETOMETER_ACCURACY 18
+void Connection::sendMagnetometerAccuracy(uint8_t sensorId, float accuracyInfo) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::MagnetometerAccuracy,
+		MagnetometerAccuracyPacket{
+			.sensorId = sensorId,
+			.accuracyInfo = accuracyInfo,
+		}
+	));
+}
+
+// PACKET_SIGNAL_STRENGTH 19
+void Connection::sendSignalStrength(uint8_t signalStrength) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::SignalStrength,
+		SignalStrengthPacket{
+			.sensorId = 255,
+			.signalStrength = signalStrength,
+		}
+	));
+}
+
+// PACKET_TEMPERATURE 20
+void Connection::sendTemperature(uint8_t sensorId, float temperature) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::Temperature,
+		TemperaturePacket{
+			.sensorId = sensorId,
+			.temperature = temperature,
+		}
+	));
+}
+
+// PACKET_FEATURE_FLAGS 22
+void Connection::sendFeatureFlags() {
+	MUST(m_Connected);
+	sendPacketCallback(SendPacketType::FeatureFlags, [&]() {
+		return write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size());
+	});
+}
+
+// PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24
+
+void Connection::sendAcknowledgeConfigChange(
+	uint8_t sensorId,
+	SensorToggles configType
+) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::AcknowledgeConfigChange,
+		AcknowledgeConfigChangePacket{
+			.sensorId = sensorId,
+			.configType = configType,
+		}
+	));
+}
+
+void Connection::sendTrackerDiscovery() {
+	MUST(!m_Connected);
+	MUST(sendPacketCallback(
+		SendPacketType::Handshake,
+		[&]() {
+			uint8_t mac[6];
+			WiFi.macAddress(mac);
+
+			MUST_TRANSFER_BOOL(sendInt(BOARD));
+			// This is kept for backwards compatibility,
+			// but the latest SlimeVR server will not initialize trackers
+			// with firmware build > 8 until it recieves a sensor info packet
+			MUST_TRANSFER_BOOL(sendInt(static_cast<int>(sensorManager.getSensorType(0)))
+			);
+			MUST_TRANSFER_BOOL(sendInt(HARDWARE_MCU));
+			// Backwards compatibility, unused IMU data
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(0));
+			MUST_TRANSFER_BOOL(sendInt(PROTOCOL_VERSION));
+			MUST_TRANSFER_BOOL(sendShortString(FIRMWARE_VERSION));
+			// MAC address string
+			MUST_TRANSFER_BOOL(sendBytes(mac, 6));
+			// Tracker type to hint the server if it's a glove or normal tracker or
+			// something else
+			MUST_TRANSFER_BOOL(sendByte(static_cast<uint8_t>(TRACKER_TYPE)));
+			static_assert(std::string_view{VENDOR_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(VENDOR_NAME));
+			static_assert(std::string_view{VENDOR_URL}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(VENDOR_URL));
+			static_assert(std::string_view{PRODUCT_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(PRODUCT_NAME));
+			static_assert(std::string_view{UPDATE_ADDRESS}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(UPDATE_ADDRESS));
+			static_assert(std::string_view{UPDATE_NAME}.size() <= 255);
+			MUST_TRANSFER_BOOL(sendShortString(UPDATE_NAME));
+			return true;
+		},
+		0
+	));
+}
+
+// PACKET_FLEX_DATA 24
+void Connection::sendFlexData(uint8_t sensorId, float flexLevel) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::FlexData,
+		FlexDataPacket{
+			.sensorId = sensorId,
+			.flexLevel = flexLevel,
+		}
+	));
+}
+
+#if ENABLE_INSPECTION
+void Connection::sendInspectionRawIMUData(
+	uint8_t sensorId,
+	int16_t rX,
+	int16_t rY,
+	int16_t rZ,
+	uint8_t rA,
+	int16_t aX,
+	int16_t aY,
+	int16_t aZ,
+	uint8_t aA,
+	int16_t mX,
+	int16_t mY,
+	int16_t mZ,
+	uint8_t mA
+) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::Inspection,
+		IntRawImuDataInspectionPacket{
+			.inspectionPacketType = InspectionPacketType::RawImuData,
+			.sensorId = sensorId,
+			.inspectionDataType = InspectionDataType::Int,
+
+			.rX = static_cast<uint32_t>(rX),
+			.rY = static_cast<uint32_t>(rY),
+			.rZ = static_cast<uint32_t>(rZ),
+			.rA = rA,
+
+			.aX = static_cast<uint32_t>(aX),
+			.aY = static_cast<uint32_t>(aY),
+			.aZ = static_cast<uint32_t>(aZ),
+			.aA = aA,
+
+			.mX = static_cast<uint32_t>(mX),
+			.mY = static_cast<uint32_t>(mY),
+			.mZ = static_cast<uint32_t>(mZ),
+			.mA = mA,
+		}
+	))
+}
+
+void Connection::sendInspectionRawIMUData(
+	uint8_t sensorId,
+	float rX,
+	float rY,
+	float rZ,
+	uint8_t rA,
+	float aX,
+	float aY,
+	float aZ,
+	uint8_t aA,
+	float mX,
+	float mY,
+	float mZ,
+	uint8_t mA
+) {
+	MUST(m_Connected);
+	MUST(sendPacket(
+		SendPacketType::Inspection,
+		FloatRawImuDataInspectionPacket{
+			.inspectionPacketType = InspectionPacketType::RawImuData,
+			.sensorId = sensorId,
+			.inspectionDataType = InspectionDataType::Float,
+
+			.rX = rX,
+			.rY = rY,
+			.rZ = rZ,
+			.rA = rA,
+
+			.aX = aX,
+			.aY = aY,
+			.aZ = aZ,
+			.aA = aA,
+
+			.mX = mX,
+			.mY = mY,
+			.mZ = mZ,
+			.mA = mA,
+		}
+	));
+}
+#endif
+
+void Connection::returnLastPacket(int len) {
+	MUST(m_Connected);
+
+	MUST(beginPacket());
+
+	MUST(sendBytes(m_Packet, len));
+
+	MUST(endPacket());
+}
+
+void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors) {
+	if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
+		return;
+	}
+
+	m_LastSensorInfoPacketTimestamp = millis();
+
+	for (int i = 0; i < (int)sensors.size(); i++) {
+		if (isSensorStateUpdated(i, sensors[i])) {
+			sendSensorInfo(*sensors[i]);
+		}
+	}
+}
+
+void Connection::maybeRequestFeatureFlags() {
+	if (m_ServerFeatures.isAvailable() || m_FeatureFlagsRequestAttempts >= 15) {
+		return;
+	}
+
+	if (millis() - m_FeatureFlagsRequestTimestamp < 500) {
+		return;
+	}
+
+	sendFeatureFlags();
+	m_FeatureFlagsRequestTimestamp = millis();
+	m_FeatureFlagsRequestAttempts++;
+}
+
+bool Connection::isSensorStateUpdated(int i, std::unique_ptr<Sensor>& sensor) {
+	return (m_AckedSensorState[i] != sensor->getSensorState()
+			|| m_AckedSensorCalibration[i] != sensor->hasCompletedRestCalibration()
+			|| m_AckedSensorConfigData[i] != sensor->getSensorConfigData())
+		&& sensor->getSensorType() != SensorTypeID::Unknown
+		&& sensor->getSensorType() != SensorTypeID::Empty;
+}
+
+void Connection::searchForServer() {
+	while (true) {
+		int packetSize = m_UDP.parsePacket();
+		if (!packetSize) {
+			break;
+		}
+
+		// receive incoming UDP packets
+		[[maybe_unused]] int len = m_UDP.read(m_Packet, sizeof(m_Packet));
+
+#ifdef DEBUG_NETWORK
+		m_Logger.trace(
+			"Received %d bytes from %s, port %d",
+			packetSize,
+			m_UDP.remoteIP().toString().c_str(),
+			m_UDP.remotePort()
+		);
+		m_Logger.traceArray("UDP packet contents: ", m_Packet, len);
+#endif
+
+		// Handshake is different, it has 3 in the first byte, not the 4th, and data
+		// starts right after
+		if (static_cast<ReceivePacketType>(m_Packet[0])
+			== ReceivePacketType::Handshake) {
+			if (strncmp((char*)m_Packet + 1, "Hey OVR =D 5", 12) != 0) {
+				m_Logger.error("Received invalid handshake packet");
+				continue;
+			}
+
+			m_ServerHost = m_UDP.remoteIP();
+			m_ServerPort = m_UDP.remotePort();
+			m_LastPacketTimestamp = millis();
+			m_Connected = true;
+
+			m_FeatureFlagsRequestAttempts = 0;
+			m_ServerFeatures = ServerFeatures{};
+
+			statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
+			ledManager.off();
+
+			m_Logger.debug(
+				"Handshake successful, server is %s:%d",
+				m_UDP.remoteIP().toString().c_str(),
+				m_UDP.remotePort()
+			);
+
+			break;
+		}
+	}
+
+	auto now = millis();
+
+	// This makes the LED blink for 20ms every second
+	if (m_LastConnectionAttemptTimestamp + 1000 < now) {
+		m_LastConnectionAttemptTimestamp = now;
+		m_Logger.info("Searching for the server on the local network...");
+		Connection::sendTrackerDiscovery();
+		ledManager.on();
+	} else if (m_LastConnectionAttemptTimestamp + 20 < now) {
+		ledManager.off();
+	}
+}
+
+void Connection::reset() {
+	m_Connected = false;
+	std::fill(
+		m_AckedSensorState,
+		m_AckedSensorState + MAX_SENSORS_COUNT,
+		SensorStatus::SENSOR_OFFLINE
+	);
+	std::fill(
+		m_AckedSensorCalibration,
+		m_AckedSensorCalibration + MAX_SENSORS_COUNT,
+		false
+	);
+	std::fill(
+		m_AckedSensorConfigData,
+		m_AckedSensorConfigData + MAX_SENSORS_COUNT,
+		SlimeVR::Configuration::SensorConfigBits{}
+	);
+
+	m_UDP.begin(m_ServerPort);
+
+	// Reset server address to broadcast if disconnected
+	m_ServerHost = IPAddress(255, 255, 255, 255);
+
+	statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
+}
+
+void Connection::update() {
+	if (!m_Connected) {
+		searchForServer();
+		return;
+	}
+
+	auto& sensors = sensorManager.getSensors();
+
+	updateSensorState(sensors);
+	maybeRequestFeatureFlags();
+
+	if (m_LastPacketTimestamp + TIMEOUT < millis()) {
+		statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
+
+		m_Connected = false;
+		std::fill(
+			m_AckedSensorState,
+			m_AckedSensorState + MAX_SENSORS_COUNT,
+			SensorStatus::SENSOR_OFFLINE
+		);
+		std::fill(
+			m_AckedSensorCalibration,
+			m_AckedSensorCalibration + MAX_SENSORS_COUNT,
+			false
+		);
+		m_Logger.warn("Connection to server timed out");
+
+		// Reset server address to broadcast if disconnected
+		m_ServerHost = IPAddress(255, 255, 255, 255);
+
+		return;
+	}
+
+	int packetSize = m_UDP.parsePacket();
+	if (!packetSize) {
+		return;
+	}
+
+	int len = m_UDP.read(m_Packet, sizeof(m_Packet));
+
+#ifdef DEBUG_NETWORK
+	m_Logger.trace(
+		"Received %d bytes from %s, port %d",
+		packetSize,
+		m_UDP.remoteIP().toString().c_str(),
+		m_UDP.remotePort()
+	);
+	m_Logger.traceArray("UDP packet contents: ", m_Packet, len);
+#else
+	(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();
+			break;
+
+		case ReceivePacketType::Vibrate:
+			break;
+
+		case ReceivePacketType::Handshake:
+			// handled above
+			break;
+
+		case ReceivePacketType::Command:
+			break;
+
+		case ReceivePacketType::Config:
+			break;
+
+		case ReceivePacketType::PingPong:
+			returnLastPacket(len);
+			break;
+
+		case ReceivePacketType::SensorInfo: {
+			if (len < 6) {
+				m_Logger.warn("Wrong sensor info packet");
+				break;
+			}
+
+			SensorInfoPacket sensorInfoPacket;
+			memcpy(&sensorInfoPacket, m_Packet + 4, sizeof(sensorInfoPacket));
+
+			for (int i = 0; i < (int)sensors.size(); i++) {
+				if (sensorInfoPacket.sensorId == sensors[i]->getSensorId()) {
+					m_AckedSensorState[i] = sensorInfoPacket.sensorState;
+					if (len < 12) {
+						m_AckedSensorCalibration[i]
+							= sensors[i]->hasCompletedRestCalibration();
+						m_AckedSensorConfigData[i] = sensors[i]->getSensorConfigData();
+						break;
+					}
+					m_AckedSensorCalibration[i]
+						= sensorInfoPacket.hasCompletedRestCalibration;
+					break;
+				}
+			}
+
+			break;
+		}
+		case ReceivePacketType::FeatureFlags: {
+			// Packet type (4) + Packet number (8) + flags (len - 12)
+			if (len < 13) {
+				m_Logger.warn("Invalid feature flags packet: too short");
+				break;
+			}
+
+			bool hadFlags = m_ServerFeatures.isAvailable();
+
+			uint32_t flagsLength = len - 12;
+			m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength);
+
+			if (!hadFlags) {
+#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
+				if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) {
+					m_Logger.debug("Server supports packet bundling");
+				}
+#endif
+			}
+
+			break;
+		}
+
+		case ReceivePacketType::SetConfigFlag: {
+			// Packet type (4) + Packet number (8) + sensor_id(1) + flag_id (2) + state
+			// (1)
+			if (len < 16) {
+				m_Logger.warn("Invalid sensor config flag packet: too short");
+				break;
+			}
+
+			SetConfigFlagPacket setConfigFlagPacket;
+			memcpy(&setConfigFlagPacket, m_Packet + 12, sizeof(SetConfigFlagPacket));
+
+			uint8_t sensorId = setConfigFlagPacket.sensorId;
+			SensorToggles flag = setConfigFlagPacket.flag;
+			bool newState = setConfigFlagPacket.newState;
+			if (sensorId == UINT8_MAX) {
+				for (auto& sensor : sensors) {
+					sensor->setFlag(flag, newState);
+				}
+			} else {
+				auto& sensors = sensorManager.getSensors();
+
+				if (sensorId >= sensors.size()) {
+					m_Logger.warn("Invalid sensor config flag packet: invalid sensor id"
+					);
+					break;
+				}
+
+				auto& sensor = sensors[sensorId];
+				sensor->setFlag(flag, newState);
+			}
+			sendAcknowledgeConfigChange(sensorId, flag);
+			configuration.save();
+			break;
+		}
+	}
+}
+
+}  // namespace SlimeVR::Network

src/network/connection.h

@@ -0,0 +1,247 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2023 SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#ifndef SLIMEVR_NETWORK_CONNECTION_H_
+#define SLIMEVR_NETWORK_CONNECTION_H_
+
+#include <Arduino.h>
+#include <WiFiUdp.h>
+
+#include <optional>
+
+#include "../configuration/SensorConfig.h"
+#include "featureflags.h"
+#include "globals.h"
+#include "packets.h"
+#include "quat.h"
+#include "sensors/sensor.h"
+#include "wifihandler.h"
+
+namespace SlimeVR::Network {
+
+#define MUST_TRANSFER_BOOL(b) \
+	if (!(b))                 \
+		return false;
+
+#define MUST(b) \
+	if (!(b))   \
+		return;
+
+class Connection {
+public:
+	Connection() {
+#ifdef SERVER_IP
+		m_ServerHost.fromString(SERVER_IP);
+#endif
+
+#ifdef SERVER_PORT
+		m_ServerPort = SERVER_PORT;
+#endif
+	}
+
+	void searchForServer();
+	void update();
+	void reset();
+	bool isConnected() const { return m_Connected; }
+
+	// PACKET_ACCEL 4
+	void sendSensorAcceleration(uint8_t sensorId, Vector3 vector);
+
+	// PACKET_BATTERY_LEVEL 12
+	void sendBatteryLevel(float batteryVoltage, float batteryPercentage);
+
+	// PACKET_TAP 13
+	void sendSensorTap(uint8_t sensorId, uint8_t value);
+
+	// PACKET_ERROR 14
+	void sendSensorError(uint8_t sensorId, uint8_t error);
+
+	// PACKET_ROTATION_DATA 17
+	void sendRotationData(
+		uint8_t sensorId,
+		Quat* const quaternion,
+		uint8_t dataType,
+		uint8_t accuracyInfo
+	);
+
+	// PACKET_MAGNETOMETER_ACCURACY 18
+	void sendMagnetometerAccuracy(uint8_t sensorId, float accuracyInfo);
+
+	// PACKET_SIGNAL_STRENGTH 19
+	void sendSignalStrength(uint8_t signalStrength);
+
+	// PACKET_TEMPERATURE 20
+	void sendTemperature(uint8_t sensorId, float temperature);
+
+	// PACKET_FEATURE_FLAGS 22
+	void sendFeatureFlags();
+
+	// PACKET_FLEX_DATA 26
+	void sendFlexData(uint8_t sensorId, float flexLevel);
+
+#if ENABLE_INSPECTION
+	void sendInspectionRawIMUData(
+		uint8_t sensorId,
+		int16_t rX,
+		int16_t rY,
+		int16_t rZ,
+		uint8_t rA,
+		int16_t aX,
+		int16_t aY,
+		int16_t aZ,
+		uint8_t aA,
+		int16_t mX,
+		int16_t mY,
+		int16_t mZ,
+		uint8_t mA
+	);
+	void sendInspectionRawIMUData(
+		uint8_t sensorId,
+		float rX,
+		float rY,
+		float rZ,
+		uint8_t rA,
+		float aX,
+		float aY,
+		float aZ,
+		uint8_t aA,
+		float mX,
+		float mY,
+		float mZ,
+		uint8_t mA
+	);
+#endif
+
+	const ServerFeatures& getServerFeatureFlags() { return m_ServerFeatures; }
+
+	bool beginBundle();
+	bool endBundle();
+
+private:
+	void updateSensorState(std::vector<std::unique_ptr<::Sensor>>& sensors);
+	void maybeRequestFeatureFlags();
+	bool isSensorStateUpdated(int i, std::unique_ptr<::Sensor>& sensor);
+
+	bool beginPacket();
+	bool endPacket();
+
+	size_t write(const uint8_t* buffer, size_t size);
+	size_t write(uint8_t byte);
+
+	bool sendPacketType(SendPacketType type);
+	bool sendPacketNumber();
+	bool sendFloat(float f);
+	bool sendByte(uint8_t c);
+	bool sendShort(uint16_t i);
+	bool sendInt(uint32_t i);
+	bool sendLong(uint64_t l);
+	bool sendBytes(const uint8_t* c, size_t length);
+	bool sendShortString(const char* str);
+	bool sendLongString(const char* str);
+
+	template <typename Packet>
+	bool sendPacket(
+		SendPacketType type,
+		Packet packet,
+		std::optional<uint64_t> packetNumberOverride = std::nullopt
+	) {
+		MUST_TRANSFER_BOOL(beginPacket());
+		MUST_TRANSFER_BOOL(sendPacketType(type));
+		if (packetNumberOverride) {
+			MUST_TRANSFER_BOOL(sendLong(*packetNumberOverride));
+		} else {
+			MUST_TRANSFER_BOOL(sendPacketNumber());
+		}
+
+		MUST_TRANSFER_BOOL(
+			sendBytes(reinterpret_cast<uint8_t*>(&packet), sizeof(Packet))
+		);
+
+		return endPacket();
+	}
+
+	template <typename Callback>
+	bool sendPacketCallback(
+		SendPacketType type,
+		Callback bodyCallback,
+		std::optional<uint64_t> packetNumberOverride = std::nullopt
+	) {
+		MUST_TRANSFER_BOOL(beginPacket());
+		MUST_TRANSFER_BOOL(sendPacketType(type));
+		if (packetNumberOverride) {
+			MUST_TRANSFER_BOOL(sendLong(*packetNumberOverride));
+		} else {
+			MUST_TRANSFER_BOOL(sendPacketNumber());
+		}
+
+		MUST_TRANSFER_BOOL(bodyCallback());
+
+		return endPacket();
+	}
+
+	int getWriteError();
+
+	void returnLastPacket(int len);
+
+	// PACKET_HEARTBEAT 0
+	void sendHeartbeat();
+
+	// PACKET_HANDSHAKE 3
+	void sendTrackerDiscovery();
+
+	// PACKET_SENSOR_INFO 15
+	void sendSensorInfo(::Sensor& sensor);
+
+	void sendAcknowledgeConfigChange(uint8_t sensorId, SensorToggles configType);
+
+	bool m_Connected = false;
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection");
+
+	WiFiUDP m_UDP;
+	unsigned char m_Packet[128];  // buffer for incoming packets
+	uint64_t m_PacketNumber = 0;
+
+	int m_ServerPort = 6969;
+	IPAddress m_ServerHost = IPAddress(255, 255, 255, 255);
+	unsigned long m_LastConnectionAttemptTimestamp;
+	unsigned long m_LastPacketTimestamp;
+
+	SensorStatus m_AckedSensorState[MAX_SENSORS_COUNT] = {SensorStatus::SENSOR_OFFLINE};
+	SlimeVR::Configuration::SensorConfigBits m_AckedSensorConfigData[MAX_SENSORS_COUNT]
+		= {};
+	bool m_AckedSensorCalibration[MAX_SENSORS_COUNT] = {false};
+	unsigned long m_LastSensorInfoPacketTimestamp = 0;
+
+	uint8_t m_FeatureFlagsRequestAttempts = 0;
+	unsigned long m_FeatureFlagsRequestTimestamp = millis();
+	ServerFeatures m_ServerFeatures{};
+
+	bool m_IsBundle = false;
+	uint16_t m_BundlePacketPosition = 0;
+	uint16_t m_BundlePacketInnerCount = 0;
+
+	unsigned char m_Buf[8];
+};
+
+}  // namespace SlimeVR::Network
+
+#endif  // SLIMEVR_NETWORK_CONNECTION_H_

src/network/featureflags.h

@@ -0,0 +1,105 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2023 SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#ifndef SLIMEVR_FEATURE_FLAGS_H_
+#define SLIMEVR_FEATURE_FLAGS_H_
+
+#include <algorithm>
+#include <cstring>
+
+/**
+ * Bit packed flags, enum values start with 0 and indicate which bit it is.
+ *
+ * Change the enums and `flagsEnabled` inside to extend.
+ */
+struct ServerFeatures {
+public:
+	enum EServerFeatureFlags : uint32_t {
+		// Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64).
+		PROTOCOL_BUNDLE_SUPPORT,
+
+		// Add new flags here
+
+		BITS_TOTAL,
+	};
+
+	bool has(EServerFeatureFlags flag) {
+		uint32_t bit = static_cast<uint32_t>(flag);
+		return m_Available && (m_Flags[bit / 8] & (1 << (bit % 8)));
+	}
+
+	/**
+	 * Whether the server supports the "feature flags" feature,
+	 * set to true when we've received flags packet from the server.
+	 */
+	bool isAvailable() { return m_Available; }
+
+	static ServerFeatures from(uint8_t* received, uint32_t length) {
+		ServerFeatures res;
+		res.m_Available = true;
+		memcpy(
+			res.m_Flags,
+			received,
+			std::min(static_cast<uint32_t>(sizeof(res.m_Flags)), length)
+		);
+		return res;
+	}
+
+private:
+	bool m_Available = false;
+
+	uint8_t m_Flags[static_cast<uint32_t>(EServerFeatureFlags::BITS_TOTAL) / 8 + 1];
+};
+
+class FirmwareFeatures {
+public:
+	enum EFirmwareFeatureFlags : uint32_t {
+		// EXAMPLE_FEATURE,
+		B64_WIFI_SCANNING = 1,
+		SENSOR_CONFIG = 2,
+		// Add new flags here
+
+		BITS_TOTAL,
+	};
+
+	// Flags to send
+	static constexpr const std::initializer_list<EFirmwareFeatureFlags> flagsEnabled = {
+		// EXAMPLE_FEATURE,
+		B64_WIFI_SCANNING,
+		SENSOR_CONFIG
+		// Add enabled flags here
+	};
+
+	static constexpr auto flags = [] {
+		constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1;
+		std::array<uint8_t, flagsLength> packed{};
+
+		for (uint32_t bit : flagsEnabled) {
+			packed[bit / 8] |= 1 << (bit % 8);
+		}
+
+		return packed;
+	}();
+};
+
+#endif

src/network/manager.cpp

@@ -0,0 +1,50 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2023 SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	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 "manager.h"
+
+#include "GlobalVars.h"
+
+namespace SlimeVR::Network {
+
+void Manager::setup() { wifiNetwork.setUp(); }
+
+void Manager::update() {
+	wifiNetwork.upkeep();
+
+	auto wasConnected = m_IsConnected;
+
+	m_IsConnected = wifiNetwork.isConnected();
+
+	if (!m_IsConnected) {
+		return;
+	}
+
+	if (!wasConnected) {
+		// WiFi was reconnected, rediscover the server and reconnect
+		networkConnection.reset();
+	}
+
+	networkConnection.update();
+}
+
+}  // namespace SlimeVR::Network

src/network/manager.h

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

src/network/network.cpp

@@ -1,42 +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 "network.h"
-
-bool lastWifiConnected = false;
-
-void Network::setUp() {
-    WiFiNetwork::setUp();
-}
-
-void Network::update(Sensor * const sensor, Sensor * const sensor2) {
-    WiFiNetwork::upkeep();
-    if(WiFiNetwork::isConnected()) {
-        if(lastWifiConnected == false) {
-            lastWifiConnected = true;
-            ServerConnection::resetConnection(); // WiFi was reconnected, reconnect to the server
-        }
-        ServerConnection::update(sensor, sensor2);
-    } else {
-        lastWifiConnected = false;
-    }
-}

src/network/packets.h

@@ -1,132 +1,236 @@
 /*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
 */
 
 #ifndef SLIMEVR_PACKETS_H_
 #define SLIMEVR_PACKETS_H_
 
-#include "sensors/sensor.h"
+#include <cstdint>
 
-#define PACKET_HEARTBEAT 0
-//#define PACKET_ROTATION 1 // Deprecated
-//#define PACKET_GYRO 2 // Deprecated
-#define PACKET_HANDSHAKE 3
-#define PACKET_ACCEL 4
-//#define PACKET_MAG 5 // Deprecated
-#define PACKET_RAW_CALIBRATION_DATA 6
-#define PACKET_CALIBRATION_FINISHED 7
-#define PACKET_CONFIG 8
-//#define PACKET_RAW_MAGNETOMETER 9 // Deprecated
-#define PACKET_PING_PONG 10
-#define PACKET_SERIAL 11
-#define PACKET_BATTERY_LEVEL 12
-#define PACKET_TAP 13
-#define PACKET_ERROR 14
-#define PACKET_SENSOR_INFO 15
-//#define PACKET_ROTATION_2 16 // Deprecated
-#define PACKET_ROTATION_DATA 17
-#define PACKET_MAGNETOMETER_ACCURACY 18
-#define PACKET_SIGNAL_STRENGTH 19
-#define PACKET_TEMPERATURE 20
+#include "../consts.h"
+#include "../sensors/sensor.h"
 
-#define PACKET_INSPECTION 105 // 0x69
+enum class SendPacketType : uint8_t {
+	HeartBeat = 0,
+	//  Rotation = 1,
+	//  Gyro = 2,
+	Handshake = 3,
+	Accel = 4,
+	// Mag = 5,
+	// RawCalibrationData = 6,
+	// CalibrationFinished = 7,
+	// RawMagnetometer = 9,
+	Serial = 11,
+	BatteryLevel = 12,
+	Tap = 13,
+	Error = 14,
+	SensorInfo = 15,
+	// Rotation2 = 16,
+	RotationData = 17,
+	MagnetometerAccuracy = 18,
+	SignalStrength = 19,
+	Temperature = 20,
+	// UserAction = 21,
+	FeatureFlags = 22,
+	// RotationAcceleration = 23,
+	AcknowledgeConfigChange = 24,
+	FlexData = 26,
+	// PositionData = 27,
+	Bundle = 100,
+	Inspection = 105,
+};
 
-#define PACKET_RECEIVE_HEARTBEAT 1
-#define PACKET_RECEIVE_VIBRATE 2
-#define PACKET_RECEIVE_HANDSHAKE 3
-#define PACKET_RECEIVE_COMMAND 4
+enum class ReceivePacketType : uint8_t {
+	HeartBeat = 1,
+	Vibrate = 2,
+	Handshake = 3,
+	Command = 4,
+	Config = 8,
+	PingPong = 10,
+	SensorInfo = 15,
+	FeatureFlags = 22,
+	SetConfigFlag = 25,
+};
 
-#define PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA 1
-#define PACKET_INSPECTION_PACKETTYPE_FUSED_IMU_DATA 2
-#define PACKET_INSPECTION_PACKETTYPE_CORRECTION_DATA 3
-#define PACKET_INSPECTION_DATATYPE_INT 1
-#define PACKET_INSPECTION_DATATYPE_FLOAT 2
+enum class InspectionPacketType : uint8_t {
+	RawImuData = 1,
+	FusedImuData = 2,
+	CorrectionData = 3,
+};
 
-namespace Network {
-    // PACKET_HEARTBEAT 0
-    void sendHeartbeat();
+enum class InspectionDataType : uint8_t {
+	Int = 1,
+	Float = 2,
+};
 
-    // PACKET_HANDSHAKE 3
-    void sendHandshake();
+// From the SH-2 interface that BNO08x use.
+enum class PacketErrorCode : uint8_t {
+	NOT_APPLICABLE = 0,
+	POWER_ON_RESET = 1,
+	INTERNAL_SYSTEM_RESET = 2,
+	WATCHDOG_TIMEOUT = 3,
+	EXTERNAL_RESET = 4,
+	OTHER = 5,
+};
 
-    // PACKET_ACCEL 4
-    void sendAccel(float* vector, uint8_t sensorId);
+#pragma pack(push, 1)
 
-    // PACKET_RAW_CALIBRATION_DATA 6
-    void sendRawCalibrationData(float* vector, uint8_t calibrationType, uint8_t sensorId);
-    void sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8_t sensorId);
-
-    // PACKET_CALIBRATION_FINISHED 7
-    void sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId);
-
-    // PACKET_BATTERY_LEVEL 12
-    void sendBatteryLevel(float batteryVoltage, float batteryPercentage);
-
-    // PACKET_TAP 13
-    void sendTap(uint8_t value, uint8_t sensorId);
-
-    // PACKET_ERROR 14
-    void sendError(uint8_t reason, uint8_t sensorId);
-
-    // PACKET_SENSOR_INFO 15
-    void sendSensorInfo(Sensor * sensor);
-
-    // PACKET_ROTATION_DATA 17
-    void sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_t accuracyInfo, uint8_t sensorId);
-
-    // PACKET_MAGNETOMETER_ACCURACY 18
-    void sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId);
-
-    // PACKET_SIGNAL_STRENGTH 19
-    void sendSignalStrength(uint8_t signalStrength);
-
-    // PACKET_TEMPERATURE 20
-    void sendTemperature(float temperature, uint8_t sensorId);
-
-#if ENABLE_INSPECTION
-    void sendInspectionRawIMUData(uint8_t sensorId, int16_t rX, int16_t rY, int16_t rZ, uint8_t rA, int16_t aX, int16_t aY, int16_t aZ, uint8_t aA, int16_t mX, int16_t mY, int16_t mZ, uint8_t mA);
-    void sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, float rZ, uint8_t rA, float aX, float aY, float aZ, uint8_t aA, float mX, float mY, float mZ, uint8_t mA);
-
-    void sendInspectionFusedIMUData(uint8_t sensorId, Quat quaternion);
-
-    void sendInspectionCorrectionData(uint8_t sensorId, Quat quaternion);
-#endif
+template <typename T>
+T swapEndianness(T value) {
+	auto* bytes = reinterpret_cast<uint8_t*>(&value);
+	std::reverse(bytes, bytes + sizeof(T));
+	return value;
 }
 
-namespace DataTransfer {
-    bool beginPacket();
-    bool endPacket();
-    void sendPacketType(uint8_t type);
-    void sendPacketNumber();
+template <typename T>
+struct BigEndian {
+	BigEndian() = default;
+	explicit(false) BigEndian(T val) { value = swapEndianness(val); }
+	explicit(false) operator T() const { return swapEndianness(value); }
 
-    void sendFloat(float f);
-    void sendByte(uint8_t c);
-    void sendInt(int i);
-    void sendLong(uint64_t l);
-    void sendBytes(const uint8_t * c, size_t length);
-    void sendShortString(const char * str);
-    void sendLongString(const char * str);
+	T value{};
+};
 
-    int getWriteError();
-}
+struct AccelPacket {
+	BigEndian<float> x;
+	BigEndian<float> y;
+	BigEndian<float> z;
+	uint8_t sensorId{};
+};
 
-#endif // SLIMEVR_PACKETS_H_
+struct BatteryLevelPacket {
+	BigEndian<float> batteryVoltage;
+	BigEndian<float> batteryPercentage;
+};
+
+struct TapPacket {
+	uint8_t sensorId;
+	uint8_t value;
+};
+
+struct ErrorPacket {
+	uint8_t sensorId;
+	uint8_t error;
+};
+
+struct SensorInfoPacket {
+	uint8_t sensorId{};
+	SensorStatus sensorState{};
+	SensorTypeID sensorType{};
+	BigEndian<SlimeVR::Configuration::SensorConfigBits> sensorConfigData{};
+	bool hasCompletedRestCalibration{};
+	SensorPosition sensorPosition{};
+	SensorDataType sensorDataType{};
+	// ADD NEW FIELDS ABOVE THIS COMMENT ^^^^^^^^
+	// WARNING! Only for debug purposes and SHOULD ALWAYS BE LAST IN THE PACKET.
+	// It WILL BE REMOVED IN THE FUTURE
+	// Send TPS
+	BigEndian<float> tpsCounterAveragedTps;
+	BigEndian<float> dataCounterAveragedTps;
+};
+
+struct RotationDataPacket {
+	uint8_t sensorId{};
+	uint8_t dataType{};
+	BigEndian<float> x;
+	BigEndian<float> y;
+	BigEndian<float> z;
+	BigEndian<float> w;
+	uint8_t accuracyInfo{};
+};
+
+struct MagnetometerAccuracyPacket {
+	uint8_t sensorId{};
+	BigEndian<float> accuracyInfo;
+};
+
+struct SignalStrengthPacket {
+	uint8_t sensorId;
+	uint8_t signalStrength;
+};
+
+struct TemperaturePacket {
+	uint8_t sensorId{};
+	BigEndian<float> temperature;
+};
+
+struct AcknowledgeConfigChangePacket {
+	uint8_t sensorId{};
+	BigEndian<SensorToggles> configType;
+};
+
+struct FlexDataPacket {
+	uint8_t sensorId{};
+	BigEndian<float> flexLevel;
+};
+
+struct IntRawImuDataInspectionPacket {
+	InspectionPacketType inspectionPacketType{};
+	uint8_t sensorId{};
+	InspectionDataType inspectionDataType{};
+
+	BigEndian<uint32_t> rX;
+	BigEndian<uint32_t> rY;
+	BigEndian<uint32_t> rZ;
+	uint8_t rA{};
+
+	BigEndian<uint32_t> aX;
+	BigEndian<uint32_t> aY;
+	BigEndian<uint32_t> aZ;
+	uint8_t aA{};
+
+	BigEndian<uint32_t> mX;
+	BigEndian<uint32_t> mY;
+	BigEndian<uint32_t> mZ;
+	uint8_t mA{};
+};
+
+struct FloatRawImuDataInspectionPacket {
+	InspectionPacketType inspectionPacketType{};
+	uint8_t sensorId{};
+	InspectionDataType inspectionDataType{};
+
+	BigEndian<float> rX;
+	BigEndian<float> rY;
+	BigEndian<float> rZ;
+	uint8_t rA{};
+
+	BigEndian<float> aX;
+	BigEndian<float> aY;
+	BigEndian<float> aZ;
+	uint8_t aA{};
+
+	BigEndian<float> mX;
+	BigEndian<float> mY;
+	BigEndian<float> mZ;
+	uint8_t mA{};
+};
+
+struct SetConfigFlagPacket {
+	uint8_t sensorId{};
+	BigEndian<SensorToggles> flag;
+	bool newState{};
+};
+
+#pragma pack(pop)
+
+#endif  // SLIMEVR_PACKETS_H_

src/network/udpclient.cpp

@@ -1,695 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
-
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
-
-    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 "udpclient.h"
-#include "packets.h"
-#include "logging/Logger.h"
-#include "GlobalVars.h"
-
-#define TIMEOUT 3000UL
-
-WiFiUDP Udp;
-unsigned char incomingPacket[128]; // buffer for incoming packets
-uint64_t packetNumber = 0;
-unsigned char handshake[12] = {0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0};
-
-int port = 6969;
-IPAddress host = IPAddress(255, 255, 255, 255);
-unsigned long lastConnectionAttemptMs;
-unsigned long lastPacketMs;
-
-bool connected = false;
-
-uint8_t sensorStateNotified1 = 0;
-uint8_t sensorStateNotified2 = 0;
-unsigned long lastSensorInfoPacket = 0;
-
-uint8_t serialBuffer[128];
-size_t serialLength = 0;
-
-unsigned char buf[8];
-
-// TODO: Cleanup with proper classes
-SlimeVR::Logging::Logger udpClientLogger("UDPClient");
-
-template <typename T>
-unsigned char * convert_to_chars(T src, unsigned char * target)
-{
-    union uwunion
-    {
-        unsigned char c[sizeof(T)];
-        T v;
-    } un;
-    un.v = src;
-    for (size_t i = 0; i < sizeof(T); i++)
-    {
-        target[i] = un.c[sizeof(T) - i - 1];
-    }
-    return target;
-}
-
-template <typename T>
-T convert_chars(unsigned char * const src)
-{
-    union uwunion
-    {
-        unsigned char c[sizeof(T)];
-        T v;
-    } un;
-    for (size_t i = 0; i < sizeof(T); i++)
-    {
-        un.c[i] = src[sizeof(T) - i - 1];
-    }
-    return un.v;
-}
-
-namespace DataTransfer {
-
-    bool beginPacket() {
-        int r = Udp.beginPacket(host, port);
-        if(r == 0) {
-            // Print error
-        }
-        return r > 0;
-    }
-
-    bool endPacket() {
-        int r = Udp.endPacket();
-        if(r == 0) {
-            // Print error
-        }
-        return r > 0;
-    }
-
-    void sendPacketType(uint8_t type) {
-        Udp.write(0);
-        Udp.write(0);
-        Udp.write(0);
-        Udp.write(type);
-    }
-
-    void sendPacketNumber() {
-        uint64_t pn = packetNumber++;
-        sendLong(pn);
-    }
-
-    void sendFloat(float f) {
-        Udp.write(convert_to_chars(f, buf), sizeof(f));
-    }
-
-    void sendByte(uint8_t c) {
-        Udp.write(&c, 1);
-    }
-
-    void sendInt(int i) {
-        Udp.write(convert_to_chars(i, buf), sizeof(i));
-    }
-
-    void sendLong(uint64_t l) {
-        Udp.write(convert_to_chars(l, buf), sizeof(l));
-    }
-
-    void sendBytes(const uint8_t * c, size_t length) {
-        Udp.write(c, length);
-    }
-
-    void sendShortString(const char * str) {
-        uint8_t size = strlen(str);
-        sendByte(size); // String size
-        sendBytes((const uint8_t *) str, size); // Firmware version string
-    }
-    
-    void sendLongString(const char * str) {
-        int size = strlen(str);
-        sendInt(size); // String size
-        sendBytes((const uint8_t *) str, size); // Firmware version string
-    }
-
-    int getWriteError() {
-        return Udp.getWriteError();
-    }
-}
-
-// PACKET_HEARTBEAT 0
-void Network::sendHeartbeat() {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_HEARTBEAT);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_ACCEL 4
-void Network::sendAccel(float* vector, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_ACCEL);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendFloat(vector[0]);
-        DataTransfer::sendFloat(vector[1]);
-        DataTransfer::sendFloat(vector[2]);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_RAW_CALIBRATION_DATA 6
-void Network::sendRawCalibrationData(float* vector, uint8_t calibrationType, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendInt(calibrationType);
-        DataTransfer::sendFloat(vector[0]);
-        DataTransfer::sendFloat(vector[1]);
-        DataTransfer::sendFloat(vector[2]);
-        DataTransfer::endPacket();
-    }
-}
-
-void Network::sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendInt(calibrationType);
-        DataTransfer::sendInt(vector[0]);
-        DataTransfer::sendInt(vector[1]);
-        DataTransfer::sendInt(vector[2]);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_CALIBRATION_FINISHED 7
-void Network::sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_CALIBRATION_FINISHED);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendInt(calibrationType);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_BATTERY_LEVEL 12
-void Network::sendBatteryLevel(float batteryVoltage, float batteryPercentage) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_BATTERY_LEVEL);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendFloat(batteryVoltage);
-        DataTransfer::sendFloat(batteryPercentage);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_TAP 13
-void Network::sendTap(uint8_t value, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_TAP);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendByte(value);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_ERROR 14
-void Network::sendError(uint8_t reason, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_ERROR);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendByte(reason);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_SENSOR_INFO 15
-void Network::sendSensorInfo(Sensor * sensor) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_SENSOR_INFO);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensor->getSensorId());
-        DataTransfer::sendByte(sensor->getSensorState());
-        DataTransfer::sendByte(sensor->getSensorType());
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_ROTATION_DATA 17
-void Network::sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_t accuracyInfo, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_ROTATION_DATA);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendByte(dataType);
-        DataTransfer::sendFloat(quaternion->x);
-        DataTransfer::sendFloat(quaternion->y);
-        DataTransfer::sendFloat(quaternion->z);
-        DataTransfer::sendFloat(quaternion->w);
-        DataTransfer::sendByte(accuracyInfo);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_MAGNETOMETER_ACCURACY 18
-void Network::sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_MAGNETOMETER_ACCURACY);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendFloat(accuracyInfo);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_SIGNAL_STRENGTH 19
-void Network::sendSignalStrength(uint8_t signalStrength) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_SIGNAL_STRENGTH);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(255);
-        DataTransfer::sendByte(signalStrength);
-        DataTransfer::endPacket();
-    }
-}
-
-// PACKET_TEMPERATURE 20
-void Network::sendTemperature(float temperature, uint8_t sensorId) {
-    if(!connected)
-    {
-        return;
-    }
-
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_TEMPERATURE);
-        DataTransfer::sendPacketNumber();
-        DataTransfer::sendByte(sensorId);
-        DataTransfer::sendFloat(temperature);
-        DataTransfer::endPacket();
-    }
-}
-
-void Network::sendHandshake() {
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendPacketType(PACKET_HANDSHAKE);
-        DataTransfer::sendLong(0); // Packet number is always 0 for handshake
-        DataTransfer::sendInt(BOARD);
-        // This is kept for backwards compatibility,
-        // but the latest SlimeVR server will not initialize trackers
-        // with firmware build > 8 until it recieves sensor info packet
-        DataTransfer::sendInt(IMU);
-        DataTransfer::sendInt(HARDWARE_MCU);
-        DataTransfer::sendInt(0); 
-        DataTransfer::sendInt(0);
-        DataTransfer::sendInt(0);
-        DataTransfer::sendInt(FIRMWARE_BUILD_NUMBER); // Firmware build number
-        DataTransfer::sendShortString(FIRMWARE_VERSION);
-        uint8_t mac[6];
-        WiFi.macAddress(mac);
-        DataTransfer::sendBytes(mac, 6); // MAC address string
-        if(!DataTransfer::endPacket()) {
-            udpClientLogger.error("Handshake write error: %d", Udp.getWriteError());
-        }
-    } else {
-        udpClientLogger.error("Handshake write error: %d", Udp.getWriteError());
-    }
-}
-
-#if ENABLE_INSPECTION
-void Network::sendInspectionRawIMUData(uint8_t sensorId, int16_t rX, int16_t rY, int16_t rZ, uint8_t rA, int16_t aX, int16_t aY, int16_t aZ, uint8_t aA, int16_t mX, int16_t mY, int16_t mZ, uint8_t mA)
-{
-    if (!connected)
-    {
-        return;
-    }
-
-    if(!DataTransfer::beginPacket()) 
-    {
-        udpClientLogger.error("RawIMUData write begin error: %d", Udp.getWriteError());
-        return;
-    }
-
-    DataTransfer::sendPacketType(PACKET_INSPECTION);
-    DataTransfer::sendPacketNumber();
-
-    DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA);
-
-    DataTransfer::sendByte(sensorId);
-    DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_INT);
-
-    DataTransfer::sendInt(rX);
-    DataTransfer::sendInt(rY);
-    DataTransfer::sendInt(rZ);
-    DataTransfer::sendByte(rA);
-
-    DataTransfer::sendInt(aX);
-    DataTransfer::sendInt(aY);
-    DataTransfer::sendInt(aZ);
-    DataTransfer::sendByte(aA);
-
-    DataTransfer::sendInt(mX);
-    DataTransfer::sendInt(mY);
-    DataTransfer::sendInt(mZ);
-    DataTransfer::sendByte(mA);
-
-    if(!DataTransfer::endPacket())
-    {
-        udpClientLogger.error("RawIMUData write end error: %d", Udp.getWriteError());
-    }
-}
-
-void Network::sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, float rZ, uint8_t rA, float aX, float aY, float aZ, uint8_t aA, float mX, float mY, float mZ, uint8_t mA)
-{
-    if (!connected) 
-    {
-        return;
-    }
-
-    if (!DataTransfer::beginPacket())
-    {
-        udpClientLogger.error("RawIMUData write begin error: %d", Udp.getWriteError());
-        return;
-    }
-
-    DataTransfer::sendPacketType(PACKET_INSPECTION);
-    DataTransfer::sendPacketNumber();
-
-    DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA);
-
-    DataTransfer::sendByte(sensorId);
-    DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
-
-    DataTransfer::sendFloat(rX);
-    DataTransfer::sendFloat(rY);
-    DataTransfer::sendFloat(rZ);
-    DataTransfer::sendByte(rA);
-
-    DataTransfer::sendFloat(aX);
-    DataTransfer::sendFloat(aY);
-    DataTransfer::sendFloat(aZ);
-    DataTransfer::sendByte(aA);
-
-    DataTransfer::sendFloat(mX);
-    DataTransfer::sendFloat(mY);
-    DataTransfer::sendFloat(mZ);
-    DataTransfer::sendByte(mA);
-
-    if(!DataTransfer::endPacket())
-    {
-        udpClientLogger.error("RawIMUData write end error: %d", Udp.getWriteError());
-    }
-}
-
-void Network::sendInspectionFusedIMUData(uint8_t sensorId, Quat quaternion)
-{
-    if (!connected) 
-    {
-        return;
-    }
-
-    if (!DataTransfer::beginPacket())
-    {
-        udpClientLogger.error("FusedIMUData write begin error: %d", Udp.getWriteError());
-        return;
-    }
-
-    DataTransfer::sendPacketType(PACKET_INSPECTION);
-    DataTransfer::sendPacketNumber();
-
-    DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_FUSED_IMU_DATA);
-
-    DataTransfer::sendByte(sensorId);
-    DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
-
-    DataTransfer::sendFloat(quaternion.x);
-    DataTransfer::sendFloat(quaternion.y);
-    DataTransfer::sendFloat(quaternion.z);
-    DataTransfer::sendFloat(quaternion.w);
-
-    if(!DataTransfer::endPacket())
-    {
-        udpClientLogger.error("FusedIMUData write end error: %d", Udp.getWriteError());
-    }
-}
-
-void Network::sendInspectionCorrectionData(uint8_t sensorId, Quat quaternion)
-{
-    if (!connected) 
-    {
-        return;
-    }
-
-    if (!DataTransfer::beginPacket())
-    {
-        udpClientLogger.error("CorrectionData write begin error: %d", Udp.getWriteError());
-        return;
-    }
-
-    DataTransfer::sendPacketType(PACKET_INSPECTION);
-    DataTransfer::sendPacketNumber();
-
-    DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_CORRECTION_DATA);
-
-    DataTransfer::sendByte(sensorId);
-    DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
-
-    DataTransfer::sendFloat(quaternion.x);
-    DataTransfer::sendFloat(quaternion.y);
-    DataTransfer::sendFloat(quaternion.z);
-    DataTransfer::sendFloat(quaternion.w);
-
-    if(!DataTransfer::endPacket())
-    {
-        udpClientLogger.error("CorrectionData write end error: %d", Udp.getWriteError());
-    }
-}
-#endif
-
-void returnLastPacket(int len) {
-    if(DataTransfer::beginPacket()) {
-        DataTransfer::sendBytes(incomingPacket, len);
-        DataTransfer::endPacket();
-    }
-}
-
-void updateSensorState(Sensor * const sensor, Sensor * const sensor2) {
-    if(millis() - lastSensorInfoPacket > 1000) {
-        lastSensorInfoPacket = millis();
-        if(sensorStateNotified1 != sensor->getSensorState())
-            Network::sendSensorInfo(sensor);
-        if(sensorStateNotified2 != sensor2->getSensorState())
-            Network::sendSensorInfo(sensor2);
-    }
-}
-
-bool ServerConnection::isConnected() {
-    return connected;
-}
-
-void ServerConnection::connect()
-{
-    unsigned long now = millis();
-    while(true) {
-        int packetSize = Udp.parsePacket();
-        if (packetSize)
-        {
-            // receive incoming UDP packets
-            int len = Udp.read(incomingPacket, sizeof(incomingPacket));
-            
-#ifdef DEBUG_NETWORK
-            udpClientLogger.trace("Received %d bytes from %s, port %d", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
-            udpClientLogger.traceArray("UDP packet contents: ", incomingPacket, len);
-#endif
-
-            // Handshake is different, it has 3 in the first byte, not the 4th, and data starts right after
-            switch (incomingPacket[0])
-            {
-            case PACKET_HANDSHAKE:
-                // Assume handshake successful, don't check it
-                // But proper handshake should contain "Hey OVR =D 5" ASCII string right after the packet number
-                // Starting on 14th byte (packet number, 12 bytes greetings, null-terminator) we can transfer SlimeVR handshake data
-                host = Udp.remoteIP();
-                port = Udp.remotePort();
-                lastPacketMs = now;
-                connected = true;
-                statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
-                ledManager.off();
-                udpClientLogger.debug("Handshake successful, server is %s:%d", Udp.remoteIP().toString().c_str(), + Udp.remotePort());
-                return;
-            default:
-            continue;
-            }
-        }
-        else
-        {
-            break;
-        }
-    }
-    if(lastConnectionAttemptMs + 1000 < now)
-    {
-        lastConnectionAttemptMs = now;
-        udpClientLogger.info("Looking for the server...");
-        Network::sendHandshake();
-        ledManager.on();
-    }
-    else if(lastConnectionAttemptMs + 20 < now)
-    {
-        ledManager.off();
-    }
-}
-
-void ServerConnection::resetConnection() {
-    Udp.begin(port);
-    connected = false;
-
-    statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
-}
-
-void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
-    if(connected) {
-        int packetSize = Udp.parsePacket();
-        if (packetSize)
-        {
-            lastPacketMs = millis();
-            int len = Udp.read(incomingPacket, sizeof(incomingPacket));
-            // receive incoming UDP packets
-
-#ifdef DEBUG_NETWORK
-            udpClientLogger.trace("Received %d bytes from %s, port %d", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
-            udpClientLogger.traceArray("UDP packet contents: ", incomingPacket, len);
-#endif
-
-            switch (convert_chars<int>(incomingPacket))
-            {
-            case PACKET_RECEIVE_HEARTBEAT:
-                Network::sendHeartbeat();
-                break;
-            case PACKET_RECEIVE_VIBRATE:
-                
-                break;
-            case PACKET_RECEIVE_HANDSHAKE:
-                // Assume handshake successful
-                udpClientLogger.warn("Handshake received again, ignoring");
-                break;
-            case PACKET_RECEIVE_COMMAND:
-                
-                break;
-            case PACKET_CONFIG:
-                
-                break;
-            case PACKET_PING_PONG:
-                returnLastPacket(len);
-                break;
-            case PACKET_SENSOR_INFO:
-                if(len < 6) {
-                    udpClientLogger.warn("Wrong sensor info packet");
-                    break;
-                }
-                if(incomingPacket[4] == 0) {
-                    sensorStateNotified1 = incomingPacket[5];
-                } else if(incomingPacket[4] == 1) {
-                    sensorStateNotified2 = incomingPacket[5];
-                }
-                break;
-            }
-        }
-        //while(Serial.available()) {
-        //    size_t bytesRead = Serial.readBytes(serialBuffer, min(Serial.available(), sizeof(serialBuffer)));
-        //    sendSerial(serialBuffer, bytesRead, PACKET_SERIAL);
-        //}
-        if(lastPacketMs + TIMEOUT < millis())
-        {
-            statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
-
-            connected = false;
-            sensorStateNotified1 = false;
-            sensorStateNotified2 = false;
-            udpClientLogger.warn("Connection to server timed out");
-        }
-    }
-        
-    if(!connected) {
-        connect();
-    } else if(sensorStateNotified1 != sensor->isWorking() || sensorStateNotified2 != sensor2->isWorking()) {
-        updateSensorState(sensor, sensor2);
-    }
-}

src/network/udpclient.h

@@ -1,40 +0,0 @@
-/*
-    SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
-
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
-
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
-
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-#ifndef SLIMEVR_UDP_CLIENT_H_
-#define SLIMEVR_UDP_CLIENT_H_
-
-#include <WiFiUdp.h>
-#include <Arduino.h>
-#include "quat.h"
-#include "sensors/sensor.h"
-#include "wifihandler.h"
-#include "globals.h"
-
-namespace ServerConnection {
-    void connect();
-    void update(Sensor * const sensor, Sensor * const sensor2);
-    void resetConnection();
-    bool isConnected();
-}
-
-#endif // SLIMEVR_UDP_CLIENT_H_