Enforce clang-format on the repo (#355)

* Enforce clang-format on the repo

* improve CI config

* fix build error

* undo src format

* apply format by hand

* fix last one

* fix build error

* wat

.clang-tidy

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

.github/workflows/actions.yml

@@ -2,9 +2,29 @@ name: Build
 
 on:
   push:
+    branches:
+      - main
   pull_request:
 
 jobs:
+  format:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - uses: jidicula/clang-format-action@v4.13.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
 

src/GlobalVars.h

@@ -27,12 +27,12 @@
 #include <arduino-timer.h>
 
 #include "LEDManager.h"
+#include "batterymonitor.h"
 #include "configuration/Configuration.h"
 #include "network/connection.h"
 #include "network/manager.h"
 #include "sensors/SensorManager.h"
 #include "status/StatusManager.h"
-#include "batterymonitor.h"
 
 extern Timer<> globalTimer;
 extern SlimeVR::LEDManager ledManager;

src/LEDManager.cpp

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

src/LEDManager.h

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

src/batterymonitor.cpp

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

src/batterymonitor.h

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

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

@@ -1,327 +1,384 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
+#include "Configuration.h"
+
 #include <LittleFS.h>
 
-#include "Configuration.h"
+#include "../FSHelper.h"
 #include "consts.h"
 #include "utils.h"
-#include "../FSHelper.h"
 
 #define DIR_CALIBRATIONS "/calibrations"
 #define DIR_TEMPERATURE_CALIBRATIONS "/tempcalibrations"
 
 namespace SlimeVR {
-    namespace Configuration {
+namespace Configuration {
-        void Configuration::setup() {
+void Configuration::setup() {
-            if (m_Loaded) {
+	if (m_Loaded) {
-                return;
+		return;
-            }
+	}
 
-            bool status = LittleFS.begin();
+	bool status = LittleFS.begin();
-            if (!status) {
+	if (!status) {
-                this->m_Logger.warn("Could not mount LittleFS, formatting");
+		this->m_Logger.warn("Could not mount LittleFS, formatting");
 
-                status = LittleFS.format();
+		status = LittleFS.format();
-                if (!status) {
+		if (!status) {
-                    this->m_Logger.warn("Could not format LittleFS, aborting");
+			this->m_Logger.warn("Could not format LittleFS, aborting");
-                    return;
+			return;
-                }
+		}
 
-                status = LittleFS.begin();
+		status = LittleFS.begin();
-                if (!status) {
+		if (!status) {
-                    this->m_Logger.error("Could not mount LittleFS, aborting");
+			this->m_Logger.error("Could not mount LittleFS, aborting");
-                    return;
+			return;
-                }
+		}
-            }
+	}
 
-            if (LittleFS.exists("/config.bin")) {
+	if (LittleFS.exists("/config.bin")) {
-                m_Logger.trace("Found configuration file");
+		m_Logger.trace("Found configuration file");
 
-                auto 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) {
+		if (m_Config.version < CURRENT_CONFIGURATION_VERSION) {
-                    m_Logger.debug("Configuration is outdated: v%d < v%d", 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)) {
+			if (!runMigrations(m_Config.version)) {
-                        m_Logger.error("Failed to migrate configuration from v%d to v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION);
+				m_Logger.error(
-                        return;
+					"Failed to migrate configuration from v%d to v%d",
-                    }
+					m_Config.version,
-                } else {
+					CURRENT_CONFIGURATION_VERSION
-                    m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
+				);
-                }
+				return;
+			}
+		} else {
+			m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
+		}
 
-                file.seek(0);
+		file.seek(0);
-                file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
+		file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
-                file.close();
+		file.close();
-            } else {
+	} else {
-                m_Logger.info("No configuration file found, creating new one");
+		m_Logger.info("No configuration file found, creating new one");
-                m_Config.version = CURRENT_CONFIGURATION_VERSION;
+		m_Config.version = CURRENT_CONFIGURATION_VERSION;
-                save();
+		save();
-            }
+	}
 
-            loadSensors();
+	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_Sensors.size(); i++) {
-                SensorConfig config = m_Sensors[i];
-                if (config.type == SensorConfigType::NONE) {
-                    continue;
-                }
-
-                char path[17];
-                sprintf(path, DIR_CALIBRATIONS"/%d", 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();
-            }
-
-            {
-                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_Sensors.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;
-        }
-
-		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
-				);
-
-				setSensor(sensorId, sensorConfig);
-			});
-		}
-
-		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;
-                }
-            }
-        }
-    }
 }
+
+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 "/%d", 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();
+	}
+
+	{
+		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_Sensors.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;
+}
+
+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
+		);
+
+		setSensor(sensorId, sensorConfig);
+	});
+}
+
+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 Configuration
+}  // namespace SlimeVR

src/configuration/Configuration.h

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

src/consts.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_CONSTS_H_
 #define SLIMEVR_CONSTS_H_
@@ -26,23 +26,23 @@
 // List of constants used in other places
 
 enum class ImuID {
-    Unknown = 0,
+	Unknown = 0,
-    MPU9250,
+	MPU9250,
-    MPU6500,
+	MPU6500,
-    BNO080,
+	BNO080,
-    BNO085,
+	BNO085,
-    BNO055,
+	BNO055,
-    MPU6050,
+	MPU6050,
-    BNO086,
+	BNO086,
-    BMI160,
+	BMI160,
-    ICM20948,
+	ICM20948,
-    ICM42688,
+	ICM42688,
-    BMI270,
+	BMI270,
-    LSM6DS3TRC,
+	LSM6DS3TRC,
-    LSM6DSV,
+	LSM6DSV,
-    LSM6DSO,
+	LSM6DSO,
-    LSM6DSR,
+	LSM6DSR,
-    Empty = 255
+	Empty = 255
 };
 
 #define IMU_UNKNOWN ErroneousSensor
@@ -63,7 +63,7 @@ enum class ImuID {
 #define IMU_LSM6DSR SoftFusionLSM6DSR
 #define IMU_MPU6050_SF SoftFusionMPU6050
 
-#define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+#define IMU_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
 
 #define BOARD_UNKNOWN 0
 #define BOARD_SLIMEVR_LEGACY 1
@@ -78,13 +78,13 @@ enum class ImuID {
 #define BOARD_LOLIN_C3_MINI 10
 #define BOARD_BEETLE32C3 11
 #define BOARD_ES32C3DEVKITM1 12
-#define BOARD_OWOTRACK 13 // Only used by owoTrack mobile app
+#define BOARD_OWOTRACK 13  // Only used by owoTrack mobile app
-#define BOARD_WRANGLER 14 // Only used by wrangler app
+#define BOARD_WRANGLER 14  // Only used by wrangler app
-#define BOARD_MOCOPI 15 // Used by mocopi/moslime
+#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_HARITORA 18  // Used by Haritora/SlimeTora
-#define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+#define BOARD_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
 
 #define BAT_EXTERNAL 1
 #define BAT_INTERNAL 2
@@ -93,25 +93,28 @@ enum class ImuID {
 
 #define LED_OFF 255
 
-#define POWER_SAVING_LEGACY 0 // No sleeping, but PS enabled
+#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_NONE 1  // No sleeping, no PS => for connection issues
-#define POWER_SAVING_MINIMUM 2 // Sleeping and PS => default
+#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_MODERATE \
-#define POWER_SAVING_MAXIMUM 4 // Actual CPU sleeping, currently has issues with disconnecting
+	3  // Sleeping and better PS => might miss broadcasts, use at own risk
+#define POWER_SAVING_MAXIMUM \
+	4  // Actual CPU sleeping, currently has issues with disconnecting
 
 // Send rotation/acceleration data as separate frames.
 // PPS: 1470 @ 5+1, 1960 @ 5+3
 #define PACKET_BUNDLING_DISABLED 0
 // Less packets. Pack data per sensor and send asap.
-// Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3 setups.
+// Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3
-// PPS: 680 @ 5+1, 740 @ 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.
+// Even less packets, if more than 1 sensor - wait for data from all sensors or until
-// Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for 5+1, by ~15% for 5+3 setups.
+// timeout, then send. Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for
-// PPS: 650 @ 5+1, 650 @ 5+3
+// 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°)
+// 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)
@@ -131,22 +134,22 @@ enum class ImuID {
 #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_OWOTRACK_ANDROID 3  // Only used by owoTrack mobile app
-#define MCU_WRANGLER 4 // Only used by wrangler app
+#define MCU_WRANGLER 4  // Only used by wrangler app
-#define MCU_OWOTRACK_IOS 5 // Only used by owoTrack mobile 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_MOCOPI 7  // Used by mocopi/moslime
-#define MCU_HARITORA 8 // Used by Haritora/SlimeTora
+#define MCU_HARITORA 8  // Used by Haritora/SlimeTora
-#define MCU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
+#define MCU_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
 
 #ifdef ESP8266
-  #define HARDWARE_MCU MCU_ESP8266
+#define HARDWARE_MCU MCU_ESP8266
 #elif defined(ESP32)
-  #define HARDWARE_MCU MCU_ESP32
+#define HARDWARE_MCU MCU_ESP32
 #else
-  #define HARDWARE_MCU MCU_UNKNOWN
+#define HARDWARE_MCU MCU_UNKNOWN
 #endif
 
 #define CURRENT_CONFIGURATION_VERSION 1
 
-#endif // SLIMEVR_CONSTS_H_
+#endif  // SLIMEVR_CONSTS_H_

src/credentials.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	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,50 +1,55 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	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 IMU_MPU6050_RUNTIME_CALIBRATION  // Comment to revert to
-#define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs
+										 // startup/traditional-calibration
-#define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently)
+#define BNO_USE_ARVR_STABILIZATION \
-#define LOAD_BIAS true // Loads the bias values from NVS on start
+	true  // Set to false to disable stabilization for BNO085+ IMUs
-#define SAVE_BIAS true // Periodically saves bias calibration data to NVS
+#define USE_6_AXIS \
-#define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only)
+	true  // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently)
-#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 LOAD_BIAS true  // Loads the bias values from NVS on start
-#define SEND_ACCELERATION true // send linear acceleration to the server
+#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
+// Debug information
 
 #define LOG_LEVEL LOG_LEVEL_DEBUG
 
 #if LOG_LEVEL == LOG_LEVEL_TRACE
-    #define DEBUG_SENSOR
+#define DEBUG_SENSOR
-    #define DEBUG_NETWORK
+#define DEBUG_NETWORK
-    #define DEBUG_CONFIGURATION
+#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
@@ -55,7 +60,7 @@
 // Sleeping options
 #define POWERSAVING_MODE POWER_SAVING_LEGACY  // Minimum causes sporadic data pauses
 #if POWERSAVING_MODE >= POWER_SAVING_MINIMUM
-    #define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000)
+#define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000)
 #endif
 
 // Packet bundling/aggregation
@@ -69,7 +74,7 @@
 // Battery configuration
 #define batterySampleRate 10000
 #define BATTERY_LOW_VOLTAGE_DEEP_SLEEP false
-#define BATTERY_LOW_POWER_VOLTAGE 3.3f // Voltage to raise error
+#define BATTERY_LOW_POWER_VOLTAGE 3.3f  // Voltage to raise error
 
 // Send updates over network only when changes are substantial
 // If "false" updates are sent at the sensor update rate (usually 100 TPS)
@@ -80,7 +85,7 @@
 #define I2C_SPEED 400000
 
 #define COMPLIANCE_MODE true
-#define USE_ATTENUATION COMPLIANCE_MODE && ESP8266
+#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
@@ -92,4 +97,4 @@
 #define PROTOCOL_VERSION 18
 #define FIRMWARE_VERSION "0.5.0"
 
-#endif // SLIMEVR_DEBUG_H_
+#endif  // SLIMEVR_DEBUG_H_

src/defines.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 // ================================================
 // See docs for configuration options and examples:
@@ -40,16 +40,33 @@
 // 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, \
+#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN,
-                                       AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)
+AXIS_REMAP_USE_Z, \ AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)
 
-IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
+IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL,
+PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
 */
 
 #ifndef IMU_DESC_LIST
-#define IMU_DESC_LIST \
+#define IMU_DESC_LIST              \
-        IMU_DESC_ENTRY(IMU,        PRIMARY_IMU_ADDRESS_ONE,   IMU_ROTATION,        PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL,   PIN_IMU_INT) \
+	IMU_DESC_ENTRY(                \
-        IMU_DESC_ENTRY(SECOND_IMU, SECONDARY_IMU_ADDRESS_TWO, SECOND_IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, SECONDARY_IMU_OPTIONAL, PIN_IMU_INT_2)
+		IMU,                       \
+		PRIMARY_IMU_ADDRESS_ONE,   \
+		IMU_ROTATION,              \
+		PIN_IMU_SCL,               \
+		PIN_IMU_SDA,               \
+		PRIMARY_IMU_OPTIONAL,      \
+		PIN_IMU_INT                \
+	)                              \
+	IMU_DESC_ENTRY(                \
+		SECOND_IMU,                \
+		SECONDARY_IMU_ADDRESS_TWO, \
+		SECOND_IMU_ROTATION,       \
+		PIN_IMU_SCL,               \
+		PIN_IMU_SDA,               \
+		SECONDARY_IMU_OPTIONAL,    \
+		PIN_IMU_INT_2              \
+	)
 #endif
 
 // Battery monitoring options (comment to disable):
@@ -59,13 +76,15 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
 #define BATTERY_MONITOR BAT_EXTERNAL
 
 // BAT_EXTERNAL definition override
-// D1 Mini boards with ESP8266 have internal resistors. For these boards you only have to adjust BATTERY_SHIELD_RESISTANCE.
+// D1 Mini boards with ESP8266 have internal resistors. For these boards you only have
-// For other boards you can now adjust the other resistor values.
+// to adjust BATTERY_SHIELD_RESISTANCE. For other boards you can now adjust the other
-// The diagram looks like this:
+// resistor values. The diagram looks like this:
-//   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2] ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
+//   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2]
-// #define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in external resistor value in kOhm
+//   ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
-// #define BATTERY_SHIELD_R1 100 // Board voltage divider resistor Ain to GND in kOhm
+// #define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in
-// #define BATTERY_SHIELD_R2 220 // Board voltage divider resistor Ain to INPUT_BOARD in kOhm
+// 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:
@@ -81,129 +100,131 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
 
 // Board-specific configurations
 #if BOARD == BOARD_SLIMEVR
-  #define PIN_IMU_SDA 14
+#define PIN_IMU_SDA 14
-  #define PIN_IMU_SCL 12
+#define PIN_IMU_SCL 12
-  #define PIN_IMU_INT 16
+#define PIN_IMU_INT 16
-  #define PIN_IMU_INT_2 13
+#define PIN_IMU_INT_2 13
-  #define PIN_BATTERY_LEVEL 17
+#define PIN_BATTERY_LEVEL 17
-  #define LED_PIN 2
+#define LED_PIN 2
-  #define LED_INVERTED true
+#define LED_INVERTED true
-  #ifndef BATTERY_SHIELD_RESISTANCE
+#ifndef BATTERY_SHIELD_RESISTANCE
-    #define BATTERY_SHIELD_RESISTANCE 0
+#define BATTERY_SHIELD_RESISTANCE 0
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R1
+#ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 10
+#define BATTERY_SHIELD_R1 10
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R2
+#ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 40.2
+#define BATTERY_SHIELD_R2 40.2
-  #endif
+#endif
 #elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV
-  #define PIN_IMU_SDA 4
+#define PIN_IMU_SDA 4
-  #define PIN_IMU_SCL 5
+#define PIN_IMU_SCL 5
-  #define PIN_IMU_INT 10
+#define PIN_IMU_INT 10
-  #define PIN_IMU_INT_2 13
+#define PIN_IMU_INT_2 13
-  #define PIN_BATTERY_LEVEL 17
+#define PIN_BATTERY_LEVEL 17
-  #define LED_PIN 2
+#define LED_PIN 2
-  #define LED_INVERTED true
+#define LED_INVERTED true
-  #ifndef BATTERY_SHIELD_RESISTANCE
+#ifndef BATTERY_SHIELD_RESISTANCE
-    #define BATTERY_SHIELD_RESISTANCE 0
+#define BATTERY_SHIELD_RESISTANCE 0
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R1
+#ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 10
+#define BATTERY_SHIELD_R1 10
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R2
+#ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 40.2
+#define BATTERY_SHIELD_R2 40.2
-  #endif
+#endif
 #elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI
-  #define PIN_IMU_SDA D2
+#define PIN_IMU_SDA D2
-  #define PIN_IMU_SCL D1
+#define PIN_IMU_SCL D1
-  #define PIN_IMU_INT D5
+#define PIN_IMU_INT D5
-  #define PIN_IMU_INT_2 D6
+#define PIN_IMU_INT_2 D6
-  #define PIN_BATTERY_LEVEL A0
+#define PIN_BATTERY_LEVEL A0
 //  #define LED_PIN 2
 //  #define LED_INVERTED true
-  #ifndef BATTERY_SHIELD_RESISTANCE
+#ifndef BATTERY_SHIELD_RESISTANCE
-    #define BATTERY_SHIELD_RESISTANCE 180
+#define BATTERY_SHIELD_RESISTANCE 180
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R1
+#ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 100
+#define BATTERY_SHIELD_R1 100
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R2
+#ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 220
+#define BATTERY_SHIELD_R2 220
-  #endif
+#endif
 #elif BOARD == BOARD_ESP01
-  #define PIN_IMU_SDA 2
+#define PIN_IMU_SDA 2
-  #define PIN_IMU_SCL 0
+#define PIN_IMU_SCL 0
-  #define PIN_IMU_INT 255
+#define PIN_IMU_INT 255
-  #define PIN_IMU_INT_2 255
+#define PIN_IMU_INT_2 255
-  #define PIN_BATTERY_LEVEL 255
+#define PIN_BATTERY_LEVEL 255
-  #define LED_PIN LED_OFF
+#define LED_PIN LED_OFF
-  #define LED_INVERTED false
+#define LED_INVERTED false
 #elif BOARD == BOARD_TTGO_TBASE
-  #define PIN_IMU_SDA 5
+#define PIN_IMU_SDA 5
-  #define PIN_IMU_SCL 4
+#define PIN_IMU_SCL 4
-  #define PIN_IMU_INT 14
+#define PIN_IMU_INT 14
-  #define PIN_IMU_INT_2 13
+#define PIN_IMU_INT_2 13
-  #define PIN_BATTERY_LEVEL A0
+#define PIN_BATTERY_LEVEL A0
 //  #define LED_PIN 2
 //  #define LED_INVERTED false
 #elif BOARD == BOARD_CUSTOM
-  // Define pins by the examples above
+// Define pins by the examples above
 #elif BOARD == BOARD_WROOM32
-  #define PIN_IMU_SDA 21
+#define PIN_IMU_SDA 21
-  #define PIN_IMU_SCL 22
+#define PIN_IMU_SCL 22
-  #define PIN_IMU_INT 23
+#define PIN_IMU_INT 23
-  #define PIN_IMU_INT_2 25
+#define PIN_IMU_INT_2 25
-  #define PIN_BATTERY_LEVEL 36
+#define PIN_BATTERY_LEVEL 36
 //  #define LED_PIN 2
 //  #define LED_INVERTED false
 #elif BOARD == BOARD_LOLIN_C3_MINI
-  #define PIN_IMU_SDA 5
+#define PIN_IMU_SDA 5
-  #define PIN_IMU_SCL 4
+#define PIN_IMU_SCL 4
-  #define PIN_IMU_INT 6
+#define PIN_IMU_INT 6
-  #define PIN_IMU_INT_2 8
+#define PIN_IMU_INT_2 8
-  #define PIN_BATTERY_LEVEL 3
+#define PIN_BATTERY_LEVEL 3
-  #define LED_PIN 7
+#define LED_PIN 7
 //  #define LED_INVERTED false
 #elif BOARD == BOARD_BEETLE32C3
-  #define PIN_IMU_SDA 8
+#define PIN_IMU_SDA 8
-  #define PIN_IMU_SCL 9
+#define PIN_IMU_SCL 9
-  #define PIN_IMU_INT 6
+#define PIN_IMU_INT 6
-  #define PIN_IMU_INT_2 7
+#define PIN_IMU_INT_2 7
-  #define PIN_BATTERY_LEVEL 3
+#define PIN_BATTERY_LEVEL 3
-  #define LED_PIN 10
+#define LED_PIN 10
-  #define LED_INVERTED false
+#define LED_INVERTED false
 #elif BOARD == BOARD_ES32C3DEVKITM1
-  #define PIN_IMU_SDA 5
+#define PIN_IMU_SDA 5
-  #define PIN_IMU_SCL 4
+#define PIN_IMU_SCL 4
-  #define PIN_IMU_INT 6
+#define PIN_IMU_INT 6
-  #define PIN_IMU_INT_2 7
+#define PIN_IMU_INT_2 7
-  #define PIN_BATTERY_LEVEL 3
+#define PIN_BATTERY_LEVEL 3
-  #define LED_PIN LED_OFF  // RGB LED Protocol would need to be implementetet did not brother for the test, because the board ideal for tracker ifself
+#define LED_PIN \
+	LED_OFF  // RGB LED Protocol would need to be implementetet did not brother for the
+			 // test, because the board ideal for tracker ifself
 //  #define LED_INVERTED false
 #elif BOARD == BOARD_WEMOSWROOM02
-  #define PIN_IMU_SDA 2
+#define PIN_IMU_SDA 2
-  #define PIN_IMU_SCL 14
+#define PIN_IMU_SCL 14
-  #define PIN_IMU_INT 0
+#define PIN_IMU_INT 0
-  #define PIN_IMU_INT_2 4
+#define PIN_IMU_INT_2 4
-  #define PIN_BATTERY_LEVEL A0
+#define PIN_BATTERY_LEVEL A0
-  #define LED_PIN 16
+#define LED_PIN 16
-  #define LED_INVERTED true
+#define LED_INVERTED true
 #elif BOARD == BOARD_XIAO_ESP32C3
-  #define PIN_IMU_SDA 6 // D4
+#define PIN_IMU_SDA 6  // D4
-  #define PIN_IMU_SCL 7 // D5
+#define PIN_IMU_SCL 7  // D5
-  #define PIN_IMU_INT 5 // D3
+#define PIN_IMU_INT 5  // D3
-  #define PIN_IMU_INT_2 10 // D10
+#define PIN_IMU_INT_2 10  // D10
-  #define LED_PIN 4 // D2
+#define LED_PIN 4  // D2
-  #define LED_INVERTED false
+#define LED_INVERTED false
-  #define PIN_BATTERY_LEVEL 2 // D0 / A0
+#define PIN_BATTERY_LEVEL 2  // D0 / A0
-  #ifndef BATTERY_SHIELD_RESISTANCE
+#ifndef BATTERY_SHIELD_RESISTANCE
-    #define BATTERY_SHIELD_RESISTANCE 0
+#define BATTERY_SHIELD_RESISTANCE 0
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R1
+#ifndef BATTERY_SHIELD_R1
-    #define BATTERY_SHIELD_R1 100
+#define BATTERY_SHIELD_R1 100
-  #endif
+#endif
-  #ifndef BATTERY_SHIELD_R2
+#ifndef BATTERY_SHIELD_R2
-    #define BATTERY_SHIELD_R2 100
+#define BATTERY_SHIELD_R2 100
-  #endif
+#endif
 #endif

src/defines_bmi160.h

@@ -1,32 +1,31 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 SlimeVR Contributors
+	Copyright (c) 2022 SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #ifndef BMI160_DEFINES_H
 #define BMI160_DEFINES_H
 
-// BMI160 magnetometer type, applies to both main and aux trackers, mixed types are not supported currently.
+// BMI160 magnetometer type, applies to both main and aux trackers, mixed types are not
-// If only 1 out of 2 trackers has a mag, tracker without a mag should still function normally.
+// supported currently. If only 1 out of 2 trackers has a mag, tracker without a mag
-// NOT USED if USE_6_AXIS == true
+// should still function normally. NOT USED if USE_6_AXIS == true Pick one:
-// Pick one:
 #define BMI160_MAG_TYPE BMI160_MAG_TYPE_HMC
 // #define BMI160_MAG_TYPE BMI160_MAG_TYPE_QMC
 
@@ -55,11 +54,9 @@
 // #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_ROTATION
 #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_6POINT
 
-// How long to run magnetometer calibration for, if enabled and you have added a magnetometer.
+// How long to run magnetometer calibration for, if enabled and you have added a
-// Magnetometer not be used until you calibrate it.
+// magnetometer. Magnetometer not be used until you calibrate it. Disables this
-// Disables this calibration step if value is 0.
+// calibration step if value is 0. NOT USED if USE_6_AXIS == true Default: 20
-// NOT USED if USE_6_AXIS == true
-// Default: 20
 #define BMI160_CALIBRATION_MAG_SECONDS 20
 
 // Send temperature to the server as AXXYY,

src/defines_sensitivity.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 SlimeVR Contributors
+	Copyright (c) 2022 SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef DEFINES_SENSITIVITY_H
@@ -47,14 +47,46 @@
 
 #define SENSORID_PRIMARY 0
 #define SENSORID_AUX 1
-struct SensitivityOffsetXYZ { const char* mac; unsigned char sensorId; double spins; double x; double y; double z; };
+struct SensitivityOffsetXYZ {
+	const char* mac;
+	unsigned char sensorId;
+	double spins;
+	double x;
+	double y;
+	double z;
+};
 const SensitivityOffsetXYZ sensitivityOffsets[] = {
-    // example values
+	// example values
-    { .mac = "A4:E5:7C:B6:00:01", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 },
+	{.mac = "A4:E5:7C:B6:00:01",
-    { .mac = "A4:E5:7C:B6:00:02", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 },
+	 .sensorId = SENSORID_PRIMARY,
-    { .mac = "A4:E5:7C:B6:00:03", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 11, .y =  2.2, .z =  -1 },
+	 .spins = 10,
-    { .mac = "A4:E5:7C:B6:00:04", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 },
+	 .x = 2.63,
-    { .mac = "A4:E5:7C:B6:00:05", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -10.63, .y = -8.25, .z = -18.6 },
+	 .y = 37.82,
+	 .z = 31.11},
+	{.mac = "A4:E5:7C:B6:00:02",
+	 .sensorId = SENSORID_PRIMARY,
+	 .spins = 10,
+	 .x = -2.38,
+	 .y = -26.8,
+	 .z = -42.78},
+	{.mac = "A4:E5:7C:B6:00:03",
+	 .sensorId = SENSORID_PRIMARY,
+	 .spins = 10,
+	 .x = 11,
+	 .y = 2.2,
+	 .z = -1},
+	{.mac = "A4:E5:7C:B6:00:04",
+	 .sensorId = SENSORID_PRIMARY,
+	 .spins = 10,
+	 .x = -7,
+	 .y = -53.7,
+	 .z = -57},
+	{.mac = "A4:E5:7C:B6:00:05",
+	 .sensorId = SENSORID_PRIMARY,
+	 .spins = 10,
+	 .x = -10.63,
+	 .y = -8.25,
+	 .z = -18.6},
 };
 
 #endif

src/globals.h

@@ -1,29 +1,30 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_GLOBALS_H_
 #define SLIMEVR_GLOBALS_H_
 
 #include <Arduino.h>
+
 #include "consts.h"
 #include "debug.h"
 #include "defines.h"
@@ -42,29 +43,29 @@
 #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 LED_PIN is not defined in "defines.h" take the default pin from "pins_arduino.h"
-// If there is no pin defined for the board, use LED_PIN 255 and disable LED
+// 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
+// LED_PIN is defined
-    #if (LED_PIN < 0) || (LED_PIN >= LED_OFF)
+#if (LED_PIN < 0) || (LED_PIN >= LED_OFF)
-        #define ENABLE_LEDS false    
+#define ENABLE_LEDS false
-    #else
-        #define ENABLE_LEDS true
-    #endif
 #else
-    // LED_PIN is not defined
+#define ENABLE_LEDS true
-    #if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0)
+#endif
-        #define LED_PIN LED_BUILTIN
+#else
-        #define ENABLE_LEDS true
+// LED_PIN is not defined
-    #else
+#if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0)
-        #define LED_PIN LED_OFF
+#define LED_PIN LED_BUILTIN
-        #define ENABLE_LEDS false
+#define ENABLE_LEDS true
-    #endif
+#else
+#define LED_PIN LED_OFF
+#define ENABLE_LEDS false
+#endif
 #endif
 
 #if !defined(LED_INVERTED)
-    // default is inverted for SlimeVR / ESP-12E
+// default is inverted for SlimeVR / ESP-12E
-    #define LED_INVERTED true
+#define LED_INVERTED true
 #endif
 
 #if LED_INVERTED
@@ -75,4 +76,4 @@
 #define LED__OFF LOW
 #endif
 
-#endif // SLIMEVR_GLOBALS_H_
+#endif  // SLIMEVR_GLOBALS_H_

src/logging/Level.cpp

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

src/logging/Level.h

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

src/logging/Logger.cpp

@@ -1,82 +1,70 @@
 #include "Logger.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Logging {
-  namespace Logging
+void Logger::setTag(const char* tag) {
-  {
+	m_Tag = (char*)malloc(strlen(tag) + 1);
-    void Logger::setTag(const char *tag)
+	strcpy(m_Tag, 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);
-    }
-  }
 }
+
+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 Logging
+}  // namespace SlimeVR

src/logging/Logger.h

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

src/main.cpp

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

src/motionprocessing/GyroTemperatureCalibrator.cpp

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

src/motionprocessing/GyroTemperatureCalibrator.h

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

src/motionprocessing/OnlinePolyfit.h

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

src/motionprocessing/RestDetection.h

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

src/motionprocessing/types.h

@@ -2,10 +2,10 @@
 #define MOTIONPROCESSING_TYPES_H
 
 #if SENSORS_DOUBLE_PRECISION
-    typedef double sensor_real_t;
+typedef double sensor_real_t;
 #else
-    typedef float sensor_real_t;
+typedef float sensor_real_t;
-    #define VQF_SINGLE_PRECISION
+#define VQF_SINGLE_PRECISION
 #endif
 
 #endif

src/network/connection.cpp

@@ -134,7 +134,7 @@ bool Connection::endBundle() {
 	return endPacket();
 }
 
-size_t Connection::write(const uint8_t *buffer, size_t size) {
+size_t Connection::write(const uint8_t* buffer, size_t size) {
 	if (m_IsBundle) {
 		if (m_BundlePacketPosition + size > sizeof(m_Packet)) {
 			return 0;
@@ -146,9 +146,7 @@ size_t Connection::write(const uint8_t *buffer, size_t size) {
 	return m_UDP.write(buffer, size);
 }
 
-size_t Connection::write(uint8_t byte) {
+size_t Connection::write(uint8_t byte) { return write(&byte, 1); }
-	return write(&byte, 1);
-}
 
 bool Connection::sendFloat(float f) {
 	convert_to_chars(f, m_Buf);
@@ -527,7 +525,7 @@ void Connection::returnLastPacket(int len) {
 	MUST(endPacket());
 }
 
-void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>> & sensors) {
+void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors) {
 	if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
 		return;
 	}
@@ -591,7 +589,7 @@ void Connection::searchForServer() {
 			m_Connected = true;
 
 			m_FeatureFlagsRequestAttempts = 0;
-			m_ServerFeatures = ServerFeatures { };
+			m_ServerFeatures = ServerFeatures{};
 
 			statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
 			ledManager.off();
@@ -621,7 +619,11 @@ void Connection::searchForServer() {
 
 void Connection::reset() {
 	m_Connected = false;
-	std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
+	std::fill(
+		m_AckedSensorState,
+		m_AckedSensorState + MAX_IMU_COUNT,
+		SensorStatus::SENSOR_OFFLINE
+	);
 
 	m_UDP.begin(m_ServerPort);
 
@@ -629,7 +631,7 @@ void Connection::reset() {
 }
 
 void Connection::update() {
-	auto & sensors = sensorManager.getSensors();
+	auto& sensors = sensorManager.getSensors();
 
 	updateSensorState(sensors);
 	maybeRequestFeatureFlags();
@@ -643,7 +645,11 @@ void Connection::update() {
 		statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
 
 		m_Connected = false;
-		std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
+		std::fill(
+			m_AckedSensorState,
+			m_AckedSensorState + MAX_IMU_COUNT,
+			SensorStatus::SENSOR_OFFLINE
+		);
 		m_Logger.warn("Connection to server timed out");
 
 		return;
@@ -720,18 +726,19 @@ void Connection::update() {
 			m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength);
 
 			if (!hadFlags) {
-				#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
+#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
-					if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) {
+				if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) {
-						m_Logger.debug("Server supports packet bundling");
+					m_Logger.debug("Server supports packet bundling");
-					}
+				}
-				#endif
+#endif
 			}
 
 			break;
 		}
 
 		case PACKET_SET_CONFIG_FLAG: {
-			// Packet type (4) + Packet number (8) + sensor_id(1) + flag_id (2) + state (1)
+			// 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;
@@ -739,17 +746,18 @@ void Connection::update() {
 			uint8_t sensorId = m_Packet[12];
 			uint16_t flagId = m_Packet[13] << 8 | m_Packet[14];
 			bool newState = m_Packet[15] > 0;
-			if(sensorId == UINT8_MAX) {
+			if (sensorId == UINT8_MAX) {
 				for (auto& sensor : sensors) {
 					sensor->setFlag(flagId, newState);
 				}
 			} else {
-				auto & sensors = sensorManager.getSensors();
+				auto& sensors = sensorManager.getSensors();
-				if(sensorId < sensors.size()) {
+				if (sensorId < sensors.size()) {
 					auto& sensor = sensors[sensorId];
 					sensor->setFlag(flagId, newState);
 				} else {
-					m_Logger.warn("Invalid sensor config flag packet: invalid sensor id");
+					m_Logger.warn("Invalid sensor config flag packet: invalid sensor id"
+					);
 					break;
 				}
 			}

src/network/connection.h

@@ -26,11 +26,11 @@
 #include <Arduino.h>
 #include <WiFiUdp.h>
 
+#include "featureflags.h"
 #include "globals.h"
 #include "quat.h"
 #include "sensors/sensor.h"
 #include "wifihandler.h"
-#include "featureflags.h"
 
 namespace SlimeVR {
 namespace Network {
@@ -117,21 +117,19 @@ public:
 	);
 #endif
 
-	const ServerFeatures& getServerFeatureFlags() {
+	const ServerFeatures& getServerFeatureFlags() { return m_ServerFeatures; }
-		return m_ServerFeatures;
-	}
 
 	bool beginBundle();
 	bool endBundle();
 
 private:
-	void updateSensorState(std::vector<std::unique_ptr<Sensor>> & sensors);
+	void updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors);
 	void maybeRequestFeatureFlags();
 
 	bool beginPacket();
 	bool endPacket();
 
-	size_t write(const uint8_t *buffer, size_t size);
+	size_t write(const uint8_t* buffer, size_t size);
 	size_t write(uint8_t byte);
 
 	bool sendPacketType(uint8_t type);

src/network/featureflags.h

@@ -24,17 +24,17 @@
 #ifndef SLIMEVR_FEATURE_FLAGS_H_
 #define SLIMEVR_FEATURE_FLAGS_H_
 
-#include <cstring>
 #include <algorithm>
+#include <cstring>
 
 /**
  * Bit packed flags, enum values start with 0 and indicate which bit it is.
  *
  * Change the enums and `flagsEnabled` inside to extend.
-*/
+ */
 struct ServerFeatures {
 public:
-	enum EServerFeatureFlags: uint32_t {
+	enum EServerFeatureFlags : uint32_t {
 		// Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64).
 		PROTOCOL_BUNDLE_SUPPORT,
 
@@ -51,15 +51,17 @@ public:
 	/**
 	 * Whether the server supports the "feature flags" feature,
 	 * set to true when we've received flags packet from the server.
-	*/
+	 */
-	bool isAvailable() {
+	bool isAvailable() { return m_Available; }
-		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));
+		memcpy(
+			res.m_Flags,
+			received,
+			std::min(static_cast<uint32_t>(sizeof(res.m_Flags)), length)
+		);
 		return res;
 	}
 
@@ -71,7 +73,7 @@ private:
 
 class FirmwareFeatures {
 public:
-	enum EFirmwareFeatureFlags: uint32_t {
+	enum EFirmwareFeatureFlags : uint32_t {
 		// EXAMPLE_FEATURE,
 		B64_WIFI_SCANNING = 1,
 		SENSOR_CONFIG = 2,
@@ -88,7 +90,7 @@ public:
 		// Add enabled flags here
 	};
 
-	static constexpr auto flags = []{
+	static constexpr auto flags = [] {
 		constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1;
 		std::array<uint8_t, flagsLength> packed{};
 

src/network/packets.h

@@ -47,7 +47,8 @@
 #define PACKET_TEMPERATURE 20
 // #define PACKET_USER_ACTION 21 // Joycon buttons only currently
 #define PACKET_FEATURE_FLAGS 22
-// #define PACKET_ROTATION_ACCELERATION 23 // Unification of rot and accel data in one packet
+// #define PACKET_ROTATION_ACCELERATION 23 // Unification of rot and accel data in one
+// packet
 #define PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24
 #define PACKET_SET_CONFIG_FLAG 25
 

src/network/wifihandler.cpp

@@ -1,28 +1,28 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
+#include "GlobalVars.h"
 #include "globals.h"
 #include "logging/Logger.h"
-#include "GlobalVars.h"
 #if !ESP8266
 #include "esp_wifi.h"
 #endif
@@ -38,211 +38,239 @@ unsigned long last_rssi_sample = 0;
 SlimeVR::Logging::Logger wifiHandlerLogger("WiFiHandler");
 
 void reportWifiError() {
-    if(lastWifiReportTime + 1000 < millis()) {
+	if (lastWifiReportTime + 1000 < millis()) {
-        lastWifiReportTime = millis();
+		lastWifiReportTime = millis();
-        Serial.print(".");
+		Serial.print(".");
-    }
+	}
 }
 
 void setStaticIPIfDefined() {
-    #ifdef WIFI_USE_STATICIP
+#ifdef WIFI_USE_STATICIP
-    const IPAddress ip(WIFI_STATIC_IP);
+	const IPAddress ip(WIFI_STATIC_IP);
-    const IPAddress gateway(WIFI_STATIC_GATEWAY);
+	const IPAddress gateway(WIFI_STATIC_GATEWAY);
-    const IPAddress subnet(WIFI_STATIC_SUBNET);
+	const IPAddress subnet(WIFI_STATIC_SUBNET);
-    WiFi.config(ip, gateway, subnet);
+	WiFi.config(ip, gateway, subnet);
-    #endif
+#endif
 }
 
-bool WiFiNetwork::isConnected() {
+bool WiFiNetwork::isConnected() { return isWifiConnected; }
-    return isWifiConnected;
+
+void WiFiNetwork::setWiFiCredentials(const char* SSID, const char* pass) {
+	stopProvisioning();
+	setStaticIPIfDefined();
+	WiFi.begin(SSID, pass);
+	// Reset state, will get back into provisioning if can't connect
+	hadWifi = false;
+	wifiState = SLIME_WIFI_SERVER_CRED_ATTEMPT;
+	wifiConnectionTimeout = millis();
 }
 
-void WiFiNetwork::setWiFiCredentials(const char * SSID, const char * pass) {
+IPAddress WiFiNetwork::getAddress() { return WiFi.localIP(); }
-    stopProvisioning();
-    setStaticIPIfDefined();
-    WiFi.begin(SSID, pass);
-    // Reset state, will get back into provisioning if can't connect
-    hadWifi = false;
-    wifiState = SLIME_WIFI_SERVER_CRED_ATTEMPT;
-    wifiConnectionTimeout = millis();
-}
-
-IPAddress WiFiNetwork::getAddress() {
-    return WiFi.localIP();
-}
 
 void WiFiNetwork::setUp() {
-    wifiHandlerLogger.info("Setting up WiFi");
+	wifiHandlerLogger.info("Setting up WiFi");
-    WiFi.persistent(true);
+	WiFi.persistent(true);
-    WiFi.mode(WIFI_STA);
+	WiFi.mode(WIFI_STA);
-    #if ESP8266
+#if ESP8266
-        #if USE_ATTENUATION
+#if USE_ATTENUATION
-            WiFi.setOutputPower(20.0 -  ATTENUATION_N);
+	WiFi.setOutputPower(20.0 - ATTENUATION_N);
-        #endif
+#endif
-    WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+	WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-    #endif
+#endif
-    WiFi.hostname("SlimeVR FBT Tracker");
+	WiFi.hostname("SlimeVR FBT Tracker");
-    wifiHandlerLogger.info("Loaded credentials for SSID %s and pass length %d", WiFi.SSID().c_str(), WiFi.psk().length());
+	wifiHandlerLogger.info(
-    setStaticIPIfDefined();
+		"Loaded credentials for SSID %s and pass length %d",
-    wl_status_t status = WiFi.begin(); // Should connect to last used access point, see https://arduino-esp8266.readthedocs.io/en/latest/esp8266wifi/station-class.html#begin
+		WiFi.SSID().c_str(),
-    wifiHandlerLogger.debug("Status: %d", status);
+		WiFi.psk().length()
-    wifiState = SLIME_WIFI_SAVED_ATTEMPT;
+	);
-    wifiConnectionTimeout = millis();
+	setStaticIPIfDefined();
+	wl_status_t status = WiFi.begin(
+	);  // Should connect to last used access point, see
+		// https://arduino-esp8266.readthedocs.io/en/latest/esp8266wifi/station-class.html#begin
+	wifiHandlerLogger.debug("Status: %d", status);
+	wifiState = SLIME_WIFI_SAVED_ATTEMPT;
+	wifiConnectionTimeout = millis();
 
 #if ESP8266
 #if POWERSAVING_MODE == POWER_SAVING_NONE
-    WiFi.setSleepMode(WIFI_NONE_SLEEP);
+	WiFi.setSleepMode(WIFI_NONE_SLEEP);
 #elif POWERSAVING_MODE == POWER_SAVING_MINIMUM
-    WiFi.setSleepMode(WIFI_MODEM_SLEEP);
+	WiFi.setSleepMode(WIFI_MODEM_SLEEP);
 #elif POWERSAVING_MODE == POWER_SAVING_MODERATE
-    WiFi.setSleepMode(WIFI_MODEM_SLEEP, 10);
+	WiFi.setSleepMode(WIFI_MODEM_SLEEP, 10);
 #elif POWERSAVING_MODE == POWER_SAVING_MAXIMUM
-    WiFi.setSleepMode(WIFI_LIGHT_SLEEP, 10);
+	WiFi.setSleepMode(WIFI_LIGHT_SLEEP, 10);
 #error "MAX POWER SAVING NOT WORKING YET, please disable!"
 #endif
 #else
 #if POWERSAVING_MODE == POWER_SAVING_NONE
-    WiFi.setSleep(WIFI_PS_NONE);
+	WiFi.setSleep(WIFI_PS_NONE);
 #elif POWERSAVING_MODE == POWER_SAVING_MINIMUM
-    WiFi.setSleep(WIFI_PS_MIN_MODEM);
+	WiFi.setSleep(WIFI_PS_MIN_MODEM);
-#elif POWERSAVING_MODE == POWER_SAVING_MODERATE || POWERSAVING_MODE == POWER_SAVING_MAXIMUM
+#elif POWERSAVING_MODE == POWER_SAVING_MODERATE \
-    wifi_config_t conf;
+	|| POWERSAVING_MODE == POWER_SAVING_MAXIMUM
-    if (esp_wifi_get_config(WIFI_IF_STA, &conf) == ESP_OK)
+	wifi_config_t conf;
-    {
+	if (esp_wifi_get_config(WIFI_IF_STA, &conf) == ESP_OK) {
-        conf.sta.listen_interval = 10;
+		conf.sta.listen_interval = 10;
-        esp_wifi_set_config(WIFI_IF_STA, &conf);
+		esp_wifi_set_config(WIFI_IF_STA, &conf);
-        WiFi.setSleep(WIFI_PS_MAX_MODEM);
+		WiFi.setSleep(WIFI_PS_MAX_MODEM);
-    }
+	} else {
-    else
+		wifiHandlerLogger.error("Unable to get WiFi config, power saving not enabled!");
-    {
+	}
-        wifiHandlerLogger.error("Unable to get WiFi config, power saving not enabled!");
-    }
 #endif
 #endif
 }
 
 void onConnected() {
-    WiFiNetwork::stopProvisioning();
+	WiFiNetwork::stopProvisioning();
-    statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false);
+	statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false);
-    isWifiConnected = true;
+	isWifiConnected = true;
-    hadWifi = true;
+	hadWifi = true;
-    wifiHandlerLogger.info("Connected successfully to SSID '%s', ip address %s", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
+	wifiHandlerLogger.info(
+		"Connected successfully to SSID '%s', ip address %s",
+		WiFi.SSID().c_str(),
+		WiFi.localIP().toString().c_str()
+	);
 }
 
-uint8_t WiFiNetwork::getWiFiState() {
+uint8_t WiFiNetwork::getWiFiState() { return wifiState; }
-    return wifiState;
-}
 
 void WiFiNetwork::upkeep() {
-    upkeepProvisioning();
+	upkeepProvisioning();
-    if(WiFi.status() != WL_CONNECTED) {
+	if (WiFi.status() != WL_CONNECTED) {
-        if(isWifiConnected) {
+		if (isWifiConnected) {
-            wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting...");
+			wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting...");
-            isWifiConnected = false;
+			isWifiConnected = false;
-        }
+		}
-        statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true);
+		statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true);
-        reportWifiError();
+		reportWifiError();
-        if(wifiConnectionTimeout + 11000 < millis()) {
+		if (wifiConnectionTimeout + 11000 < millis()) {
-            switch(wifiState) {
+			switch (wifiState) {
-                case SLIME_WIFI_NOT_SETUP: // Wasn't set up
+				case SLIME_WIFI_NOT_SETUP:  // Wasn't set up
-                return;
+					return;
-                case SLIME_WIFI_SAVED_ATTEMPT: // Couldn't connect with first set of credentials
+				case SLIME_WIFI_SAVED_ATTEMPT:  // Couldn't connect with first set of
-                    #if ESP8266
+												// credentials
-                        // Try again but with 11G
+#if ESP8266
-                        // But only if there are credentials, otherwise we just waste time before
+					// Try again but with 11G but only if there are credentials,
-                        // switching to hardcoded credentials.
+					// otherwise we just waste time before switching to hardcoded
-                        if (WiFi.SSID().length() > 0) {
+					// credentials.
-                            #if USE_ATTENUATION
+					if (WiFi.SSID().length() > 0) {
-                                WiFi.setOutputPower(20.0 -  ATTENUATION_G);
+#if USE_ATTENUATION
-                            #endif
+						WiFi.setOutputPower(20.0 - ATTENUATION_G);
-                            WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+#endif
-                            setStaticIPIfDefined();
+						WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-                            WiFi.begin();
+						setStaticIPIfDefined();
-                            wifiConnectionTimeout = millis();
+						WiFi.begin();
-                            wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status());
+						wifiConnectionTimeout = millis();
-                            wifiHandlerLogger.debug("Trying saved credentials with PHY Mode G...");
+						wifiHandlerLogger.error(
-                        } else {
+							"Can't connect from saved credentials, status: %d.",
-                            wifiHandlerLogger.debug("Skipping PHY Mode G attempt on 0-length SSID...");
+							WiFi.status()
-                        }
+						);
-                    #endif
+						wifiHandlerLogger.debug(
-                    wifiState = SLIME_WIFI_SAVED_G_ATTEMPT;
+							"Trying saved credentials with PHY Mode G..."
-                return;
+						);
-                case SLIME_WIFI_SAVED_G_ATTEMPT: // Couldn't connect with first set of credentials with PHY Mode G
+					} else {
-                    #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
+						wifiHandlerLogger.debug(
-                        // Try hardcoded credentials now
+							"Skipping PHY Mode G attempt on 0-length SSID..."
-                        #if ESP8266
+						);
-                            #if USE_ATTENUATION
+					}
-                                WiFi.setOutputPower(20.0 -  ATTENUATION_N);
+#endif
-                            #endif
+					wifiState = SLIME_WIFI_SAVED_G_ATTEMPT;
-                            WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+					return;
-                        #endif
+				case SLIME_WIFI_SAVED_G_ATTEMPT:  // Couldn't connect with first set of
-                        setStaticIPIfDefined();
+												  // credentials with PHY Mode G
-                        WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
+#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
-                        wifiConnectionTimeout = millis();
+												  // Try hardcoded credentials now
-                        wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status());
+#if ESP8266
-                        wifiHandlerLogger.debug("Trying hardcoded credentials...");
+#if USE_ATTENUATION
-                    #endif
+					WiFi.setOutputPower(20.0 - ATTENUATION_N);
-                    wifiState = SLIME_WIFI_HARDCODE_ATTEMPT;
+#endif
-                return;
+					WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-                case SLIME_WIFI_HARDCODE_ATTEMPT: // Couldn't connect with second set of credentials
+#endif
-                    #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266
+					setStaticIPIfDefined();
-                        // Try hardcoded credentials again, but with PHY Mode G
+					WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
-                        #if USE_ATTENUATION
+					wifiConnectionTimeout = millis();
-                            WiFi.setOutputPower(20.0 -  ATTENUATION_G);
+					wifiHandlerLogger.error(
-                        #endif
+						"Can't connect from saved credentials, status: %d.",
-                        WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+						WiFi.status()
-                        setStaticIPIfDefined();
+					);
-                        WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
+					wifiHandlerLogger.debug("Trying hardcoded credentials...");
-                        wifiConnectionTimeout = millis();
+#endif
-                        wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status());
+					wifiState = SLIME_WIFI_HARDCODE_ATTEMPT;
-                        wifiHandlerLogger.debug("Trying hardcoded credentials with WiFi PHY Mode G...");
+					return;
-                    #endif
+				case SLIME_WIFI_HARDCODE_ATTEMPT:  // Couldn't connect with second set
-                    wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT;
+												   // of credentials
-                return;
+#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266
-                case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with server-sent credentials.
+												   // Try hardcoded credentials again,
-                    #if ESP8266
+												   // but with PHY Mode G
-                        // Try again silently but with 11G
+#if USE_ATTENUATION
-                        #if USE_ATTENUATION
+					WiFi.setOutputPower(20.0 - ATTENUATION_G);
-                            WiFi.setOutputPower(20.0 -  ATTENUATION_G);
+#endif
-                        #endif
+					WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-                        WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+					setStaticIPIfDefined();
-                        setStaticIPIfDefined();
+					WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
-                        WiFi.begin();
+					wifiConnectionTimeout = millis();
-                        wifiConnectionTimeout = millis();
+					wifiHandlerLogger.error(
-                        wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT;
+						"Can't connect from saved credentials, status: %d.",
-                    #endif
+						WiFi.status()
-                return;
+					);
-                case SLIME_WIFI_HARDCODE_G_ATTEMPT: // Couldn't connect with second set of credentials with PHY Mode G.
+					wifiHandlerLogger.debug(
-                case SLIME_WIFI_SERVER_CRED_G_ATTEMPT: // Or if couldn't connect with server-sent credentials
+						"Trying hardcoded credentials with WiFi PHY Mode G..."
-                    // Return to the default PHY Mode N.
+					);
-                    #if ESP8266
+#endif
-                        #if USE_ATTENUATION
+					wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT;
-                            WiFi.setOutputPower(20.0 -  ATTENUATION_N);
+					return;
-                        #endif
+				case SLIME_WIFI_SERVER_CRED_ATTEMPT:  // Couldn't connect with
-                        WiFi.setPhyMode(WIFI_PHY_MODE_11N);
+													  // server-sent credentials.
-                    #endif
+#if ESP8266
-                    // Start smart config
+													  // Try again silently but with 11G
-                    if(!hadWifi && !WiFi.smartConfigDone() && wifiConnectionTimeout + 11000 < millis()) {
+#if USE_ATTENUATION
-                        if(WiFi.status() != WL_IDLE_STATUS) {
+					WiFi.setOutputPower(20.0 - ATTENUATION_G);
-                            wifiHandlerLogger.error("Can't connect from any credentials, status: %d.", WiFi.status());
+#endif
-                            wifiConnectionTimeout = millis();
+					WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-                        }
+					setStaticIPIfDefined();
-                        startProvisioning();
+					WiFi.begin();
-                    }
+					wifiConnectionTimeout = millis();
-                return;
+					wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT;
-            }
+#endif
-        }
+					return;
-        return;
+				case SLIME_WIFI_HARDCODE_G_ATTEMPT:  // Couldn't connect with second set
-    }
+													 // of credentials with PHY Mode G.
-    if(!isWifiConnected) {
+				case SLIME_WIFI_SERVER_CRED_G_ATTEMPT:  // Or if couldn't connect with
-        onConnected();
+														// server-sent credentials
-        return;
+// Return to the default PHY Mode N.
-    } else {
+#if ESP8266
-        if(millis() - last_rssi_sample >= 2000) {
+#if USE_ATTENUATION
-            last_rssi_sample = millis();
+					WiFi.setOutputPower(20.0 - ATTENUATION_N);
-            uint8_t signalStrength = WiFi.RSSI();
+#endif
-            networkConnection.sendSignalStrength(signalStrength);
+					WiFi.setPhyMode(WIFI_PHY_MODE_11N);
-        }
+#endif
-    }
+					// Start smart config
-    return;
+					if (!hadWifi && !WiFi.smartConfigDone()
+						&& wifiConnectionTimeout + 11000 < millis()) {
+						if (WiFi.status() != WL_IDLE_STATUS) {
+							wifiHandlerLogger.error(
+								"Can't connect from any credentials, status: %d.",
+								WiFi.status()
+							);
+							wifiConnectionTimeout = millis();
+						}
+						startProvisioning();
+					}
+					return;
+			}
+		}
+		return;
+	}
+	if (!isWifiConnected) {
+		onConnected();
+		return;
+	} else {
+		if (millis() - last_rssi_sample >= 2000) {
+			last_rssi_sample = millis();
+			uint8_t signalStrength = WiFi.RSSI();
+			networkConnection.sendSignalStrength(signalStrength);
+		}
+	}
+	return;
 }

src/network/wifihandler.h

@@ -1,52 +1,52 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_WIFI_H_
 #define SLIMEVR_WIFI_H_
 
 #ifdef ESP8266
-    #include <ESP8266WiFi.h>
+#include <ESP8266WiFi.h>
 #else
-    #include <WiFi.h>
+#include <WiFi.h>
 #endif
 
 namespace WiFiNetwork {
-    bool isConnected();
+bool isConnected();
-    void setUp();
+void setUp();
-    void upkeep();
+void upkeep();
-    void setWiFiCredentials(const char * SSID, const char * pass);
+void setWiFiCredentials(const char* SSID, const char* pass);
-    IPAddress getAddress();
+IPAddress getAddress();
-    uint8_t getWiFiState();
+uint8_t getWiFiState();
-}
+}  // namespace WiFiNetwork
 
 /** Wifi Reconnection Statuses **/
 typedef enum {
-    SLIME_WIFI_NOT_SETUP = 0,
+	SLIME_WIFI_NOT_SETUP = 0,
-    SLIME_WIFI_SAVED_ATTEMPT,
+	SLIME_WIFI_SAVED_ATTEMPT,
-    SLIME_WIFI_SAVED_G_ATTEMPT,
+	SLIME_WIFI_SAVED_G_ATTEMPT,
-    SLIME_WIFI_HARDCODE_ATTEMPT,
+	SLIME_WIFI_HARDCODE_ATTEMPT,
-    SLIME_WIFI_HARDCODE_G_ATTEMPT,
+	SLIME_WIFI_HARDCODE_G_ATTEMPT,
-    SLIME_WIFI_SERVER_CRED_ATTEMPT,
+	SLIME_WIFI_SERVER_CRED_ATTEMPT,
-    SLIME_WIFI_SERVER_CRED_G_ATTEMPT
+	SLIME_WIFI_SERVER_CRED_G_ATTEMPT
 } wifi_reconnection_statuses;
 
-#endif // SLIMEVR_WIFI_H_
+#endif  // SLIMEVR_WIFI_H_

src/network/wifiprovisioning.cpp

@@ -1,28 +1,29 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain
+	Copyright (c) 2021 Eiren Rain
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #include "wifiprovisioning.h"
-#include "wifihandler.h"
+
 #include "logging/Logger.h"
+#include "wifihandler.h"
 
 // TODO Currently provisioning implemented via SmartConfig
 // it sucks.
@@ -33,25 +34,24 @@ SlimeVR::Logging::Logger wifiProvisioningLogger("WiFiProvisioning");
 bool provisioning = false;
 
 void WiFiNetwork::upkeepProvisioning() {
-    // Called even when not provisioning to do things like provide neighbours or other upkeep
+	// Called even when not provisioning to do things like provide neighbours or other
+	// upkeep
 }
 
 void WiFiNetwork::startProvisioning() {
-    if(WiFi.beginSmartConfig()) {
+	if (WiFi.beginSmartConfig()) {
-        provisioning = true;
+		provisioning = true;
-        wifiProvisioningLogger.info("SmartConfig started");
+		wifiProvisioningLogger.info("SmartConfig started");
-    }
+	}
 }
 
 void WiFiNetwork::stopProvisioning() {
-    WiFi.stopSmartConfig();
+	WiFi.stopSmartConfig();
-    provisioning = false;
+	provisioning = false;
 }
 
 void WiFiNetwork::provideNeighbours() {
-    // TODO: SmartConfig can't do this, created for future
+	// TODO: SmartConfig can't do this, created for future
 }
 
-bool WiFiNetwork::isProvisioning() {
+bool WiFiNetwork::isProvisioning() { return provisioning && !WiFi.smartConfigDone(); }
-    return provisioning && !WiFi.smartConfigDone();
-}

src/network/wifiprovisioning.h

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

src/sensors/EmptySensor.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SENSORS_EMPTYSENSOR_H
@@ -26,26 +26,23 @@
 
 #include "sensor.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+class EmptySensor : public Sensor {
-    {
+public:
-        class EmptySensor : public Sensor
+	EmptySensor(uint8_t id)
-        {
+		: Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){};
-        public:
+	~EmptySensor(){};
-            EmptySensor(uint8_t id) : Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){};
-            ~EmptySensor(){};
 
-            void motionSetup() override final{};
+	void motionSetup() override final{};
-            void motionLoop() override final{};
+	void motionLoop() override final{};
-            void sendData() override final{};
+	void sendData() override final{};
-            void startCalibration(int calibrationType) override final{};
+	void startCalibration(int calibrationType) override final{};
-            SensorStatus getSensorState() override final
+	SensorStatus getSensorState() override final {
-            {
+		return SensorStatus::SENSOR_OFFLINE;
-                return SensorStatus::SENSOR_OFFLINE;
+	};
-            };
+};
-        };
+}  // namespace Sensors
-    }
+}  // namespace SlimeVR
-}
 
 #endif

src/sensors/ErroneousSensor.cpp

@@ -1,41 +1,39 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #include "ErroneousSensor.h"
+
 #include "GlobalVars.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+void ErroneousSensor::motionSetup() {
-    {
+	m_Logger.error(
-        void ErroneousSensor::motionSetup()
+		"IMU of type %s failed to initialize",
-        {
+		getIMUNameByType(m_ExpectedType)
-            m_Logger.error("IMU of type %s failed to initialize", getIMUNameByType(m_ExpectedType));
+	);
-        }
-
-        SensorStatus ErroneousSensor::getSensorState()
-        {
-            return SensorStatus::SENSOR_ERROR;
-        };
-    }
 }
+
+SensorStatus ErroneousSensor::getSensorState() { return SensorStatus::SENSOR_ERROR; };
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/ErroneousSensor.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SENSORS_ERRONEOUSSENSOR_H
@@ -26,26 +26,25 @@
 
 #include "sensor.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+class ErroneousSensor : public Sensor {
-    {
+public:
-        class ErroneousSensor : public Sensor
+	ErroneousSensor(uint8_t id, ImuID type)
-        {
+		: Sensor("ErroneousSensor", type, id, 0, 0.0)
-        public:
+		, m_ExpectedType(type){};
-            ErroneousSensor(uint8_t id, ImuID type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){};
+	~ErroneousSensor(){};
-            ~ErroneousSensor(){};
 
-            void motionSetup() override;
+	void motionSetup() override;
-            void motionLoop() override final{};
+	void motionLoop() override final{};
-            void sendData() override{};
+	void sendData() override{};
-            void startCalibration(int calibrationType) override final{};
+	void startCalibration(int calibrationType) override final{};
-            SensorStatus getSensorState() override final;
+	SensorStatus getSensorState() override final;
 
-        private:
+private:
-            ImuID m_ExpectedType;
+	ImuID m_ExpectedType;
-        };
+};
-    }
+}  // namespace Sensors
-}
+}  // namespace SlimeVR
 
 #endif

src/sensors/SensorFusion.cpp

@@ -1,181 +1,215 @@
 #include "SensorFusion.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
-    {
-        
-        void SensorFusion::update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat)
-        {
-            updateAcc(Axyz, deltat);
-            updateGyro(Gxyz, deltat);
-        }
 
-        void SensorFusion::update9D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t Mxyz[3], sensor_real_t deltat)
+void SensorFusion::update6D(
-        {
+	sensor_real_t Axyz[3],
-            updateMag(Mxyz, deltat);
+	sensor_real_t Gxyz[3],
-            updateAcc(Axyz, deltat);
+	sensor_real_t deltat
-            updateGyro(Gxyz, deltat);
+) {
-        }
+	updateAcc(Axyz, deltat);
-
+	updateGyro(Gxyz, deltat);
-        void SensorFusion::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat)
-        {
-            if (deltat < 0) deltat = accTs;
-
-            std::copy(Axyz, Axyz+3, bAxyz);
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                accelUpdated = true;
-            #elif SENSOR_USE_BASICVQF
-                basicvqf.updateAcc(Axyz);
-            #elif SENSOR_USE_VQF
-                vqf.updateAcc(Axyz);
-            #endif
-        }
-
-        void SensorFusion::updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat)
-        {
-            if (deltat < 0) deltat = magTs;
-
-            if (!magExist) {
-                if (Mxyz[0] != 0.0f || Mxyz[1] != 0.0f || Mxyz[2] != 0.0f) {
-                    magExist = true;
-                } else {
-                    return;
-                }
-            }
-
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                std::copy(Mxyz, Mxyz+3, bMxyz);
-            #elif SENSOR_USE_BASICVQF
-                basicvqf.updateMag(Mxyz);
-            #elif SENSOR_USE_VQF
-                vqf.updateMag(Mxyz);
-            #endif
-        }
-
-        void SensorFusion::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat)
-        {
-            if (deltat < 0) deltat = gyrTs;
-
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                sensor_real_t Axyz[3] {0.0f, 0.0f, 0.0f};
-                if (accelUpdated) {
-                    std::copy(bAxyz, bAxyz+3, Axyz);
-                    accelUpdated = false;
-                }
-            #endif
-
-            #if SENSOR_USE_MAHONY
-                if (!magExist) {
-                    mahony.update(qwxyz, Axyz[0], Axyz[1], Axyz[2],
-                                         Gxyz[0], Gxyz[1], Gxyz[2],
-                                         deltat);
-                } else {
-                    mahony.update(qwxyz,  Axyz[0],  Axyz[1],  Axyz[2],
-                                          Gxyz[0],  Gxyz[1],  Gxyz[2],
-                                         bMxyz[0], bMxyz[1], bMxyz[2],
-                                         deltat);
-                }
-            #elif SENSOR_USE_MADGWICK
-                if (!magExist) {
-                    madgwick.update(qwxyz, Axyz[0], Axyz[1], Axyz[2],
-                                           Gxyz[0], Gxyz[1], Gxyz[2],
-                                           deltat);
-                } else {
-                    madgwick.update(qwxyz,  Axyz[0],  Axyz[1],  Axyz[2],
-                                            Gxyz[0],  Gxyz[1],  Gxyz[2],
-                                           bMxyz[0], bMxyz[1], bMxyz[2],
-                                           deltat);
-                }
-            #elif SENSOR_USE_BASICVQF
-                basicvqf.updateGyr(Gxyz, deltat);
-            #elif SENSOR_USE_VQF
-                vqf.updateGyr(Gxyz, deltat);
-            #endif
-
-            updated = true;
-            gravityReady = false;
-            linaccelReady = false;
-        }
-
-        bool SensorFusion::isUpdated()
-        {
-            return updated;
-        }
-
-        void SensorFusion::clearUpdated()
-        {
-            updated = false;
-        }
-        
-        sensor_real_t const * SensorFusion::getQuaternion()
-        {
-            #if SENSOR_USE_BASICVQF
-                if(magExist) {
-                    basicvqf.getQuat9D(qwxyz);
-                } else {
-                    basicvqf.getQuat6D(qwxyz);
-                }
-            #elif SENSOR_USE_VQF
-                if(magExist) {
-                    vqf.getQuat9D(qwxyz);
-                } else {
-                    vqf.getQuat6D(qwxyz);
-                }
-            #endif
-
-            return qwxyz;
-        }
-
-        Quat SensorFusion::getQuaternionQuat()
-        {
-            getQuaternion();
-            return Quat(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
-        }
-
-        sensor_real_t const * SensorFusion::getGravityVec()
-        {
-            if (!gravityReady) {
-                calcGravityVec(qwxyz, vecGravity);
-                gravityReady = true;
-            }
-            return vecGravity;
-        }
-
-        sensor_real_t const * SensorFusion::getLinearAcc()
-        {
-            if (!linaccelReady) {
-                getGravityVec();
-                calcLinearAcc(bAxyz, vecGravity, linAccel);
-                linaccelReady = true;
-            }
-            return linAccel;
-        }
-
-        void SensorFusion::getLinearAcc(sensor_real_t outLinAccel[3])
-        {
-            getLinearAcc();
-            std::copy(linAccel, linAccel+3, outLinAccel);
-        }
-
-        Vector3 SensorFusion::getLinearAccVec()
-        {
-            getLinearAcc();
-            return Vector3(linAccel[0], linAccel[1], linAccel[2]);
-        }
-
-        void SensorFusion::calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3])
-        {
-            gravVec[0] = 2 * (qwxyz[1] * qwxyz[3] - qwxyz[0] * qwxyz[2]);
-            gravVec[1] = 2 * (qwxyz[0] * qwxyz[1] + qwxyz[2] * qwxyz[3]);
-            gravVec[2] = qwxyz[0]*qwxyz[0] - qwxyz[1]*qwxyz[1] - qwxyz[2]*qwxyz[2] + qwxyz[3]*qwxyz[3];
-        }
-        
-        void SensorFusion::calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3])
-        {
-            accout[0] = accin[0] - gravVec[0] * CONST_EARTH_GRAVITY;
-            accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY;
-            accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
-        }
-    }
 }
+
+void SensorFusion::update9D(
+	sensor_real_t Axyz[3],
+	sensor_real_t Gxyz[3],
+	sensor_real_t Mxyz[3],
+	sensor_real_t deltat
+) {
+	updateMag(Mxyz, deltat);
+	updateAcc(Axyz, deltat);
+	updateGyro(Gxyz, deltat);
+}
+
+void SensorFusion::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat) {
+	if (deltat < 0) {
+		deltat = accTs;
+	}
+
+	std::copy(Axyz, Axyz + 3, bAxyz);
+#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
+	accelUpdated = true;
+#elif SENSOR_USE_BASICVQF
+	basicvqf.updateAcc(Axyz);
+#elif SENSOR_USE_VQF
+	vqf.updateAcc(Axyz);
+#endif
+}
+
+void SensorFusion::updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat) {
+	if (deltat < 0) {
+		deltat = magTs;
+	}
+
+	if (!magExist) {
+		if (Mxyz[0] != 0.0f || Mxyz[1] != 0.0f || Mxyz[2] != 0.0f) {
+			magExist = true;
+		} else {
+			return;
+		}
+	}
+
+#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
+	std::copy(Mxyz, Mxyz + 3, bMxyz);
+#elif SENSOR_USE_BASICVQF
+	basicvqf.updateMag(Mxyz);
+#elif SENSOR_USE_VQF
+	vqf.updateMag(Mxyz);
+#endif
+}
+
+void SensorFusion::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat) {
+	if (deltat < 0) {
+		deltat = gyrTs;
+	}
+
+#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
+	sensor_real_t Axyz[3]{0.0f, 0.0f, 0.0f};
+	if (accelUpdated) {
+		std::copy(bAxyz, bAxyz + 3, Axyz);
+		accelUpdated = false;
+	}
+#endif
+
+#if SENSOR_USE_MAHONY
+	if (!magExist) {
+		mahony.update(
+			qwxyz,
+			Axyz[0],
+			Axyz[1],
+			Axyz[2],
+			Gxyz[0],
+			Gxyz[1],
+			Gxyz[2],
+			deltat
+		);
+	} else {
+		mahony.update(
+			qwxyz,
+			Axyz[0],
+			Axyz[1],
+			Axyz[2],
+			Gxyz[0],
+			Gxyz[1],
+			Gxyz[2],
+			bMxyz[0],
+			bMxyz[1],
+			bMxyz[2],
+			deltat
+		);
+	}
+#elif SENSOR_USE_MADGWICK
+	if (!magExist) {
+		madgwick.update(
+			qwxyz,
+			Axyz[0],
+			Axyz[1],
+			Axyz[2],
+			Gxyz[0],
+			Gxyz[1],
+			Gxyz[2],
+			deltat
+		);
+	} else {
+		madgwick.update(
+			qwxyz,
+			Axyz[0],
+			Axyz[1],
+			Axyz[2],
+			Gxyz[0],
+			Gxyz[1],
+			Gxyz[2],
+			bMxyz[0],
+			bMxyz[1],
+			bMxyz[2],
+			deltat
+		);
+	}
+#elif SENSOR_USE_BASICVQF
+	basicvqf.updateGyr(Gxyz, deltat);
+#elif SENSOR_USE_VQF
+	vqf.updateGyr(Gxyz, deltat);
+#endif
+
+	updated = true;
+	gravityReady = false;
+	linaccelReady = false;
+}
+
+bool SensorFusion::isUpdated() { return updated; }
+
+void SensorFusion::clearUpdated() { updated = false; }
+
+sensor_real_t const* SensorFusion::getQuaternion() {
+#if SENSOR_USE_BASICVQF
+	if (magExist) {
+		basicvqf.getQuat9D(qwxyz);
+	} else {
+		basicvqf.getQuat6D(qwxyz);
+	}
+#elif SENSOR_USE_VQF
+	if (magExist) {
+		vqf.getQuat9D(qwxyz);
+	} else {
+		vqf.getQuat6D(qwxyz);
+	}
+#endif
+
+	return qwxyz;
+}
+
+Quat SensorFusion::getQuaternionQuat() {
+	getQuaternion();
+	return Quat(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
+}
+
+sensor_real_t const* SensorFusion::getGravityVec() {
+	if (!gravityReady) {
+		calcGravityVec(qwxyz, vecGravity);
+		gravityReady = true;
+	}
+	return vecGravity;
+}
+
+sensor_real_t const* SensorFusion::getLinearAcc() {
+	if (!linaccelReady) {
+		getGravityVec();
+		calcLinearAcc(bAxyz, vecGravity, linAccel);
+		linaccelReady = true;
+	}
+	return linAccel;
+}
+
+void SensorFusion::getLinearAcc(sensor_real_t outLinAccel[3]) {
+	getLinearAcc();
+	std::copy(linAccel, linAccel + 3, outLinAccel);
+}
+
+Vector3 SensorFusion::getLinearAccVec() {
+	getLinearAcc();
+	return Vector3(linAccel[0], linAccel[1], linAccel[2]);
+}
+
+void SensorFusion::calcGravityVec(
+	const sensor_real_t qwxyz[4],
+	sensor_real_t gravVec[3]
+) {
+	gravVec[0] = 2 * (qwxyz[1] * qwxyz[3] - qwxyz[0] * qwxyz[2]);
+	gravVec[1] = 2 * (qwxyz[0] * qwxyz[1] + qwxyz[2] * qwxyz[3]);
+	gravVec[2] = qwxyz[0] * qwxyz[0] - qwxyz[1] * qwxyz[1] - qwxyz[2] * qwxyz[2]
+			   + qwxyz[3] * qwxyz[3];
+}
+
+void SensorFusion::calcLinearAcc(
+	const sensor_real_t accin[3],
+	const sensor_real_t gravVec[3],
+	sensor_real_t accout[3]
+) {
+	accout[0] = accin[0] - gravVec[0] * CONST_EARTH_GRAVITY;
+	accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY;
+	accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
+}
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/SensorFusion.h

@@ -14,13 +14,13 @@
 #define SENSOR_FUSION_VQF 4
 
 #if SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY
-    #define SENSOR_FUSION_TYPE_STRING "mahony"
+#define SENSOR_FUSION_TYPE_STRING "mahony"
 #elif SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK
-    #define SENSOR_FUSION_TYPE_STRING "madgwick"
+#define SENSOR_FUSION_TYPE_STRING "madgwick"
 #elif SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF
-    #define SENSOR_FUSION_TYPE_STRING "bvqf"
+#define SENSOR_FUSION_TYPE_STRING "bvqf"
 #elif SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF
-    #define SENSOR_FUSION_TYPE_STRING "vqf"
+#define SENSOR_FUSION_TYPE_STRING "vqf"
 #endif
 
 #define SENSOR_USE_MAHONY (SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY)
@@ -28,106 +28,122 @@
 #define SENSOR_USE_BASICVQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF)
 #define SENSOR_USE_VQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF)
 
+#include <basicvqf.h>
+#include <vqf.h>
 
 #include "../motionprocessing/types.h"
-
-#include "mahony.h"
 #include "madgwick.h"
-#include <vqf.h>
+#include "mahony.h"
-#include <basicvqf.h>
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+#if SENSOR_USE_VQF
-    {
+struct SensorVQFParams : VQFParams {
-        #if SENSOR_USE_VQF
+	SensorVQFParams()
-        struct SensorVQFParams: VQFParams {
+		: VQFParams() {
-            SensorVQFParams() : VQFParams() {
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-                #ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+		motionBiasEstEnabled = true;
-                motionBiasEstEnabled = true;
+#endif
-                #endif
+		tauAcc = 2.0f;
-                tauAcc = 2.0f;
+		restMinT = 2.0f;
-                restMinT = 2.0f;
+		restThGyr = 0.6f;  // 400 norm
-                restThGyr = 0.6f; // 400 norm
+		restThAcc = 0.06f;  // 100 norm
-                restThAcc = 0.06f; // 100 norm
+	}
-            }
+};
-        };
+#endif
-        #endif
 
-        class SensorFusion
+class SensorFusion {
-        {
+public:
-        public:
+	SensorFusion(
-            SensorFusion(sensor_real_t gyrTs, sensor_real_t accTs=-1.0, sensor_real_t magTs=-1.0)
+		sensor_real_t gyrTs,
-                : gyrTs(gyrTs), 
+		sensor_real_t accTs = -1.0,
-                  accTs( (accTs<0) ? gyrTs : accTs ), 
+		sensor_real_t magTs = -1.0
-                  magTs( (magTs<0) ? gyrTs : magTs )
+	)
-            #if SENSOR_USE_MAHONY
+		: gyrTs(gyrTs)
-            #elif SENSOR_USE_MADGWICK
+		, accTs((accTs < 0) ? gyrTs : accTs)
-            #elif SENSOR_USE_BASICVQF
+		, magTs((magTs < 0) ? gyrTs : magTs)
-                , basicvqf(gyrTs, ((accTs<0) ? gyrTs : accTs),
+#if SENSOR_USE_MAHONY
-                                    ((magTs<0) ? gyrTs : magTs))
+#elif SENSOR_USE_MADGWICK
-            #elif SENSOR_USE_VQF
+#elif SENSOR_USE_BASICVQF
-                , vqf(vqfParams, gyrTs, ((accTs<0) ? gyrTs : accTs), 
+		, basicvqf(gyrTs, ((accTs < 0) ? gyrTs : accTs), ((magTs < 0) ? gyrTs : magTs))
-                                        ((magTs<0) ? gyrTs : magTs))
+#elif SENSOR_USE_VQF
-            #endif
+		, vqf(vqfParams,
-            {}
+			  gyrTs,
+			  ((accTs < 0) ? gyrTs : accTs),
+			  ((magTs < 0) ? gyrTs : magTs))
+#endif
+	{
+	}
 
-            void update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f);
+	void update6D(
-            void update9D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f);
+		sensor_real_t Axyz[3],
-            void updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat=-1.0f);
+		sensor_real_t Gxyz[3],
-            void updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f);
+		sensor_real_t deltat = -1.0f
-            void updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f);
+	);
+	void update9D(
+		sensor_real_t Axyz[3],
+		sensor_real_t Gxyz[3],
+		sensor_real_t Mxyz[3],
+		sensor_real_t deltat = -1.0f
+	);
+	void updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat = -1.0f);
+	void updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat = -1.0f);
+	void updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat = -1.0f);
 
-            bool isUpdated();
+	bool isUpdated();
-            void clearUpdated();
+	void clearUpdated();
-            sensor_real_t const * getQuaternion();
+	sensor_real_t const* getQuaternion();
-            Quat getQuaternionQuat();
+	Quat getQuaternionQuat();
-            sensor_real_t const * getGravityVec();
+	sensor_real_t const* getGravityVec();
-            sensor_real_t const * getLinearAcc();
+	sensor_real_t const* getLinearAcc();
-            void getLinearAcc(sensor_real_t outLinAccel[3]);
+	void getLinearAcc(sensor_real_t outLinAccel[3]);
-            Vector3 getLinearAccVec();
+	Vector3 getLinearAccVec();
 
-            static void calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3]);
+	static void calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3]);
-            static void calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3]);
+	static void calcLinearAcc(
+		const sensor_real_t accin[3],
+		const sensor_real_t gravVec[3],
+		sensor_real_t accout[3]
+	);
 
-        protected:
+protected:
-            sensor_real_t gyrTs;
+	sensor_real_t gyrTs;
-            sensor_real_t accTs;
+	sensor_real_t accTs;
-            sensor_real_t magTs;
+	sensor_real_t magTs;
 
-            #if SENSOR_USE_MAHONY
+#if SENSOR_USE_MAHONY
-                Mahony<sensor_real_t> mahony;
+	Mahony<sensor_real_t> mahony;
-            #elif SENSOR_USE_MADGWICK
+#elif SENSOR_USE_MADGWICK
-                Madgwick<sensor_real_t> madgwick;
+	Madgwick<sensor_real_t> madgwick;
-            #elif SENSOR_USE_BASICVQF
+#elif SENSOR_USE_BASICVQF
-                BasicVQF basicvqf;
+	BasicVQF basicvqf;
-            #elif SENSOR_USE_VQF
+#elif SENSOR_USE_VQF
-                SensorVQFParams vqfParams {};
+	SensorVQFParams vqfParams{};
-                VQF vqf;
+	VQF vqf;
-            #endif
+#endif
 
-            // A also used for linear acceleration extraction
+	// A also used for linear acceleration extraction
-            sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
+	sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
 
-            #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
+#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK
-                // Buffer M here to keep the behavior of BMI160
+	// Buffer M here to keep the behavior of BMI160
-                sensor_real_t bMxyz[3]{0.0f, 0.0f, 0.0f};
+	sensor_real_t bMxyz[3]{0.0f, 0.0f, 0.0f};
-                bool accelUpdated = false;
+	bool accelUpdated = false;
-            #endif
+#endif
 
-            bool magExist = false;
+	bool magExist = false;
-            sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
+	sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
-            bool updated = false;
+	bool updated = false;
 
-            bool gravityReady = false;
+	bool gravityReady = false;
-            sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
+	sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
-            bool linaccelReady = false;
+	bool linaccelReady = false;
-            sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
+	sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
-            #if ESP32
+#if ESP32
-            sensor_real_t linAccel_guard;   // Temporary patch for some weird ESP32 bug
+	sensor_real_t linAccel_guard;  // Temporary patch for some weird ESP32 bug
-            #endif
+#endif
-        };
+};
-    }
+}  // namespace Sensors
-}
+}  // namespace SlimeVR
 
-#endif // SLIMEVR_SENSORFUSION_H
+#endif  // SLIMEVR_SENSORFUSION_H

src/sensors/SensorFusionDMP.cpp

@@ -1,113 +1,100 @@
 #include "SensorFusionDMP.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+void SensorFusionDMP::updateAcc(sensor_real_t Axyz[3]) {
-    {
+	std::copy(Axyz, Axyz + 3, bAxyz);
-        void SensorFusionDMP::updateAcc(sensor_real_t Axyz[3])
-        {
-            std::copy(Axyz, Axyz+3, bAxyz);
-        }
-
-        void SensorFusionDMP::updateMag(sensor_real_t Mxyz[3])
-        {
-            if (!magExist) {
-                if (Mxyz[0] != 0.0f || Mxyz[1] != 0.0f || Mxyz[2] != 0.0f) {
-                    magExist = true;
-                } else {
-                    return;
-                }
-            }
-
-            getGravityVec();
-            dmpmag.update(qwxyz, bqwxyz, vecGravity, Mxyz);
-        }
-
-        void SensorFusionDMP::updateQuaternion(sensor_real_t nqwxyz[4])
-        {
-            std::copy(nqwxyz, nqwxyz+4, bqwxyz);
-
-            updated = true;
-            gravityReady = false;
-            linaccelReady = false;
-        }
-
-        void SensorFusionDMP::updateQuaternion(Quaternion const & nq)
-        {
-            bqwxyz[0] = nq.w; bqwxyz[1] = nq.x; bqwxyz[2] = nq.y; bqwxyz[3] = nq.z;
-
-            updated = true;
-            gravityReady = false;
-            linaccelReady = false;
-        }
-
-        void SensorFusionDMP::updateQuaternion(Quat const & nq)
-        {
-            bqwxyz[0] = nq.w; bqwxyz[1] = nq.x; bqwxyz[2] = nq.y; bqwxyz[3] = nq.z;
-
-            updated = true;
-            gravityReady = false;
-            linaccelReady = false;
-        }
-
-        bool SensorFusionDMP::isUpdated()
-        {
-            return updated;
-        }
-
-        void SensorFusionDMP::clearUpdated()
-        {
-            updated = false;
-        }
-        
-        sensor_real_t const * SensorFusionDMP::getQuaternion()
-        {
-            if (!magExist) {
-                // remap axis from DMP space to sensor space
-                qwxyz[0] = bqwxyz[0];
-                qwxyz[1] = -bqwxyz[2];
-                qwxyz[2] = bqwxyz[1];
-                qwxyz[3] = bqwxyz[3];
-            }
-            // dmpmag remaps axis during Mag update
-            return qwxyz;
-        }
-
-        Quat SensorFusionDMP::getQuaternionQuat()
-        {
-            getQuaternion();
-            return Quat(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
-        }
-
-        sensor_real_t const * SensorFusionDMP::getGravityVec()
-        {
-            if (!gravityReady) {
-                SensorFusion::calcGravityVec(bqwxyz, vecGravity);
-                gravityReady = true;
-            }
-            return vecGravity;
-        }
-
-        sensor_real_t const * SensorFusionDMP::getLinearAcc()
-        {
-            if (!linaccelReady) {
-                getGravityVec();
-                SensorFusion::calcLinearAcc(bAxyz, vecGravity, linAccel);
-                linaccelReady = true;
-            }
-            return linAccel;
-        }
-
-        void SensorFusionDMP::getLinearAcc(sensor_real_t outLinAccel[3])
-        {
-            getLinearAcc();
-            std::copy(linAccel, linAccel+3, outLinAccel);
-        }
-
-        Vector3 SensorFusionDMP::getLinearAccVec()
-        {
-            getLinearAcc();
-            return Vector3(linAccel[0], linAccel[1], linAccel[2]);
-        }        
-    }
 }
+
+void SensorFusionDMP::updateMag(sensor_real_t Mxyz[3]) {
+	if (!magExist) {
+		if (Mxyz[0] != 0.0f || Mxyz[1] != 0.0f || Mxyz[2] != 0.0f) {
+			magExist = true;
+		} else {
+			return;
+		}
+	}
+
+	getGravityVec();
+	dmpmag.update(qwxyz, bqwxyz, vecGravity, Mxyz);
+}
+
+void SensorFusionDMP::updateQuaternion(sensor_real_t nqwxyz[4]) {
+	std::copy(nqwxyz, nqwxyz + 4, bqwxyz);
+
+	updated = true;
+	gravityReady = false;
+	linaccelReady = false;
+}
+
+void SensorFusionDMP::updateQuaternion(Quaternion const& nq) {
+	bqwxyz[0] = nq.w;
+	bqwxyz[1] = nq.x;
+	bqwxyz[2] = nq.y;
+	bqwxyz[3] = nq.z;
+
+	updated = true;
+	gravityReady = false;
+	linaccelReady = false;
+}
+
+void SensorFusionDMP::updateQuaternion(Quat const& nq) {
+	bqwxyz[0] = nq.w;
+	bqwxyz[1] = nq.x;
+	bqwxyz[2] = nq.y;
+	bqwxyz[3] = nq.z;
+
+	updated = true;
+	gravityReady = false;
+	linaccelReady = false;
+}
+
+bool SensorFusionDMP::isUpdated() { return updated; }
+
+void SensorFusionDMP::clearUpdated() { updated = false; }
+
+sensor_real_t const* SensorFusionDMP::getQuaternion() {
+	if (!magExist) {
+		// remap axis from DMP space to sensor space
+		qwxyz[0] = bqwxyz[0];
+		qwxyz[1] = -bqwxyz[2];
+		qwxyz[2] = bqwxyz[1];
+		qwxyz[3] = bqwxyz[3];
+	}
+	// dmpmag remaps axis during Mag update
+	return qwxyz;
+}
+
+Quat SensorFusionDMP::getQuaternionQuat() {
+	getQuaternion();
+	return Quat(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
+}
+
+sensor_real_t const* SensorFusionDMP::getGravityVec() {
+	if (!gravityReady) {
+		SensorFusion::calcGravityVec(bqwxyz, vecGravity);
+		gravityReady = true;
+	}
+	return vecGravity;
+}
+
+sensor_real_t const* SensorFusionDMP::getLinearAcc() {
+	if (!linaccelReady) {
+		getGravityVec();
+		SensorFusion::calcLinearAcc(bAxyz, vecGravity, linAccel);
+		linaccelReady = true;
+	}
+	return linAccel;
+}
+
+void SensorFusionDMP::getLinearAcc(sensor_real_t outLinAccel[3]) {
+	getLinearAcc();
+	std::copy(linAccel, linAccel + 3, outLinAccel);
+}
+
+Vector3 SensorFusionDMP::getLinearAccVec() {
+	getLinearAcc();
+	return Vector3(linAccel[0], linAccel[1], linAccel[2]);
+}
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/SensorFusionDMP.h

@@ -4,47 +4,44 @@
 #include "SensorFusion.h"
 #include "dmpmag.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+class SensorFusionDMP {
-    {
+public:
-        class SensorFusionDMP
+	void updateQuaternion(sensor_real_t nqwxyz[4]);
-        {
+	void updateQuaternion(Quaternion const& nq);
-        public:
+	void updateQuaternion(Quat const& nq);
-            void updateQuaternion(sensor_real_t nqwxyz[4]);
+	void updateAcc(sensor_real_t Axyz[3]);
-            void updateQuaternion(Quaternion const & nq);
+	void updateMag(sensor_real_t Mxyz[3]);
-            void updateQuaternion(Quat const & nq);
-            void updateAcc(sensor_real_t Axyz[3]);
-            void updateMag(sensor_real_t Mxyz[3]);
 
-            bool isUpdated();
+	bool isUpdated();
-            void clearUpdated();
+	void clearUpdated();
-            sensor_real_t const * getQuaternion();
+	sensor_real_t const* getQuaternion();
-            Quat getQuaternionQuat();
+	Quat getQuaternionQuat();
-            sensor_real_t const * getGravityVec();
+	sensor_real_t const* getGravityVec();
-            sensor_real_t const * getLinearAcc();
+	sensor_real_t const* getLinearAcc();
-            void getLinearAcc(sensor_real_t outLinAccel[3]);
+	void getLinearAcc(sensor_real_t outLinAccel[3]);
-            Vector3 getLinearAccVec();
+	Vector3 getLinearAccVec();
 
-        protected:
+protected:
-            DMPMag<sensor_real_t> dmpmag;
+	DMPMag<sensor_real_t> dmpmag;
 
-            sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
+	sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
-            
-            bool magExist = false;
-            sensor_real_t bqwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
-            sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
-            bool updated = false;
 
-            bool gravityReady = false;
+	bool magExist = false;
-            sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
+	sensor_real_t bqwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
-            bool linaccelReady = false;
+	sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f};
-            sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
+	bool updated = false;
-            #if ESP32
-            sensor_real_t linAccel_guard;   // Temporary patch for some weird ESP32 bug
-            #endif
-        };
-    }
-}
 
-#endif // SLIMEVR_SENSORFUSIONDMP_H
+	bool gravityReady = false;
+	sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
+	bool linaccelReady = false;
+	sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
+#if ESP32
+	sensor_real_t linAccel_guard;  // Temporary patch for some weird ESP32 bug
+#endif
+};
+}  // namespace Sensors
+}  // namespace SlimeVR
+
+#endif  // SLIMEVR_SENSORFUSIONDMP_H

src/sensors/SensorFusionRestDetect.cpp

@@ -1,32 +1,37 @@
 #include "SensorFusionRestDetect.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+#if !SENSOR_FUSION_WITH_RESTDETECT
-    {
+void SensorFusionRestDetect::updateAcc(
-        #if !SENSOR_FUSION_WITH_RESTDETECT
+	const sensor_real_t Axyz[3],
-        void SensorFusionRestDetect::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat)
+	sensor_real_t deltat
-        {
+) {
-            if (deltat < 0) deltat = accTs;
+	if (deltat < 0) {
-            restDetection.updateAcc(deltat, Axyz);
+		deltat = accTs;
-            SensorFusion::updateAcc(Axyz, deltat);
+	}
-        }
+	restDetection.updateAcc(deltat, Axyz);
-
+	SensorFusion::updateAcc(Axyz, deltat);
-        void SensorFusionRestDetect::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat)
-        {
-            if (deltat < 0) deltat = gyrTs;
-            restDetection.updateGyr(Gxyz);
-            SensorFusion::updateGyro(Gxyz, deltat);
-        }
-        #endif
-
-        bool SensorFusionRestDetect::getRestDetected()
-        {
-            #if !SENSOR_FUSION_WITH_RESTDETECT
-                return restDetection.getRestDetected();
-            #elif SENSOR_USE_VQF
-                return vqf.getRestDetected();
-            #endif
-        }
-    }
 }
+
+void SensorFusionRestDetect::updateGyro(
+	const sensor_real_t Gxyz[3],
+	sensor_real_t deltat
+) {
+	if (deltat < 0) {
+		deltat = gyrTs;
+	}
+	restDetection.updateGyr(Gxyz);
+	SensorFusion::updateGyro(Gxyz, deltat);
+}
+#endif
+
+bool SensorFusionRestDetect::getRestDetected() {
+#if !SENSOR_FUSION_WITH_RESTDETECT
+	return restDetection.getRestDetected();
+#elif SENSOR_USE_VQF
+	return vqf.getRestDetected();
+#endif
+}
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/SensorFusionRestDetect.h

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

src/sensors/SensorManager.cpp

@@ -1,192 +1,199 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #include "SensorManager.h"
 
+#include "bmi160sensor.h"
 #include "bno055sensor.h"
 #include "bno080sensor.h"
-#include "mpu9250sensor.h"
-#include "mpu6050sensor.h"
-#include "bmi160sensor.h"
 #include "icm20948sensor.h"
+#include "mpu6050sensor.h"
+#include "mpu9250sensor.h"
 #include "sensoraddresses.h"
-#include "softfusion/softfusionsensor.h"
-#include "softfusion/drivers/lsm6ds3trc.h"
-#include "softfusion/drivers/icm42688.h"
 #include "softfusion/drivers/bmi270.h"
-#include "softfusion/drivers/lsm6dsv.h"
+#include "softfusion/drivers/icm42688.h"
+#include "softfusion/drivers/lsm6ds3trc.h"
 #include "softfusion/drivers/lsm6dso.h"
 #include "softfusion/drivers/lsm6dsr.h"
+#include "softfusion/drivers/lsm6dsv.h"
 #include "softfusion/drivers/mpu6050.h"
-
 #include "softfusion/i2cimpl.h"
+#include "softfusion/softfusionsensor.h"
 
 #if ESP32
-    #include "driver/i2c.h"
+#include "driver/i2c.h"
 #endif
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+using SoftFusionLSM6DS3TRC
-    {
+	= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl>;
-        using SoftFusionLSM6DS3TRC = SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl>;
+using SoftFusionICM42688
-        using SoftFusionICM42688 = SoftFusionSensor<SoftFusion::Drivers::ICM42688, SoftFusion::I2CImpl>;
+	= SoftFusionSensor<SoftFusion::Drivers::ICM42688, SoftFusion::I2CImpl>;
-        using SoftFusionBMI270 = SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl>;
+using SoftFusionBMI270
-        using SoftFusionLSM6DSV = SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl>;
+	= SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl>;
-        using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
+using SoftFusionLSM6DSV
-        using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
+	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl>;
-        using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
+using SoftFusionLSM6DSO
+	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
+using SoftFusionLSM6DSR
+	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
+using SoftFusionMPU6050
+	= SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
 
-        // TODO Make it more generic in the future and move another place (abstract sensor interface)
+// TODO Make it more generic in the future and move another place (abstract sensor
-        void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin)
+// interface)
-        {
+void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) {
-            if (sclPin != activeSCL || sdaPin != activeSDA || !running) {
+	if (sclPin != activeSCL || sdaPin != activeSDA || !running) {
-                Wire.flush();
+		Wire.flush();
-                #if ESP32
+#if ESP32
-                    if (running) {}
+		if (running) {
-                    else {
+		} else {
-                        // Reset HWI2C to avoid being affected by I2CBUS reset
+			// Reset HWI2C to avoid being affected by I2CBUS reset
-                        Wire.end();
+			Wire.end();
-                    }
+		}
-                    // Disconnect pins from HWI2C
+		// Disconnect pins from HWI2C
-                    gpio_set_direction((gpio_num_t)activeSCL, GPIO_MODE_INPUT);
+		gpio_set_direction((gpio_num_t)activeSCL, GPIO_MODE_INPUT);
-                    gpio_set_direction((gpio_num_t)activeSDA, GPIO_MODE_INPUT);
+		gpio_set_direction((gpio_num_t)activeSDA, GPIO_MODE_INPUT);
 
-                    if (running) {
+		if (running) {
-                        i2c_set_pin(I2C_NUM_0, sdaPin, sclPin, false, false, I2C_MODE_MASTER);
+			i2c_set_pin(I2C_NUM_0, sdaPin, sclPin, false, false, I2C_MODE_MASTER);
-                    } else {
+		} else {
-                        Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin), I2C_SPEED);
+			Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin), I2C_SPEED);
-                        Wire.setTimeOut(150);
+			Wire.setTimeOut(150);
-                    }
+		}
-                #else
+#else
-                    Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin));
+		Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin));
-                #endif
+#endif
 
-                activeSCL = sclPin;
+		activeSCL = sclPin;
-                activeSDA = sdaPin;
+		activeSDA = sdaPin;
-            }
+	}
-        }
+}
 
-        void SensorManager::setup()
+void SensorManager::setup() {
-        {
+	running = false;
-            running = false;
+	activeSCL = PIN_IMU_SCL;
-            activeSCL = PIN_IMU_SCL;
+	activeSDA = PIN_IMU_SDA;
-            activeSDA = PIN_IMU_SDA;
 
-            uint8_t sensorID = 0;
+	uint8_t sensorID = 0;
-            uint8_t activeSensorCount = 0;
+	uint8_t activeSensorCount = 0;
-#define IMU_DESC_ENTRY(ImuType, ...)                                  \
+#define IMU_DESC_ENTRY(ImuType, ...)                               \
-            {                                                         \
+	{                                                              \
-                auto sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__); \
+		auto sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__); \
-                if (sensor->isWorking()) {                            \
+		if (sensor->isWorking()) {                                 \
-                    m_Logger.info("Sensor %d configured", sensorID+1);\
+			m_Logger.info("Sensor %d configured", sensorID + 1);   \
-                    activeSensorCount++;                              \
+			activeSensorCount++;                                   \
-                }                                                     \
+		}                                                          \
-                m_Sensors.push_back(std::move(sensor));               \
+		m_Sensors.push_back(std::move(sensor));                    \
-                sensorID++;                                           \
+		sensorID++;                                                \
-            }
+	}
-            // Apply descriptor list and expand to entrys
+	// Apply descriptor list and expand to entrys
-            IMU_DESC_LIST;
+	IMU_DESC_LIST;
 
 #undef IMU_DESC_ENTRY
-            m_Logger.info("%d sensor(s) configured", activeSensorCount);
+	m_Logger.info("%d sensor(s) configured", activeSensorCount);
-            // Check and scan i2c if no sensors active
+	// Check and scan i2c if no sensors active
-            if (activeSensorCount == 0) {
+	if (activeSensorCount == 0) {
-                m_Logger.error("Can't find I2C device on provided addresses, scanning for all I2C devices and returning");
+		m_Logger.error(
-                I2CSCAN::scani2cports();
+			"Can't find I2C device on provided addresses, scanning for all I2C devices "
-            }
+			"and returning"
-        }
+		);
-
+		I2CSCAN::scani2cports();
-        void SensorManager::postSetup()
+	}
-        {
-            running = true;
-            for (auto &sensor : m_Sensors) {
-                if (sensor->isWorking()) {
-                    swapI2C(sensor->sclPin, sensor->sdaPin);
-                    sensor->postSetup();
-                }
-            }
-        }
-
-        void SensorManager::update()
-        {
-            // Gather IMU data
-            bool allIMUGood = true;
-            for (auto &sensor : m_Sensors) {
-                if (sensor->isWorking()) {
-                    swapI2C(sensor->sclPin, sensor->sdaPin);
-                    sensor->motionLoop();
-                }
-                if (sensor->getSensorState() == SensorStatus::SENSOR_ERROR)
-                {
-                    allIMUGood = false;
-                }
-            }
-
-            statusManager.setStatus(SlimeVR::Status::IMU_ERROR, !allIMUGood);
-
-            if (!networkConnection.isConnected()) {
-                return;
-            }
-
-            #ifndef PACKET_BUNDLING
-                static_assert(false, "PACKET_BUNDLING not set");
-            #endif
-            #if PACKET_BUNDLING == PACKET_BUNDLING_BUFFERED
-                uint32_t now = micros();
-                bool shouldSend = false;
-                bool allSensorsReady = true;
-                for (auto &sensor : m_Sensors) {
-                    if (!sensor->isWorking()) continue;
-                    if (sensor->hasNewDataToSend()) shouldSend = true;
-                    allSensorsReady &= sensor->hasNewDataToSend();
-                }
-
-                if (now - m_LastBundleSentAtMicros < PACKET_BUNDLING_BUFFER_SIZE_MICROS) {
-                    shouldSend &= allSensorsReady;
-                }
-
-                if (!shouldSend) {
-                    return;
-                }
-
-                m_LastBundleSentAtMicros = now;
-            #endif
-            
-            #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
-                networkConnection.beginBundle();
-            #endif
-
-            for (auto &sensor : m_Sensors) {
-                if (sensor->isWorking()) {
-                    sensor->sendData();
-                }
-            }
-
-            #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
-                networkConnection.endBundle();
-            #endif
-        }
-
-    }
 }
+
+void SensorManager::postSetup() {
+	running = true;
+	for (auto& sensor : m_Sensors) {
+		if (sensor->isWorking()) {
+			swapI2C(sensor->sclPin, sensor->sdaPin);
+			sensor->postSetup();
+		}
+	}
+}
+
+void SensorManager::update() {
+	// Gather IMU data
+	bool allIMUGood = true;
+	for (auto& sensor : m_Sensors) {
+		if (sensor->isWorking()) {
+			swapI2C(sensor->sclPin, sensor->sdaPin);
+			sensor->motionLoop();
+		}
+		if (sensor->getSensorState() == SensorStatus::SENSOR_ERROR) {
+			allIMUGood = false;
+		}
+	}
+
+	statusManager.setStatus(SlimeVR::Status::IMU_ERROR, !allIMUGood);
+
+	if (!networkConnection.isConnected()) {
+		return;
+	}
+
+#ifndef PACKET_BUNDLING
+	static_assert(false, "PACKET_BUNDLING not set");
+#endif
+#if PACKET_BUNDLING == PACKET_BUNDLING_BUFFERED
+	uint32_t now = micros();
+	bool shouldSend = false;
+	bool allSensorsReady = true;
+	for (auto& sensor : m_Sensors) {
+		if (!sensor->isWorking()) {
+			continue;
+		}
+		if (sensor->hasNewDataToSend()) {
+			shouldSend = true;
+		}
+		allSensorsReady &= sensor->hasNewDataToSend();
+	}
+
+	if (now - m_LastBundleSentAtMicros < PACKET_BUNDLING_BUFFER_SIZE_MICROS) {
+		shouldSend &= allSensorsReady;
+	}
+
+	if (!shouldSend) {
+		return;
+	}
+
+	m_LastBundleSentAtMicros = now;
+#endif
+
+#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
+	networkConnection.beginBundle();
+#endif
+
+	for (auto& sensor : m_Sensors) {
+		if (sensor->isWorking()) {
+			sensor->sendData();
+		}
+	}
+
+#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
+	networkConnection.endBundle();
+#endif
+}
+
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/SensorManager.h

@@ -1,112 +1,136 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 TheDevMinerTV
+	Copyright (c) 2022 TheDevMinerTV
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SLIMEVR_SENSORMANAGER
 #define SLIMEVR_SENSORMANAGER
 
-#include "globals.h"
-#include "sensor.h"
-#include "EmptySensor.h"
-#include "ErroneousSensor.h"
-#include "logging/Logger.h"
-
 #include <i2cscan.h>
 
 #include <memory>
 
+#include "EmptySensor.h"
+#include "ErroneousSensor.h"
+#include "globals.h"
+#include "logging/Logger.h"
+#include "sensor.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Sensors {
-    namespace Sensors
+class SensorManager {
-    {
+public:
-        class SensorManager
+	SensorManager()
-        {
+		: m_Logger(SlimeVR::Logging::Logger("SensorManager")) {}
-        public:
+	void setup();
-            SensorManager()
+	void postSetup();
-                : m_Logger(SlimeVR::Logging::Logger("SensorManager")) { }
-            void setup();
-            void postSetup();
 
-            void update();
+	void update();
-            
-            std::vector<std::unique_ptr<Sensor>> & getSensors() { return m_Sensors; };
-            ImuID getSensorType(size_t id) {
-                if(id < m_Sensors.size()) {
-                    return m_Sensors[id]->getSensorType();
-                }
-                return ImuID::Unknown;
-            }
 
-        private:
+	std::vector<std::unique_ptr<Sensor>>& getSensors() { return m_Sensors; };
-            SlimeVR::Logging::Logger m_Logger;
+	ImuID getSensorType(size_t id) {
+		if (id < m_Sensors.size()) {
+			return m_Sensors[id]->getSensorType();
+		}
+		return ImuID::Unknown;
+	}
 
-            std::vector<std::unique_ptr<Sensor>> m_Sensors;
+private:
+	SlimeVR::Logging::Logger m_Logger;
 
-            template <typename ImuType>
+	std::vector<std::unique_ptr<Sensor>> m_Sensors;
-            std::unique_ptr<Sensor> buildSensor(uint8_t sensorID, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, bool optional = false, int extraParam = 0)
+
-            {
+	template <typename ImuType>
-                const uint8_t address = ImuType::Address + addrSuppl;
+	std::unique_ptr<Sensor> buildSensor(
-                m_Logger.trace("Building IMU with: id=%d,\n\
+		uint8_t sensorID,
+		uint8_t addrSuppl,
+		float rotation,
+		uint8_t sclPin,
+		uint8_t sdaPin,
+		bool optional = false,
+		int extraParam = 0
+	) {
+		const uint8_t address = ImuType::Address + addrSuppl;
+		m_Logger.trace(
+			"Building IMU with: id=%d,\n\
                                 address=0x%02X, rotation=%f,\n\
                                 sclPin=%d, sdaPin=%d, extraParam=%d, optional=%d",
-                                sensorID, address, rotation,
+			sensorID,
-                                sclPin, sdaPin, extraParam, optional);
+			address,
+			rotation,
+			sclPin,
+			sdaPin,
+			extraParam,
+			optional
+		);
 
-                // Now start detecting and building the IMU
+		// Now start detecting and building the IMU
-                std::unique_ptr<Sensor> sensor;
+		std::unique_ptr<Sensor> sensor;
 
-                // Clear and reset I2C bus for each sensor upon startup
+		// Clear and reset I2C bus for each sensor upon startup
-                I2CSCAN::clearBus(sdaPin, sclPin);
+		I2CSCAN::clearBus(sdaPin, sclPin);
-                swapI2C(sclPin, sdaPin);
+		swapI2C(sclPin, sdaPin);
 
-                if (I2CSCAN::hasDevOnBus(address)) {
+		if (I2CSCAN::hasDevOnBus(address)) {
-                    m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address);
+			m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address);
-                } else {
+		} else {
-                    if (!optional) {
+			if (!optional) {
-                        m_Logger.error("Mandatory sensor %d not found at address 0x%02X", sensorID + 1, address);
+				m_Logger.error(
-                        sensor = std::make_unique<ErroneousSensor>(sensorID, ImuType::TypeID);
+					"Mandatory sensor %d not found at address 0x%02X",
-                    }
+					sensorID + 1,
-                    else {
+					address
-                        m_Logger.debug("Optional sensor %d not found at address 0x%02X", sensorID + 1, address);
+				);
-                        sensor = std::make_unique<EmptySensor>(sensorID);
+				sensor = std::make_unique<ErroneousSensor>(sensorID, ImuType::TypeID);
-                    }
+			} else {
-                    return sensor;
+				m_Logger.debug(
-                }
+					"Optional sensor %d not found at address 0x%02X",
+					sensorID + 1,
+					address
+				);
+				sensor = std::make_unique<EmptySensor>(sensorID);
+			}
+			return sensor;
+		}
 
-                uint8_t intPin = extraParam;
+		uint8_t intPin = extraParam;
-                sensor = std::make_unique<ImuType>(sensorID, addrSuppl, rotation, sclPin, sdaPin, intPin);
+		sensor = std::make_unique<ImuType>(
+			sensorID,
+			addrSuppl,
+			rotation,
+			sclPin,
+			sdaPin,
+			intPin
+		);
 
-                sensor->motionSetup();
+		sensor->motionSetup();
-                return sensor;
+		return sensor;
-            }            
+	}
-            uint8_t activeSCL = 0;
+	uint8_t activeSCL = 0;
-            uint8_t activeSDA = 0;
+	uint8_t activeSDA = 0;
-            bool running = false;
+	bool running = false;
-            void swapI2C(uint8_t scl, uint8_t sda);
+	void swapI2C(uint8_t scl, uint8_t sda);
-            
-            uint32_t m_LastBundleSentAtMicros = micros();
-        };
-    }
-}
 
-#endif // SLIMEVR_SENSORFACTORY_H_
+	uint32_t m_LastBundleSentAtMicros = micros();
+};
+}  // namespace Sensors
+}  // namespace SlimeVR
+
+#endif  // SLIMEVR_SENSORFACTORY_H_

src/sensors/axisremap.h

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2023 SlimeVR Contributors
+	Copyright (c) 2023 SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #ifndef AXISREMAP_H
 #define AXISREMAP_H
@@ -26,9 +26,9 @@
 // Number label for axis selections
 // 3 bits for each axis, 9 bits for one sensor, 18 bits at total
 // bit 1-0
-#define AXIS_REMAP_USE_X  0
+#define AXIS_REMAP_USE_X 0
-#define AXIS_REMAP_USE_Y  1
+#define AXIS_REMAP_USE_Y 1
-#define AXIS_REMAP_USE_Z  2
+#define AXIS_REMAP_USE_Z 2
 // bit 2
 #define AXIS_REMAP_INVERT 4
 
@@ -54,32 +54,46 @@
 #define AXIS_REMAP_AXIS_MAGY(y) ((y & 0x07) << 12)
 #define AXIS_REMAP_AXIS_MAGZ(z) ((z & 0x07) << 15)
 
-#define AXIS_REMAP_BUILD(x, y, z, mx, my, mz) \
+#define AXIS_REMAP_BUILD(x, y, z, mx, my, mz)                           \
-    (AXIS_REMAP_AXIS_X(x) | AXIS_REMAP_AXIS_Y(y) | AXIS_REMAP_AXIS_Z(z) | \
+	(AXIS_REMAP_AXIS_X(x) | AXIS_REMAP_AXIS_Y(y) | AXIS_REMAP_AXIS_Z(z) \
-     AXIS_REMAP_AXIS_MAGX(mx) | AXIS_REMAP_AXIS_MAGY(my) | AXIS_REMAP_AXIS_MAGZ(mz))
+	 | AXIS_REMAP_AXIS_MAGX(mx) | AXIS_REMAP_AXIS_MAGY(my) | AXIS_REMAP_AXIS_MAGZ(mz))
 
-#define AXIS_REMAP_DEFAULT AXIS_REMAP_BUILD(AXIS_REMAP_USE_X, AXIS_REMAP_USE_Y, AXIS_REMAP_USE_Z, \
+#define AXIS_REMAP_DEFAULT \
-                                                AXIS_REMAP_USE_X, AXIS_REMAP_USE_Y, AXIS_REMAP_USE_Z)
+	AXIS_REMAP_BUILD(      \
+		AXIS_REMAP_USE_X,  \
+		AXIS_REMAP_USE_Y,  \
+		AXIS_REMAP_USE_Z,  \
+		AXIS_REMAP_USE_X,  \
+		AXIS_REMAP_USE_Y,  \
+		AXIS_REMAP_USE_Z   \
+	)
 
 // Template functions for remapping
-template<typename T>
+template <typename T>
 T inline remapOneAxis(int axisdesc, T x, T y, T z) {
-    T result;
+	T result;
-    switch (axisdesc & 0x3) {
+	switch (axisdesc & 0x3) {
-        case AXIS_REMAP_USE_X: result = x; break;
+		case AXIS_REMAP_USE_X:
-        case AXIS_REMAP_USE_Y: result = y; break;
+			result = x;
-        case AXIS_REMAP_USE_Z: result = z; break;
+			break;
-        default: result = 0;
+		case AXIS_REMAP_USE_Y:
-    }
+			result = y;
-    return (axisdesc & AXIS_REMAP_INVERT) ? -result : result;
+			break;
+		case AXIS_REMAP_USE_Z:
+			result = z;
+			break;
+		default:
+			result = 0;
+	}
+	return (axisdesc & AXIS_REMAP_INVERT) ? -result : result;
 }
 
-template<typename T>
+template <typename T>
-void inline remapAllAxis(int axisdesc, T *x, T *y, T *z) {
+void inline remapAllAxis(int axisdesc, T* x, T* y, T* z) {
-    T bx = *x, by = *y, bz = *z;
+	T bx = *x, by = *y, bz = *z;
-    *x = remapOneAxis(AXIS_REMAP_GET_X(axisdesc), bx, by, bz);
+	*x = remapOneAxis(AXIS_REMAP_GET_X(axisdesc), bx, by, bz);
-    *y = remapOneAxis(AXIS_REMAP_GET_Y(axisdesc), bx, by, bz);
+	*y = remapOneAxis(AXIS_REMAP_GET_Y(axisdesc), bx, by, bz);
-    *z = remapOneAxis(AXIS_REMAP_GET_Z(axisdesc), bx, by, bz);
+	*z = remapOneAxis(AXIS_REMAP_GET_Z(axisdesc), bx, by, bz);
 }
 
-#endif // AXISREMAP_H
+#endif  // AXISREMAP_H

src/sensors/bmi160sensor.h

@@ -1,52 +1,51 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2022 SlimeVR Contributors
+	Copyright (c) 2022 SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SENSORS_BMI160SENSOR_H
 #define SENSORS_BMI160SENSOR_H
 
-#include "sensor.h"
-#include "sensors/axisremap.h"
-#include "magneto1.4.h"
-
 #include <BMI160.h>
-#include "SensorFusionRestDetect.h"
-#include "../motionprocessing/types.h"
 
 #include "../motionprocessing/GyroTemperatureCalibrator.h"
 #include "../motionprocessing/RestDetection.h"
+#include "../motionprocessing/types.h"
+#include "SensorFusionRestDetect.h"
+#include "magneto1.4.h"
+#include "sensor.h"
+#include "sensors/axisremap.h"
 
 #if BMI160_USE_VQF
-    #if USE_6_AXIS
+#if USE_6_AXIS
-        #define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ
+#define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ
-    #else
-        #define BMI160_GYRO_RATE BMI160_GYRO_RATE_200HZ
-    #endif
 #else
-    #if USE_6_AXIS
+#define BMI160_GYRO_RATE BMI160_GYRO_RATE_200HZ
-        #define BMI160_GYRO_RATE BMI160_GYRO_RATE_800HZ
+#endif
-    #else
+#else
-        #define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ
+#if USE_6_AXIS
-    #endif
+#define BMI160_GYRO_RATE BMI160_GYRO_RATE_800HZ
+#else
+#define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ
+#endif
 #endif
 #define BMI160_GYRO_RANGE BMI160_GYRO_RANGE_1000
 #define BMI160_GYRO_FILTER_MODE BMI160_DLPF_MODE_NORM
@@ -59,37 +58,43 @@
 
 #define BMI160_TIMESTAMP_RESOLUTION_MICROS 39.0625f
 // #define BMI160_TIMESTAMP_RESOLUTION_MICROS 39.0f
-#define BMI160_MAP_ODR_MICROS(micros) ((uint16_t)((micros) / BMI160_TIMESTAMP_RESOLUTION_MICROS) * BMI160_TIMESTAMP_RESOLUTION_MICROS)
+#define BMI160_MAP_ODR_MICROS(micros)                          \
+	((uint16_t)((micros) / BMI160_TIMESTAMP_RESOLUTION_MICROS) \
+	 * BMI160_TIMESTAMP_RESOLUTION_MICROS)
 constexpr float BMI160_ODR_GYR_HZ = 25.0f * (1 << (BMI160_GYRO_RATE - 6));
 constexpr float BMI160_ODR_ACC_HZ = 12.5f * (1 << (BMI160_ACCEL_RATE - 5));
-constexpr float BMI160_ODR_GYR_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_GYR_HZ * 1e6f);
+constexpr float BMI160_ODR_GYR_MICROS
-constexpr float BMI160_ODR_ACC_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_ACC_HZ * 1e6f);
+	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_GYR_HZ * 1e6f);
+constexpr float BMI160_ODR_ACC_MICROS
+	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_ACC_HZ * 1e6f);
 #if !USE_6_AXIS
-// note: this value only sets polling and fusion update rate - HMC is internally sampled at 75hz, QMC at 200hz
+// note: this value only sets polling and fusion update rate - HMC is internally sampled
+// at 75hz, QMC at 200hz
 #define BMI160_MAG_RATE BMI160_MAG_RATE_50HZ
-constexpr float BMI160_ODR_MAG_HZ = (25.0f/32.0f) * (1 << (BMI160_MAG_RATE - 1));
+constexpr float BMI160_ODR_MAG_HZ = (25.0f / 32.0f) * (1 << (BMI160_MAG_RATE - 1));
-constexpr float BMI160_ODR_MAG_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_MAG_HZ * 1e6f);
+constexpr float BMI160_ODR_MAG_MICROS
+	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_MAG_HZ * 1e6f);
 #else
 constexpr float BMI160_ODR_MAG_HZ = 0;
 constexpr float BMI160_ODR_MAG_MICROS = 0;
 #endif
 
-constexpr uint16_t BMI160_SETTINGS_MAX_ODR_HZ = max(max(BMI160_ODR_GYR_HZ, BMI160_ODR_ACC_HZ), BMI160_ODR_MAG_HZ);
+constexpr uint16_t BMI160_SETTINGS_MAX_ODR_HZ
-constexpr uint16_t BMI160_SETTINGS_MAX_ODR_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_SETTINGS_MAX_ODR_HZ * 1e6f);
+	= max(max(BMI160_ODR_GYR_HZ, BMI160_ODR_ACC_HZ), BMI160_ODR_MAG_HZ);
+constexpr uint16_t BMI160_SETTINGS_MAX_ODR_MICROS
+	= BMI160_MAP_ODR_MICROS(1.0f / BMI160_SETTINGS_MAX_ODR_HZ * 1e6f);
 
-constexpr float BMI160_FIFO_AVG_DATA_FRAME_LENGTH = (
+constexpr float BMI160_FIFO_AVG_DATA_FRAME_LENGTH
-    BMI160_SETTINGS_MAX_ODR_HZ * 1 +
+	= (BMI160_SETTINGS_MAX_ODR_HZ * 1 + BMI160_ODR_GYR_HZ * BMI160_FIFO_G_LEN
-    BMI160_ODR_GYR_HZ * BMI160_FIFO_G_LEN +
+	   + BMI160_ODR_ACC_HZ * BMI160_FIFO_A_LEN + BMI160_ODR_MAG_HZ * BMI160_FIFO_M_LEN)
-    BMI160_ODR_ACC_HZ * BMI160_FIFO_A_LEN +
+	/ BMI160_SETTINGS_MAX_ODR_HZ;
-    BMI160_ODR_MAG_HZ * BMI160_FIFO_M_LEN
-) / BMI160_SETTINGS_MAX_ODR_HZ;
 constexpr float BMI160_FIFO_READ_BUFFER_SIZE_MICROS = 30000;
-constexpr float BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES =
+constexpr float BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES
-    BMI160_SETTINGS_MAX_ODR_HZ * BMI160_FIFO_READ_BUFFER_SIZE_MICROS / 1e6f;
+	= BMI160_SETTINGS_MAX_ODR_HZ * BMI160_FIFO_READ_BUFFER_SIZE_MICROS / 1e6f;
 constexpr uint16_t BMI160_FIFO_MAX_LENGTH = 1024;
 constexpr uint16_t BMI160_FIFO_READ_BUFFER_SIZE_BYTES = min(
-    (float)BMI160_FIFO_MAX_LENGTH - 64,
+	(float)BMI160_FIFO_MAX_LENGTH - 64,
-    BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES * BMI160_FIFO_AVG_DATA_FRAME_LENGTH * 1.25f
+	BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES* BMI160_FIFO_AVG_DATA_FRAME_LENGTH * 1.25f
 );
 
 // Typical sensitivity at 25C
@@ -99,140 +104,167 @@ constexpr uint16_t BMI160_FIFO_READ_BUFFER_SIZE_BYTES = min(
 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 65.6f  // 500 deg   2
 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 131.2f // 250 deg   3
 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 262.4f // 125 deg   4
-constexpr double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB = (16.4f * (1 << BMI160_GYRO_RANGE));
+constexpr double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB
+	= (16.4f * (1 << BMI160_GYRO_RANGE));
 
-constexpr std::pair<uint8_t, float> BMI160_ACCEL_SENSITIVITY_LSB_MAP[] = {
+constexpr std::pair<uint8_t, float> BMI160_ACCEL_SENSITIVITY_LSB_MAP[]
-    {BMI160_ACCEL_RANGE_2G, 16384.0f},
+	= {{BMI160_ACCEL_RANGE_2G, 16384.0f},
-    {BMI160_ACCEL_RANGE_4G, 8192.0f},
+	   {BMI160_ACCEL_RANGE_4G, 8192.0f},
-    {BMI160_ACCEL_RANGE_8G, 4096.0f},
+	   {BMI160_ACCEL_RANGE_8G, 4096.0f},
-    {BMI160_ACCEL_RANGE_16G, 2048.0f}
+	   {BMI160_ACCEL_RANGE_16G, 2048.0f}};
-};
+constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB
-constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB = BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second;
+	= BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second;
-constexpr double BMI160_ASCALE = CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB;
+constexpr double BMI160_ASCALE
+	= CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB;
 
 // Scale conversion steps: LSB/°/s -> °/s -> step/°/s -> step/rad/s
-constexpr double BMI160_GSCALE = ((32768. / BMI160_GYRO_TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0);
+constexpr double BMI160_GSCALE
+	= ((32768. / BMI160_GYRO_TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0);
 
 constexpr float targetSampleRateMs = 10.0f;
 constexpr uint32_t targetSampleRateMicros = (uint32_t)targetSampleRateMs * 1e3;
 
-constexpr uint32_t BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP =
+constexpr uint32_t BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP
-    TEMP_CALIBRATION_SECONDS_PER_STEP / (BMI160_ODR_GYR_MICROS / 1e6);
+	= TEMP_CALIBRATION_SECONDS_PER_STEP / (BMI160_ODR_GYR_MICROS / 1e6);
-static_assert(0x7FFF * BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP < 0x7FFFFFFF, "Temperature calibration sum overflow");
+static_assert(
+	0x7FFF * BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP < 0x7FFFFFFF,
+	"Temperature calibration sum overflow"
+);
 
 class BMI160Sensor : public Sensor {
-    public:
+public:
-        static constexpr uint8_t Address = 0x68;
+	static constexpr uint8_t Address = 0x68;
-        static constexpr auto TypeID = ImuID::BMI160;
+	static constexpr auto TypeID = ImuID::BMI160;
 
-        BMI160Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, int axisRemapParam) :
+	BMI160Sensor(
-            Sensor("BMI160Sensor", ImuID::BMI160, id, Address+addrSuppl, rotation, sclPin, sdaPin),
+		uint8_t id,
-            sfusion(BMI160_ODR_GYR_MICROS / 1e6f, BMI160_ODR_ACC_MICROS / 1e6f, BMI160_ODR_MAG_MICROS / 1e6f)
+		uint8_t addrSuppl,
-        {
+		float rotation,
-            if (axisRemapParam < 256) {
+		uint8_t sclPin,
-                axisRemap = AXIS_REMAP_DEFAULT;
+		uint8_t sdaPin,
-            }
+		int axisRemapParam
-            else {
+	)
-                axisRemap = axisRemapParam;
+		: Sensor(
-            }
+			"BMI160Sensor",
-        };
+			ImuID::BMI160,
-        ~BMI160Sensor(){};
+			id,
-        void initHMC(BMI160MagRate magRate);
+			Address + addrSuppl,
-        void initQMC(BMI160MagRate magRate);
+			rotation,
+			sclPin,
+			sdaPin
+		)
+		, sfusion(
+			  BMI160_ODR_GYR_MICROS / 1e6f,
+			  BMI160_ODR_ACC_MICROS / 1e6f,
+			  BMI160_ODR_MAG_MICROS / 1e6f
+		  ) {
+		if (axisRemapParam < 256) {
+			axisRemap = AXIS_REMAP_DEFAULT;
+		} else {
+			axisRemap = axisRemapParam;
+		}
+	};
+	~BMI160Sensor(){};
+	void initHMC(BMI160MagRate magRate);
+	void initQMC(BMI160MagRate magRate);
 
-        void motionSetup() override final;
+	void motionSetup() override final;
-        void motionLoop() override final;
+	void motionLoop() override final;
-        void startCalibration(int calibrationType) override final;
+	void startCalibration(int calibrationType) override final;
-        void maybeCalibrateGyro();
+	void maybeCalibrateGyro();
-        void maybeCalibrateAccel();
+	void maybeCalibrateAccel();
-        void maybeCalibrateMag();
+	void maybeCalibrateMag();
 
-        void printTemperatureCalibrationState() override final;
+	void printTemperatureCalibrationState() override final;
-        void printDebugTemperatureCalibrationState() override final;
+	void printDebugTemperatureCalibrationState() override final;
-        void resetTemperatureCalibrationState() override final {
+	void resetTemperatureCalibrationState() override final {
-            gyroTempCalibrator->reset();
+		gyroTempCalibrator->reset();
-            m_Logger.info("Temperature calibration state has been reset for sensorId:%i", sensorId);
+		m_Logger.info(
-        };
+			"Temperature calibration state has been reset for sensorId:%i",
-        void saveTemperatureCalibration() override final;
+			sensorId
+		);
+	};
+	void saveTemperatureCalibration() override final;
 
-        void applyAccelCalibrationAndScale(sensor_real_t Axyz[3]);
+	void applyAccelCalibrationAndScale(sensor_real_t Axyz[3]);
-        void applyMagCalibrationAndScale(sensor_real_t Mxyz[3]);
+	void applyMagCalibrationAndScale(sensor_real_t Mxyz[3]);
 
-        bool hasGyroCalibration();
+	bool hasGyroCalibration();
-        bool hasAccelCalibration();
+	bool hasAccelCalibration();
-        bool hasMagCalibration();
+	bool hasMagCalibration();
 
-        void onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
+	void onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
-        void onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
+	void onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
-        void onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
+	void onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z);
-        void readFIFO();
+	void readFIFO();
 
-        void getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z);
+	void
+	getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z);
 
-        void remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z);
+	void remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z);
-        void remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z);
+	void remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z);
-        void getRemappedRotation(int16_t* x, int16_t* y, int16_t* z);
+	void getRemappedRotation(int16_t* x, int16_t* y, int16_t* z);
-        void getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z);
+	void getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z);
 
-        bool getTemperature(float* out);
+	bool getTemperature(float* out);
 
-    private:
+private:
-        BMI160 imu {};
+	BMI160 imu{};
-        int axisRemap;
+	int axisRemap;
 
-        SlimeVR::Sensors::SensorFusionRestDetect sfusion;
+	SlimeVR::Sensors::SensorFusionRestDetect sfusion;
 
-        // clock sync and sample timestamping
+	// clock sync and sample timestamping
-        uint32_t sensorTime0 = 0;
+	uint32_t sensorTime0 = 0;
-        uint32_t sensorTime1 = 0;
+	uint32_t sensorTime1 = 0;
-        uint32_t localTime0 = 0;
+	uint32_t localTime0 = 0;
-        uint32_t localTime1 = 0;
+	uint32_t localTime1 = 0;
-        double sensorTimeRatio = 1;
+	double sensorTimeRatio = 1;
-        double sensorTimeRatioEma = 1;
+	double sensorTimeRatioEma = 1;
-        double sampleDtMicros = BMI160_ODR_GYR_MICROS;
+	double sampleDtMicros = BMI160_ODR_GYR_MICROS;
-        uint32_t syncLatencyMicros = 0;
+	uint32_t syncLatencyMicros = 0;
-        uint32_t samplesSinceClockSync = 0;
+	uint32_t samplesSinceClockSync = 0;
-        uint32_t timestamp0 = 0;
+	uint32_t timestamp0 = 0;
-        uint32_t timestamp1 = 0;
+	uint32_t timestamp1 = 0;
 
-        // scheduling
+	// scheduling
-        uint32_t lastPollTime = micros();
+	uint32_t lastPollTime = micros();
-        uint32_t lastClockPollTime = micros();
+	uint32_t lastClockPollTime = micros();
-        #if BMI160_DEBUG
+#if BMI160_DEBUG
-        uint32_t cpuUsageMicros = 0;
+	uint32_t cpuUsageMicros = 0;
-        uint32_t lastCpuUsagePrinted = 0;
+	uint32_t lastCpuUsagePrinted = 0;
-        uint32_t gyrReads = 0;
+	uint32_t gyrReads = 0;
-        uint32_t accReads = 0;
+	uint32_t accReads = 0;
-        uint32_t magReads = 0;
+	uint32_t magReads = 0;
-        uint16_t numFIFODropped = 0;
+	uint16_t numFIFODropped = 0;
-        uint16_t numFIFOFailedReads = 0;
+	uint16_t numFIFOFailedReads = 0;
-        #endif
+#endif
 
-        uint32_t lastRotationPacketSent = 0;
+	uint32_t lastRotationPacketSent = 0;
-        uint32_t lastTemperaturePacketSent = 0;
+	uint32_t lastTemperaturePacketSent = 0;
 
-        struct BMI160FIFO {
+	struct BMI160FIFO {
-            uint8_t data[BMI160_FIFO_READ_BUFFER_SIZE_BYTES];
+		uint8_t data[BMI160_FIFO_READ_BUFFER_SIZE_BYTES];
-            uint16_t length;
+		uint16_t length;
-        } fifo {};
+	} fifo{};
-        float temperature = 0;
+	float temperature = 0;
-        GyroTemperatureCalibrator* gyroTempCalibrator = nullptr;
+	GyroTemperatureCalibrator* gyroTempCalibrator = nullptr;
-        sensor_real_t Gxyz[3] = {0};
+	sensor_real_t Gxyz[3] = {0};
-        sensor_real_t Axyz[3] = {0};
+	sensor_real_t Axyz[3] = {0};
-        sensor_real_t Mxyz[3] = {0};
+	sensor_real_t Mxyz[3] = {0};
-        sensor_real_t lastAxyz[3] = {0};
+	sensor_real_t lastAxyz[3] = {0};
 
-        double gscaleX = BMI160_GSCALE;
+	double gscaleX = BMI160_GSCALE;
-        double gscaleY = BMI160_GSCALE;
+	double gscaleY = BMI160_GSCALE;
-        double gscaleZ = BMI160_GSCALE;
+	double gscaleZ = BMI160_GSCALE;
 
-        double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0};
+	double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0};
 
-        bool isGyroCalibrated = false;
+	bool isGyroCalibrated = false;
-        bool isAccelCalibrated = false;
+	bool isAccelCalibrated = false;
-        bool isMagCalibrated = false;
+	bool isMagCalibrated = false;
 
-        SlimeVR::Configuration::BMI160SensorConfig m_Config = {};
+	SlimeVR::Configuration::BMI160SensorConfig m_Config = {};
 };
 
 #endif

src/sensors/bno055sensor.cpp

@@ -1,72 +1,80 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #include "bno055sensor.h"
-#include "globals.h"
+
 #include "GlobalVars.h"
+#include "globals.h"
 
 void BNO055Sensor::motionSetup() {
-    imu = Adafruit_BNO055(sensorId, addr);
+	imu = Adafruit_BNO055(sensorId, addr);
-    delay(3000);
+	delay(3000);
 #if USE_6_AXIS
-    if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_IMUPLUS))
+	if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_IMUPLUS))
 #else
-    if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_NDOF))
+	if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_NDOF))
 #endif
-    {
+	{
-        m_Logger.fatal("Can't connect to BNO055 at address 0x%02x", addr);
+		m_Logger.fatal("Can't connect to BNO055 at address 0x%02x", addr);
-        ledManager.pattern(50, 50, 200);
+		ledManager.pattern(50, 50, 200);
-        return;
+		return;
-    }
+	}
 
-    delay(1000);
+	delay(1000);
-    imu.setExtCrystalUse(true); //Adafruit BNO055's use external crystal. Enable it, otherwise it does not work.
+	imu.setExtCrystalUse(true);  // Adafruit BNO055's use external crystal. Enable it,
-    imu.setAxisRemap(Adafruit_BNO055::REMAP_CONFIG_P0);
+								 // otherwise it does not work.
-    imu.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P0);
+	imu.setAxisRemap(Adafruit_BNO055::REMAP_CONFIG_P0);
-    m_Logger.info("Connected to BNO055 at address 0x%02x", addr);
+	imu.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P0);
+	m_Logger.info("Connected to BNO055 at address 0x%02x", addr);
 
-    working = true;
+	working = true;
 }
 
 void BNO055Sensor::motionLoop() {
 #if ENABLE_INSPECTION
-    {
+	{
-        Vector3 gyro = imu.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
+		Vector3 gyro = imu.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
-        Vector3 accel = imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
+		Vector3 accel = imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
-        Vector3 mag = imu.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
+		Vector3 mag = imu.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
 
-        networkConnection.sendInspectionRawIMUData(sensorId, UNPACK_VECTOR(gyro), 255, UNPACK_VECTOR(accel), 255, UNPACK_VECTOR(mag), 255);
+		networkConnection.sendInspectionRawIMUData(
-    }
+			sensorId,
+			UNPACK_VECTOR(gyro),
+			255,
+			UNPACK_VECTOR(accel),
+			255,
+			UNPACK_VECTOR(mag),
+			255
+		);
+	}
 #endif
 
-    // TODO Optimize a bit with setting rawQuat directly
+	// TODO Optimize a bit with setting rawQuat directly
-    setFusedRotation(imu.getQuat());
+	setFusedRotation(imu.getQuat());
-    hadData = true;
+	hadData = true;
 
 #if SEND_ACCELERATION
-    setAcceleration(imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL));
+	setAcceleration(imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL));
 #endif
 }
 
-void BNO055Sensor::startCalibration(int calibrationType) {
+void BNO055Sensor::startCalibration(int calibrationType) {}
-
-}

src/sensors/bno055sensor.h

@@ -1,49 +1,63 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SENSORS_BNO055SENSOR_H
 #define SENSORS_BNO055SENSOR_H
 
-#include "sensor.h"
-
 #include <Adafruit_BNO055.h>
 
-class BNO055Sensor : public Sensor
+#include "sensor.h"
-{
-public:
-    static constexpr auto TypeID = ImuID::BNO055;
-    static constexpr uint8_t Address = 0x28;
 
-    BNO055Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t)
+class BNO055Sensor : public Sensor {
-        : Sensor("BNO055Sensor", ImuID::BNO055, id, Address+addrSuppl, rotation, sclPin, sdaPin){};
+public:
-    ~BNO055Sensor(){};
+	static constexpr auto TypeID = ImuID::BNO055;
-    void motionSetup() override final;
+	static constexpr uint8_t Address = 0x28;
-    void motionLoop() override final;
+
-    void startCalibration(int calibrationType) override final;
+	BNO055Sensor(
+		uint8_t id,
+		uint8_t addrSuppl,
+		float rotation,
+		uint8_t sclPin,
+		uint8_t sdaPin,
+		uint8_t
+	)
+		: Sensor(
+			"BNO055Sensor",
+			ImuID::BNO055,
+			id,
+			Address + addrSuppl,
+			rotation,
+			sclPin,
+			sdaPin
+		){};
+	~BNO055Sensor(){};
+	void motionSetup() override final;
+	void motionLoop() override final;
+	void startCalibration(int calibrationType) override final;
 
 private:
-    Adafruit_BNO055 imu;
+	Adafruit_BNO055 imu;
-    SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {};
+	SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {};
 };
 
 #endif

src/sensors/bno080sensor.cpp

@@ -1,231 +1,286 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #include "sensors/bno080sensor.h"
-#include "utils.h"
+
 #include "GlobalVars.h"
+#include "utils.h"
 
-void BNO080Sensor::motionSetup()
+void BNO080Sensor::motionSetup() {
-{
 #ifdef DEBUG_SENSOR
-    imu.enableDebugging(Serial);
+	imu.enableDebugging(Serial);
 #endif
-    if(!imu.begin(addr, Wire, m_IntPin)) {
+	if (!imu.begin(addr, Wire, m_IntPin)) {
-        m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr);
+		m_Logger.fatal(
-        ledManager.pattern(50, 50, 200);
+			"Can't connect to %s at address 0x%02x",
-        return;
+			getIMUNameByType(sensorType),
-    }
+			addr
+		);
+		ledManager.pattern(50, 50, 200);
+		return;
+	}
 
-    m_Logger.info("Connected to %s on 0x%02x. "
+	m_Logger.info(
-                  "Info: SW Version Major: 0x%02x "
+		"Connected to %s on 0x%02x. "
-                  "SW Version Minor: 0x%02x "
+		"Info: SW Version Major: 0x%02x "
-                  "SW Part Number: 0x%02x "
+		"SW Version Minor: 0x%02x "
-                  "SW Build Number: 0x%02x "
+		"SW Part Number: 0x%02x "
-                  "SW Version Patch: 0x%02x",
+		"SW Build Number: 0x%02x "
-                  getIMUNameByType(sensorType),
+		"SW Version Patch: 0x%02x",
-                  addr,
+		getIMUNameByType(sensorType),
-                  imu.swMajor,
+		addr,
-                  imu.swMinor,
+		imu.swMajor,
-                  imu.swPartNumber,
+		imu.swMinor,
-                  imu.swBuildNumber,
+		imu.swPartNumber,
-                  imu.swVersionPatch
+		imu.swBuildNumber,
-                );
+		imu.swVersionPatch
+	);
 
-    this->imu.enableLinearAccelerometer(10);
+	this->imu.enableLinearAccelerometer(10);
 
-    SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId);
+	SlimeVR::Configuration::SensorConfig sensorConfig
-    // If no compatible calibration data is found, the calibration data will just be zero-ed out
+		= configuration.getSensor(sensorId);
-    switch (sensorConfig.type) {
+	// If no compatible calibration data is found, the calibration data will just be
-    case SlimeVR::Configuration::SensorConfigType::BNO0XX:
+	// zero-ed out
-        m_Config = sensorConfig.data.bno0XX;
+	switch (sensorConfig.type) {
-        magStatus = m_Config.magEnabled ? MagnetometerStatus::MAG_ENABLED
+		case SlimeVR::Configuration::SensorConfigType::BNO0XX:
-										: MagnetometerStatus::MAG_DISABLED;
+			m_Config = sensorConfig.data.bno0XX;
-        break;
+			magStatus = m_Config.magEnabled ? MagnetometerStatus::MAG_ENABLED
-    default:
+											: MagnetometerStatus::MAG_DISABLED;
-        // Ignore lack of config for BNO, by default use from FW build
+			break;
-		magStatus = USE_6_AXIS ? MagnetometerStatus::MAG_DISABLED
+		default:
-								: MagnetometerStatus::MAG_ENABLED;
+			// Ignore lack of config for BNO, by default use from FW build
-        break;
+			magStatus = USE_6_AXIS ? MagnetometerStatus::MAG_DISABLED
-    }
+								   : MagnetometerStatus::MAG_ENABLED;
+			break;
+	}
 
-    if(!isMagEnabled()) {
+	if (!isMagEnabled()) {
-        if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) {
+		if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086)
-            imu.enableARVRStabilizedGameRotationVector(10);
+			&& BNO_USE_ARVR_STABILIZATION) {
-        } else {
+			imu.enableARVRStabilizedGameRotationVector(10);
-            imu.enableGameRotationVector(10);
+		} else {
-        }
+			imu.enableGameRotationVector(10);
-    } else {
+		}
-        if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) {
+	} else {
-            imu.enableARVRStabilizedRotationVector(10);
+		if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086)
-        } else {
+			&& BNO_USE_ARVR_STABILIZATION) {
-            imu.enableRotationVector(10);
+			imu.enableARVRStabilizedRotationVector(10);
-        }
+		} else {
-    }
+			imu.enableRotationVector(10);
+		}
+	}
 
 #if ENABLE_INSPECTION
-    imu.enableRawGyro(10);
+	imu.enableRawGyro(10);
-    imu.enableRawAccelerometer(10);
+	imu.enableRawAccelerometer(10);
-    imu.enableRawMagnetometer(10);
+	imu.enableRawMagnetometer(10);
 #endif
 
-    lastReset = 0;
+	lastReset = 0;
-    lastData = millis();
+	lastData = millis();
-    working = true;
+	working = true;
-    configured = true;
+	configured = true;
 }
 
-void BNO080Sensor::motionLoop()
+void BNO080Sensor::motionLoop() {
-{
+	// Look for reports from the IMU
-    //Look for reports from the IMU
+	while (imu.dataAvailable()) {
-    while (imu.dataAvailable())
+		hadData = true;
-    {
-        hadData = true;
 #if ENABLE_INSPECTION
-        {
+		{
-            int16_t rX = imu.getRawGyroX();
+			int16_t rX = imu.getRawGyroX();
-            int16_t rY = imu.getRawGyroY();
+			int16_t rY = imu.getRawGyroY();
-            int16_t rZ = imu.getRawGyroZ();
+			int16_t rZ = imu.getRawGyroZ();
-            uint8_t rA = imu.getGyroAccuracy();
+			uint8_t rA = imu.getGyroAccuracy();
 
-            int16_t aX = imu.getRawAccelX();
+			int16_t aX = imu.getRawAccelX();
-            int16_t aY = imu.getRawAccelY();
+			int16_t aY = imu.getRawAccelY();
-            int16_t aZ = imu.getRawAccelZ();
+			int16_t aZ = imu.getRawAccelZ();
-            uint8_t aA = imu.getAccelAccuracy();
+			uint8_t aA = imu.getAccelAccuracy();
 
-            int16_t mX = imu.getRawMagX();
+			int16_t mX = imu.getRawMagX();
-            int16_t mY = imu.getRawMagY();
+			int16_t mY = imu.getRawMagY();
-            int16_t mZ = imu.getRawMagZ();
+			int16_t mZ = imu.getRawMagZ();
-            uint8_t mA = imu.getMagAccuracy();
+			uint8_t mA = imu.getMagAccuracy();
 
-            networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA);
+			networkConnection.sendInspectionRawIMUData(
-        }
+				sensorId,
+				rX,
+				rY,
+				rZ,
+				rA,
+				aX,
+				aY,
+				aZ,
+				aA,
+				mX,
+				mY,
+				mZ,
+				mA
+			);
+		}
 #endif
 
-        lastReset = 0;
+		lastReset = 0;
-        lastData = millis();
+		lastData = millis();
 
-        if(!isMagEnabled()) {
+		if (!isMagEnabled()) {
-                if (imu.hasNewGameQuat()) // New quaternion if context
+			if (imu.hasNewGameQuat())  // New quaternion if context
-                {
+			{
-                    Quat nRotation;
+				Quat nRotation;
-                    imu.getGameQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, calibrationAccuracy);
+				imu.getGameQuat(
+					nRotation.x,
+					nRotation.y,
+					nRotation.z,
+					nRotation.w,
+					calibrationAccuracy
+				);
 
-                    setFusedRotation(nRotation);
+				setFusedRotation(nRotation);
-                    // Leave new quaternion if context open, it's closed later
+				// Leave new quaternion if context open, it's closed later
-                }
+			}
-        } else {
+		} else {
+			if (imu.hasNewQuat())  // New quaternion if context
+			{
+				Quat nRotation;
+				imu.getQuat(
+					nRotation.x,
+					nRotation.y,
+					nRotation.z,
+					nRotation.w,
+					magneticAccuracyEstimate,
+					calibrationAccuracy
+				);
 
-                if (imu.hasNewQuat()) // New quaternion if context
+				setFusedRotation(nRotation);
-                {
-                    Quat nRotation;
-                    imu.getQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, magneticAccuracyEstimate, calibrationAccuracy);
 
-                    setFusedRotation(nRotation);
+				// Leave new quaternion if context open, it's closed later
+			}  // Closing new quaternion if context
+		}
 
-                    // Leave new quaternion if context open, it's closed later
+		// Continuation of the new quaternion if context, used for both 6 and 9 axis
-                } // Closing new quaternion if context
-        }
-
-        // Continuation of the new quaternion if context, used for both 6 and 9 axis
 #if SEND_ACCELERATION
-        {
+		{
-            uint8_t acc;
+			uint8_t acc;
-            Vector3 nAccel;
+			Vector3 nAccel;
-            imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc);
+			imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc);
-            setAcceleration(nAccel);
+			setAcceleration(nAccel);
-        }
+		}
-#endif // SEND_ACCELERATION
+#endif  // SEND_ACCELERATION
 
-        if (imu.getTapDetected())
+		if (imu.getTapDetected()) {
-        {
+			tap = imu.getTapDetector();
-            tap = imu.getTapDetector();
+		}
-        }
+		if (m_IntPin == 255 || imu.I2CTimedOut()) {
-        if (m_IntPin == 255 || imu.I2CTimedOut())
+			break;
-            break;
+		}
-    }
+	}
-    if (lastData + 1000 < millis() && configured)
+	if (lastData + 1000 < millis() && configured) {
-    {
+		while (true) {
-        while(true) {
+			BNO080Error error = imu.readError();
-            BNO080Error error = imu.readError();
+			if (error.error_source == 255) {
-            if(error.error_source == 255)
+				break;
-                break;
+			}
-            lastError = error;
+			lastError = error;
-            m_Logger.error("BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d",
+			m_Logger.error(
-                error.severity, error.error_sequence_number, error.error_source, error.error, error.error_module, error.error_code);
+				"BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: "
-        }
+				"%d",
-        statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true);
+				error.severity,
-        working = false;
+				error.error_sequence_number,
-        lastData = millis();
+				error.error_source,
-        uint8_t rr = imu.resetReason();
+				error.error,
-        if (rr != lastReset)
+				error.error_module,
-        {
+				error.error_code
-            lastReset = rr;
+			);
-            networkConnection.sendSensorError(this->sensorId, rr);
+		}
-        }
+		statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true);
+		working = false;
+		lastData = millis();
+		uint8_t rr = imu.resetReason();
+		if (rr != lastReset) {
+			lastReset = rr;
+			networkConnection.sendSensorError(this->sensorId, rr);
+		}
 
-        m_Logger.error("Sensor %d doesn't respond. Last reset reason:", sensorId, lastReset);
+		m_Logger.error(
-        m_Logger.error("Last error: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d",
+			"Sensor %d doesn't respond. Last reset reason:",
-                lastError.severity, lastError.error_sequence_number, lastError.error_source, lastError.error, lastError.error_module, lastError.error_code);
+			sensorId,
-    }
+			lastReset
+		);
+		m_Logger.error(
+			"Last error: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d",
+			lastError.severity,
+			lastError.error_sequence_number,
+			lastError.error_source,
+			lastError.error,
+			lastError.error_module,
+			lastError.error_code
+		);
+	}
 }
 
-SensorStatus BNO080Sensor::getSensorState()
+SensorStatus BNO080Sensor::getSensorState() {
-{
+	return lastReset > 0 ? SensorStatus::SENSOR_ERROR
-    return lastReset > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE;
+		 : isWorking()   ? SensorStatus::SENSOR_OK
+						 : SensorStatus::SENSOR_OFFLINE;
 }
 
-void BNO080Sensor::sendData()
+void BNO080Sensor::sendData() {
-{
+	if (newFusedRotation) {
-    if (newFusedRotation)
+		newFusedRotation = false;
-    {
+		networkConnection.sendRotationData(
-        newFusedRotation = false;
+			sensorId,
-        networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy);
+			&fusedRotation,
+			DATA_TYPE_NORMAL,
+			calibrationAccuracy
+		);
 
 #ifdef DEBUG_SENSOR
-        m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation));
+		m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation));
 #endif
 
 #if SEND_ACCELERATION
-        if (newAcceleration)
+		if (newAcceleration) {
-        {
+			newAcceleration = false;
-            newAcceleration = false;
+			networkConnection.sendSensorAcceleration(
-            networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration);
+				this->sensorId,
-        }
+				this->acceleration
+			);
+		}
 #endif
-    }
+	}
 
-    if (tap != 0)
+	if (tap != 0) {
-    {
+		networkConnection.sendSensorTap(sensorId, tap);
-        networkConnection.sendSensorTap(sensorId, tap);
+		tap = 0;
-        tap = 0;
+	}
-    }
 }
 
-void BNO080Sensor::setFlag(uint16_t flagId, bool state)
+void BNO080Sensor::setFlag(uint16_t flagId, bool state) {
-{
+	if (flagId == FLAG_SENSOR_BNO0XX_MAG_ENABLED) {
-    if(flagId == FLAG_SENSOR_BNO0XX_MAG_ENABLED) {
+		m_Config.magEnabled = state;
-        m_Config.magEnabled = state;
+		magStatus = state ? MagnetometerStatus::MAG_ENABLED
-        magStatus = state ? MagnetometerStatus::MAG_ENABLED
 						  : MagnetometerStatus::MAG_DISABLED;
 
 		SlimeVR::Configuration::SensorConfig config;
@@ -238,9 +293,8 @@ void BNO080Sensor::setFlag(uint16_t flagId, bool state)
 	}
 }
 
-void BNO080Sensor::startCalibration(int calibrationType)
+void BNO080Sensor::startCalibration(int calibrationType) {
-{
+	// BNO does automatic calibration,
-    // BNO does automatic calibration,
+	// it's always enabled except accelerometer
-    // it's always enabled except accelerometer
+	// that is disabled 30 seconds after startup
-    // that is disabled 30 seconds after startup
 }

src/sensors/bno080sensor.h

@@ -1,91 +1,146 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SENSORS_BNO080SENSOR_H
 #define SENSORS_BNO080SENSOR_H
 
-#include "sensor.h"
 #include <BNO080.h>
 
+#include "sensor.h"
+
 #define FLAG_SENSOR_BNO0XX_MAG_ENABLED 1
 
-class BNO080Sensor : public Sensor
+class BNO080Sensor : public Sensor {
-{
 public:
-    static constexpr auto TypeID = ImuID::BNO080;
+	static constexpr auto TypeID = ImuID::BNO080;
-    static constexpr uint8_t Address = 0x4a;
+	static constexpr uint8_t Address = 0x4a;
 
-    BNO080Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin)
+	BNO080Sensor(
-        : Sensor("BNO080Sensor", ImuID::BNO080, id, Address+addrSuppl, rotation, sclPin, sdaPin), m_IntPin(intPin) {};
+		uint8_t id,
-    ~BNO080Sensor(){};
+		uint8_t addrSuppl,
-    void motionSetup() override final;
+		float rotation,
-    void postSetup() override {
+		uint8_t sclPin,
-        lastData = millis();
+		uint8_t sdaPin,
-    }
+		uint8_t intPin
+	)
+		: Sensor(
+			"BNO080Sensor",
+			ImuID::BNO080,
+			id,
+			Address + addrSuppl,
+			rotation,
+			sclPin,
+			sdaPin
+		)
+		, m_IntPin(intPin){};
+	~BNO080Sensor(){};
+	void motionSetup() override final;
+	void postSetup() override { lastData = millis(); }
 
-    void motionLoop() override final;
+	void motionLoop() override final;
-    void sendData() override final;
+	void sendData() override final;
-    void startCalibration(int calibrationType) override final;
+	void startCalibration(int calibrationType) override final;
-    SensorStatus getSensorState() override final;
+	SensorStatus getSensorState() override final;
-    void setFlag(uint16_t flagId, bool state) override final;
+	void setFlag(uint16_t flagId, bool state) override final;
 
 protected:
-    // forwarding constructor
+	// forwarding constructor
-    BNO080Sensor(const char* sensorName, ImuID imuId, uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin)
+	BNO080Sensor(
-        : Sensor(sensorName, imuId, id, Address+addrSuppl, rotation, sclPin, sdaPin), m_IntPin(intPin) {};
+		const char* sensorName,
+		ImuID imuId,
+		uint8_t id,
+		uint8_t addrSuppl,
+		float rotation,
+		uint8_t sclPin,
+		uint8_t sdaPin,
+		uint8_t intPin
+	)
+		: Sensor(sensorName, imuId, id, Address + addrSuppl, rotation, sclPin, sdaPin)
+		, m_IntPin(intPin){};
+
 private:
-    BNO080 imu{};
+	BNO080 imu{};
 
-    uint8_t m_IntPin;
+	uint8_t m_IntPin;
 
-    uint8_t tap;
+	uint8_t tap;
-    unsigned long lastData = 0;
+	unsigned long lastData = 0;
-    uint8_t lastReset = 0;
+	uint8_t lastReset = 0;
-    BNO080Error lastError{};
+	BNO080Error lastError{};
-    SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {};
+	SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {};
 
-    // Magnetometer specific members
+	// Magnetometer specific members
-    Quat magQuaternion{};
+	Quat magQuaternion{};
-    uint8_t magCalibrationAccuracy = 0;
+	uint8_t magCalibrationAccuracy = 0;
-    float magneticAccuracyEstimate = 999;
+	float magneticAccuracyEstimate = 999;
-    bool newMagData = false;
+	bool newMagData = false;
-    bool configured = false;
+	bool configured = false;
 };
 
-class BNO085Sensor : public BNO080Sensor
+class BNO085Sensor : public BNO080Sensor {
-{
 public:
-    static constexpr auto TypeID = ImuID::BNO085;
+	static constexpr auto TypeID = ImuID::BNO085;
-    BNO085Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin)
+	BNO085Sensor(
-    : BNO080Sensor("BNO085Sensor", ImuID::BNO085, id, address, rotation, sclPin, sdaPin, intPin) {};
+		uint8_t id,
+		uint8_t address,
+		float rotation,
+		uint8_t sclPin,
+		uint8_t sdaPin,
+		uint8_t intPin
+	)
+		: BNO080Sensor(
+			"BNO085Sensor",
+			ImuID::BNO085,
+			id,
+			address,
+			rotation,
+			sclPin,
+			sdaPin,
+			intPin
+		){};
 };
 
-class BNO086Sensor : public BNO080Sensor
+class BNO086Sensor : public BNO080Sensor {
-{
 public:
-    static constexpr auto TypeID = ImuID::BNO086;
+	static constexpr auto TypeID = ImuID::BNO086;
-    BNO086Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin)
+	BNO086Sensor(
-    : BNO080Sensor("BNO086Sensor", ImuID::BNO086, id, address, rotation, sclPin, sdaPin, intPin) {};
+		uint8_t id,
+		uint8_t address,
+		float rotation,
+		uint8_t sclPin,
+		uint8_t sdaPin,
+		uint8_t intPin
+	)
+		: BNO080Sensor(
+			"BNO086Sensor",
+			ImuID::BNO086,
+			id,
+			address,
+			rotation,
+			sclPin,
+			sdaPin,
+			intPin
+		){};
 };
 
 #endif

src/sensors/icm20948sensor.h

@@ -1,85 +1,98 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #ifndef SLIMEVR_ICM20948SENSOR_H_
 #define SLIMEVR_ICM20948SENSOR_H_
 
 #include <ICM_20948.h>
-#include "sensor.h"
+
 #include "SensorFusionDMP.h"
+#include "sensor.h"
 
-class ICM20948Sensor : public Sensor
+class ICM20948Sensor : public Sensor {
-{
 public:
-    static constexpr auto TypeID = ImuID::ICM20948;
+	static constexpr auto TypeID = ImuID::ICM20948;
-    static constexpr uint8_t Address = 0x68;
+	static constexpr uint8_t Address = 0x68;
 
-    ICM20948Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t)
+	ICM20948Sensor(
-        : Sensor("ICM20948Sensor", ImuID::ICM20948, id, Address+addrSuppl, rotation, sclPin, sdaPin) {}
+		uint8_t id,
-    ~ICM20948Sensor() override = default;
+		uint8_t addrSuppl,
-    void motionSetup() override final;
+		float rotation,
-    void postSetup() override {
+		uint8_t sclPin,
-        this->lastData = millis();
+		uint8_t sdaPin,
-    }
+		uint8_t
+	)
+		: Sensor(
+			"ICM20948Sensor",
+			ImuID::ICM20948,
+			id,
+			Address + addrSuppl,
+			rotation,
+			sclPin,
+			sdaPin
+		) {}
+	~ICM20948Sensor() override = default;
+	void motionSetup() override final;
+	void postSetup() override { this->lastData = millis(); }
 
-    void motionLoop() override final;
+	void motionLoop() override final;
-    void sendData() override final;
+	void sendData() override final;
-    void startCalibration(int calibrationType) override final;
+	void startCalibration(int calibrationType) override final;
 
 private:
-    void calculateAccelerationWithoutGravity(Quat *quaternion);
+	void calculateAccelerationWithoutGravity(Quat* quaternion);
-    unsigned long lastData = 0;
+	unsigned long lastData = 0;
-    int bias_save_counter = 0;
+	int bias_save_counter = 0;
-    bool hasdata = false;
+	bool hasdata = false;
-// Performance test
+	// Performance test
-/*
+	/*
-    uint8_t cntbuf = 0;
+		uint8_t cntbuf = 0;
-    int32_t cntrounds = 0;
+		int32_t cntrounds = 0;
-    unsigned long lastData2 = 0;
+		unsigned long lastData2 = 0;
-*/
+	*/
 
-    #define DMPNUMBERTODOUBLECONVERTER 1073741824.0;
+#define DMPNUMBERTODOUBLECONVERTER 1073741824.0;
 
-    ICM_20948_I2C imu;
+	ICM_20948_I2C imu;
-    ICM_20948_Device_t pdev;
+	ICM_20948_Device_t pdev;
-    icm_20948_DMP_data_t dmpData{};
+	icm_20948_DMP_data_t dmpData{};
-    icm_20948_DMP_data_t dmpDataTemp{};
+	icm_20948_DMP_data_t dmpDataTemp{};
 
-    SlimeVR::Configuration::ICM20948SensorConfig m_Config = {};
+	SlimeVR::Configuration::ICM20948SensorConfig m_Config = {};
 
-    SlimeVR::Sensors::SensorFusionDMP sfusion;
+	SlimeVR::Sensors::SensorFusionDMP sfusion;
 
-    void saveCalibration(bool repeat);
+	void saveCalibration(bool repeat);
-    void loadCalibration();
+	void loadCalibration();
-    void startCalibrationAutoSave();
+	void startCalibrationAutoSave();
-    void startDMP();
+	void startDMP();
-    void connectSensor();
+	void connectSensor();
-    void startMotionLoop();
+	void startMotionLoop();
-    void checkSensorTimeout();
+	void checkSensorTimeout();
-    void readRotation();
+	void readRotation();
-    void readFIFOToEnd();
+	void readFIFOToEnd();
 
 #define OVERRIDEDMPSETUP true
-    // TapDetector tapDetector;
+	// TapDetector tapDetector;
 };
 
-#endif // SLIMEVR_ICM20948SENSOR_H_
+#endif  // SLIMEVR_ICM20948SENSOR_H_

src/sensors/mpu6050sensor.cpp

@@ -1,24 +1,24 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #include "globals.h"
@@ -29,174 +29,207 @@
 #include "MPU6050_6Axis_MotionApps20.h"
 #endif
 
-#include "mpu6050sensor.h"
 #include <i2cscan.h>
-#include "calibration.h"
+
 #include "GlobalVars.h"
+#include "calibration.h"
+#include "mpu6050sensor.h"
 
 #define ACCEL_SENSITIVITY_2G 16384.0f
 
 // Accel scale conversion steps: LSB/G -> G -> m/s^2
-constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY;
+constexpr float ASCALE_2G
+	= ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY;
 
-void MPU6050Sensor::motionSetup()
+void MPU6050Sensor::motionSetup() {
-{
+	imu.initialize(addr);
-    imu.initialize(addr);
+	if (!imu.testConnection()) {
-    if (!imu.testConnection())
+		m_Logger.fatal(
-    {
+			"Can't connect to %s (reported device ID 0x%02x) at address 0x%02x",
-        m_Logger.fatal("Can't connect to %s (reported device ID 0x%02x) at address 0x%02x", getIMUNameByType(sensorType), imu.getDeviceID(), addr);
+			getIMUNameByType(sensorType),
-        return;
+			imu.getDeviceID(),
-    }
+			addr
+		);
+		return;
+	}
 
-    m_Logger.info("Connected to %s (reported device ID 0x%02x) at address 0x%02x", getIMUNameByType(sensorType), imu.getDeviceID(), addr);
+	m_Logger.info(
+		"Connected to %s (reported device ID 0x%02x) at address 0x%02x",
+		getIMUNameByType(sensorType),
+		imu.getDeviceID(),
+		addr
+	);
 
 #ifndef IMU_MPU6050_RUNTIME_CALIBRATION
-    // Initialize the configuration
+	// Initialize the configuration
-    {
+	{
-        SlimeVR::Configuration::SensorConfig sensorCalibration = configuration.getCalibration(sensorId);
+		SlimeVR::Configuration::SensorConfig sensorCalibration
-        // If no compatible calibration data is found, the calibration data will just be zero-ed out
+			= configuration.getCalibration(sensorId);
-        switch (sensorCalibration.type) {
+		// If no compatible calibration data is found, the calibration data will just be
-        case SlimeVR::Configuration::SensorConfigType::MPU6050:
+		// zero-ed out
-            m_Config = sensorCalibration.data.mpu6050;
+		switch (sensorCalibration.type) {
-            break;
+			case SlimeVR::Configuration::SensorConfigType::MPU6050:
+				m_Config = sensorCalibration.data.mpu6050;
+				break;
 
-        case SlimeVR::Configuration::SensorConfigType::NONE:
+			case SlimeVR::Configuration::SensorConfigType::NONE:
-            m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId);
+				m_Logger.warn(
-            m_Logger.info("Calibration is advised");
+					"No calibration data found for sensor %d, ignoring...",
-            break;
+					sensorId
+				);
+				m_Logger.info("Calibration is advised");
+				break;
 
-        default:
+			default:
-            m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId);
+				m_Logger.warn(
-            m_Logger.info("Calibration is advised");
+					"Incompatible calibration data found for sensor %d, ignoring...",
-        }
+					sensorId
-    }
+				);
+				m_Logger.info("Calibration is advised");
+		}
+	}
 #endif
 
-    devStatus = imu.dmpInitialize();
+	devStatus = imu.dmpInitialize();
 
-    if (devStatus == 0)
+	if (devStatus == 0) {
-    {
 #ifdef IMU_MPU6050_RUNTIME_CALIBRATION
-        // We don't have to manually calibrate if we are using the dmp's automatic calibration
+		// We don't have to manually calibrate if we are using the dmp's automatic
+		// calibration
 #else  // IMU_MPU6050_RUNTIME_CALIBRATION
 
-        m_Logger.debug("Performing startup calibration of accel and gyro...");
+		m_Logger.debug("Performing startup calibration of accel and gyro...");
-        // Do a quick and dirty calibration. As the imu warms up the offsets will change a bit, but this will be good-enough
+		// Do a quick and dirty calibration. As the imu warms up the offsets will change
-        delay(1000); // A small sleep to give the users a chance to stop it from moving
+		// a bit, but this will be good-enough
+		delay(1000);  // A small sleep to give the users a chance to stop it from moving
 
-        imu.CalibrateGyro(6);
+		imu.CalibrateGyro(6);
-        imu.CalibrateAccel(6);
+		imu.CalibrateAccel(6);
-        imu.PrintActiveOffsets();
+		imu.PrintActiveOffsets();
-#endif // IMU_MPU6050_RUNTIME_CALIBRATION
+#endif  // IMU_MPU6050_RUNTIME_CALIBRATION
 
-        ledManager.pattern(50, 50, 5);
+		ledManager.pattern(50, 50, 5);
 
-        // turn on the DMP, now that it's ready
+		// turn on the DMP, now that it's ready
-        m_Logger.debug("Enabling DMP...");
+		m_Logger.debug("Enabling DMP...");
-        imu.setDMPEnabled(true);
+		imu.setDMPEnabled(true);
 
-        // TODO: Add interrupt support
+		// TODO: Add interrupt support
-        // mpuIntStatus = imu.getIntStatus();
+		// mpuIntStatus = imu.getIntStatus();
 
-        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+		// set our DMP Ready flag so the main loop() function knows it's okay to use it
-        m_Logger.debug("DMP ready! Waiting for first interrupt...");
+		m_Logger.debug("DMP ready! Waiting for first interrupt...");
-        dmpReady = true;
+		dmpReady = true;
 
-        // get expected DMP packet size for later comparison
+		// get expected DMP packet size for later comparison
-        packetSize = imu.dmpGetFIFOPacketSize();
+		packetSize = imu.dmpGetFIFOPacketSize();
 
-        working = true;
+		working = true;
-    }
+	} else {
-    else
+		// ERROR!
-    {
+		// 1 = initial memory load failed
-        // ERROR!
+		// 2 = DMP configuration updates failed
-        // 1 = initial memory load failed
+		// (if it's going to break, usually the code will be 1)
-        // 2 = DMP configuration updates failed
+		m_Logger.error("DMP Initialization failed (code %d)", devStatus);
-        // (if it's going to break, usually the code will be 1)
+	}
-        m_Logger.error("DMP Initialization failed (code %d)", devStatus);
-    }
 }
 
-void MPU6050Sensor::motionLoop()
+void MPU6050Sensor::motionLoop() {
-{
 #if ENABLE_INSPECTION
-    {
+	{
-        int16_t rX, rY, rZ, aX, aY, aZ;
+		int16_t rX, rY, rZ, aX, aY, aZ;
-        imu.getRotation(&rX, &rY, &rZ);
+		imu.getRotation(&rX, &rY, &rZ);
-        imu.getAcceleration(&aX, &aY, &aZ);
+		imu.getAcceleration(&aX, &aY, &aZ);
 
-        networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255);
+		networkConnection.sendInspectionRawIMUData(
-    }
+			sensorId,
+			rX,
+			rY,
+			rZ,
+			255,
+			aX,
+			aY,
+			aZ,
+			255,
+			0,
+			0,
+			0,
+			255
+		);
+	}
 #endif
 
-    if (!dmpReady)
+	if (!dmpReady) {
-        return;
+		return;
+	}
 
-    if (imu.dmpGetCurrentFIFOPacket(fifoBuffer))
+	if (imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
-    {
+		imu.dmpGetQuaternion(&rawQuat, fifoBuffer);
-        imu.dmpGetQuaternion(&rawQuat, fifoBuffer);
+		hadData = true;
-        hadData = true;
 
-        sfusion.updateQuaternion(rawQuat);
+		sfusion.updateQuaternion(rawQuat);
 
-        setFusedRotation(sfusion.getQuaternionQuat());
+		setFusedRotation(sfusion.getQuaternionQuat());
 
-        #if SEND_ACCELERATION
+#if SEND_ACCELERATION
-        {
+		{
-            this->imu.dmpGetAccel(&this->rawAccel, this->fifoBuffer);
+			this->imu.dmpGetAccel(&this->rawAccel, this->fifoBuffer);
 
-            float Axyz[3] = {(float)rawAccel.x * ASCALE_2G,
+			float Axyz[3]
-                             (float)rawAccel.y * ASCALE_2G,
+				= {(float)rawAccel.x * ASCALE_2G,
-                             (float)rawAccel.z * ASCALE_2G };
+				   (float)rawAccel.y * ASCALE_2G,
+				   (float)rawAccel.z * ASCALE_2G};
 
-            sfusion.updateAcc(Axyz);
+			sfusion.updateAcc(Axyz);
 
 			setAcceleration(sfusion.getLinearAccVec());
-        }
+		}
-        #endif
+#endif
-    }
+	}
 }
 
 void MPU6050Sensor::startCalibration(int calibrationType) {
-    ledManager.on();
+	ledManager.on();
 
 #ifdef IMU_MPU6050_RUNTIME_CALIBRATION
-    m_Logger.info("MPU is using automatic runtime calibration. Place down the device and it should automatically calibrate after a few seconds");
+	m_Logger.info(
-#else //!IMU_MPU6050_RUNTIME_CALIBRATION
+		"MPU is using automatic runtime calibration. Place down the device and it "
-    m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
+		"should automatically calibrate after a few seconds"
-    delay(2000);
+	);
+#else  //! IMU_MPU6050_RUNTIME_CALIBRATION
+	m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
+	delay(2000);
 
-    imu.setDMPEnabled(false);
+	imu.setDMPEnabled(false);
-    imu.CalibrateGyro(6);
+	imu.CalibrateGyro(6);
-    imu.CalibrateAccel(6);
+	imu.CalibrateAccel(6);
-    imu.setDMPEnabled(true);
+	imu.setDMPEnabled(true);
 
-    m_Logger.debug("Gathered baseline gyro reading");
+	m_Logger.debug("Gathered baseline gyro reading");
-    m_Logger.debug("Starting offset finder");
+	m_Logger.debug("Starting offset finder");
-    switch (calibrationType)
+	switch (calibrationType) {
-    {
+		case CALIBRATION_TYPE_INTERNAL_ACCEL:
-    case CALIBRATION_TYPE_INTERNAL_ACCEL:
+			imu.CalibrateAccel(10);
-        imu.CalibrateAccel(10);
+			m_Config.A_B[0] = imu.getXAccelOffset();
-        m_Config.A_B[0] = imu.getXAccelOffset();
+			m_Config.A_B[1] = imu.getYAccelOffset();
-        m_Config.A_B[1] = imu.getYAccelOffset();
+			m_Config.A_B[2] = imu.getZAccelOffset();
-        m_Config.A_B[2] = imu.getZAccelOffset();
+			break;
-        break;
+		case CALIBRATION_TYPE_INTERNAL_GYRO:
-    case CALIBRATION_TYPE_INTERNAL_GYRO:
+			imu.CalibrateGyro(10);
-        imu.CalibrateGyro(10);
+			m_Config.G_off[0] = imu.getXGyroOffset();
-        m_Config.G_off[0] = imu.getXGyroOffset();
+			m_Config.G_off[1] = imu.getYGyroOffset();
-        m_Config.G_off[1] = imu.getYGyroOffset();
+			m_Config.G_off[2] = imu.getZGyroOffset();
-        m_Config.G_off[2] = imu.getZGyroOffset();
+			break;
-        break;
+	}
-    }
 
-    SlimeVR::Configuration::SensorConfig calibration;
+	SlimeVR::Configuration::SensorConfig calibration;
-    calibration.type = SlimeVR::Configuration::SensorConfigType::MPU6050;
+	calibration.type = SlimeVR::Configuration::SensorConfigType::MPU6050;
-    calibration.data.mpu6050 = m_Config;
+	calibration.data.mpu6050 = m_Config;
-    configuration.setCalibration(sensorId, calibration);
+	configuration.setCalibration(sensorId, calibration);
-    configuration.save();
+	configuration.save();
 
-    m_Logger.info("Calibration finished");
+	m_Logger.info("Calibration finished");
-#endif // !IMU_MPU6050_RUNTIME_CALIBRATION
+#endif  // !IMU_MPU6050_RUNTIME_CALIBRATION
 
-    ledManager.off();
+	ledManager.off();
 }

src/sensors/mpu6050sensor.h

@@ -1,62 +1,78 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SENSORS_MPU6050SENSOR_H
 #define SENSORS_MPU6050SENSOR_H
 
-#include "sensor.h"
 #include <MPU6050.h>
+
 #include "SensorFusionDMP.h"
+#include "sensor.h"
 
-class MPU6050Sensor : public Sensor
+class MPU6050Sensor : public Sensor {
-{
 public:
-    static constexpr auto TypeID = ImuID::MPU6050;
+	static constexpr auto TypeID = ImuID::MPU6050;
-    static constexpr uint8_t Address = 0x68;
+	static constexpr uint8_t Address = 0x68;
 
-    MPU6050Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t)
+	MPU6050Sensor(
-        : Sensor("MPU6050Sensor", ImuID::MPU6050, id, Address+addrSuppl, rotation, sclPin, sdaPin){};
+		uint8_t id,
-    ~MPU6050Sensor(){};
+		uint8_t addrSuppl,
-    void motionSetup() override final;
+		float rotation,
-    void motionLoop() override final;
+		uint8_t sclPin,
-    void startCalibration(int calibrationType) override final;
+		uint8_t sdaPin,
+		uint8_t
+	)
+		: Sensor(
+			"MPU6050Sensor",
+			ImuID::MPU6050,
+			id,
+			Address + addrSuppl,
+			rotation,
+			sclPin,
+			sdaPin
+		){};
+	~MPU6050Sensor(){};
+	void motionSetup() override final;
+	void motionLoop() override final;
+	void startCalibration(int calibrationType) override final;
 
 private:
-    MPU6050 imu{};
+	MPU6050 imu{};
-    Quaternion rawQuat{};
+	Quaternion rawQuat{};
-    VectorInt16 rawAccel{};
+	VectorInt16 rawAccel{};
-    // MPU dmp control/status vars
+	// MPU dmp control/status vars
-    bool dmpReady = false;    // set true if DMP init was successful
+	bool dmpReady = false;  // set true if DMP init was successful
-    uint8_t mpuIntStatus;     // holds actual interrupt status byte from MPU
+	uint8_t mpuIntStatus;  // holds actual interrupt status byte from MPU
-    uint8_t devStatus;        // return status after each device operation (0 = success, !0 = error)
+	uint8_t devStatus;  // return status after each device operation (0 = success, !0 =
-    uint16_t packetSize;      // expected DMP packet size (default is 42 bytes)
+						// error)
-    uint16_t fifoCount;       // count of all bytes currently in FIFO
+	uint16_t packetSize;  // expected DMP packet size (default is 42 bytes)
-    uint8_t fifoBuffer[64]{}; // FIFO storage buffer
+	uint16_t fifoCount;  // count of all bytes currently in FIFO
+	uint8_t fifoBuffer[64]{};  // FIFO storage buffer
 
-    SlimeVR::Sensors::SensorFusionDMP sfusion;
+	SlimeVR::Sensors::SensorFusionDMP sfusion;
 
 #ifndef IMU_MPU6050_RUNTIME_CALIBRATION
-    SlimeVR::Configuration::MPU6050SensorConfig m_Config = {};
+	SlimeVR::Configuration::MPU6050SensorConfig m_Config = {};
 #endif
 };
 

src/sensors/mpu9250sensor.cpp

@@ -1,421 +1,491 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain, S.J. Remington & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain, S.J. Remington & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #include "mpu9250sensor.h"
+
+#include <i2cscan.h>
+
+#include "GlobalVars.h"
+#include "calibration.h"
 #include "globals.h"
 #include "helper_3dmath.h"
-#include <i2cscan.h>
-#include "calibration.h"
 #include "magneto1.4.h"
-#include "GlobalVars.h"
 #if MPU_USE_DMPMAG
 #include "dmpmag.h"
 #endif
 
-//#if defined(_MAHONY_H_) || defined(_MADGWICK_H_)
+// #if defined(_MAHONY_H_) || defined(_MADGWICK_H_)
-constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB per d/s -> rad/s
+constexpr float gscale
-//#endif
+	= (250. / 32768.0) * (PI / 180.0);  // gyro default 250 LSB per d/s -> rad/s
+// #endif
 
 #define ACCEL_SENSITIVITY_2G 16384.0f
 
 // Accel scale conversion steps: LSB/G -> G -> m/s^2
-constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY;
+constexpr float ASCALE_2G
+	= ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY;
 
 void MPU9250Sensor::motionSetup() {
-    // initialize device
+	// initialize device
-    imu.initialize(addr);
+	imu.initialize(addr);
-    if(!imu.testConnection()) {
+	if (!imu.testConnection()) {
-        m_Logger.fatal("Can't connect to MPU9250 (reported device ID 0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
+		m_Logger.fatal(
-        return;
+			"Can't connect to MPU9250 (reported device ID 0x%02x) at address 0x%02x",
-    }
+			imu.getDeviceID(),
+			addr
+		);
+		return;
+	}
 
-    m_Logger.info("Connected to MPU9250 (reported device ID 0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
+	m_Logger.info(
+		"Connected to MPU9250 (reported device ID 0x%02x) at address 0x%02x",
+		imu.getDeviceID(),
+		addr
+	);
 
-    int16_t ax,ay,az;
+	int16_t ax, ay, az;
 
-    // turn on while flip back to calibrate. then, flip again after 5 seconds.
+	// turn on while flip back to calibrate. then, flip again after 5 seconds.
-    // TODO: Move calibration invoke after calibrate button on slimeVR server available
+	// TODO: Move calibration invoke after calibrate button on slimeVR server available
-    imu.getAcceleration(&ax, &ay, &az);
+	imu.getAcceleration(&ax, &ay, &az);
-    float g_az = (float)az / 16384; // For 2G sensitivity
+	float g_az = (float)az / 16384;  // For 2G sensitivity
-    if(g_az < -0.75f) {
+	if (g_az < -0.75f) {
-        ledManager.on();
+		ledManager.on();
-        m_Logger.info("Flip front to confirm start calibration");
+		m_Logger.info("Flip front to confirm start calibration");
-        delay(5000);
+		delay(5000);
-        ledManager.off();
+		ledManager.off();
 
-        imu.getAcceleration(&ax, &ay, &az);
+		imu.getAcceleration(&ax, &ay, &az);
-        g_az = (float)az / 16384;
+		g_az = (float)az / 16384;
-        if(g_az > 0.75f) {
+		if (g_az > 0.75f) {
-            m_Logger.debug("Starting calibration...");
+			m_Logger.debug("Starting calibration...");
-            startCalibration(0);
+			startCalibration(0);
-        }
+		}
-    }
+	}
 
-    // Initialize the configuration
+	// Initialize the configuration
-    {
+	{
-        SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId);
+		SlimeVR::Configuration::SensorConfig sensorConfig
-        // If no compatible calibration data is found, the calibration data will just be zero-ed out
+			= configuration.getSensor(sensorId);
-        switch (sensorConfig.type) {
+		// If no compatible calibration data is found, the calibration data will just be
-        case SlimeVR::Configuration::SensorConfigType::MPU9250:
+		// zero-ed out
-            m_Config = sensorConfig.data.mpu9250;
+		switch (sensorConfig.type) {
-            break;
+			case SlimeVR::Configuration::SensorConfigType::MPU9250:
+				m_Config = sensorConfig.data.mpu9250;
+				break;
 
-        case SlimeVR::Configuration::SensorConfigType::NONE:
+			case SlimeVR::Configuration::SensorConfigType::NONE:
-            m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId);
+				m_Logger.warn(
-            m_Logger.info("Calibration is advised");
+					"No calibration data found for sensor %d, ignoring...",
-            break;
+					sensorId
+				);
+				m_Logger.info("Calibration is advised");
+				break;
 
-        default:
+			default:
-            m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId);
+				m_Logger.warn(
-            m_Logger.info("Calibration is advised");
+					"Incompatible calibration data found for sensor %d, ignoring...",
-        }
+					sensorId
-    }
+				);
+				m_Logger.info("Calibration is advised");
+		}
+	}
 
 #if MPU_USE_DMPMAG
-    uint8_t devStatus = imu.dmpInitialize();
+	uint8_t devStatus = imu.dmpInitialize();
-    if(devStatus == 0){
+	if (devStatus == 0) {
-        ledManager.pattern(50, 50, 5);
+		ledManager.pattern(50, 50, 5);
 
-        // turn on the DMP, now that it's ready
+		// turn on the DMP, now that it's ready
-        m_Logger.debug("Enabling DMP...");
+		m_Logger.debug("Enabling DMP...");
-        imu.setDMPEnabled(true);
+		imu.setDMPEnabled(true);
 
-        // TODO: Add interrupt support
+		// TODO: Add interrupt support
-        // mpuIntStatus = imu.getIntStatus();
+		// mpuIntStatus = imu.getIntStatus();
 
-        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+		// set our DMP Ready flag so the main loop() function knows it's okay to use it
-        m_Logger.debug("DMP ready! Waiting for first interrupt...");
+		m_Logger.debug("DMP ready! Waiting for first interrupt...");
-        dmpReady = true;
+		dmpReady = true;
 
-        // get expected DMP packet size for later comparison
+		// get expected DMP packet size for later comparison
-        packetSize = imu.dmpGetFIFOPacketSize();
+		packetSize = imu.dmpGetFIFOPacketSize();
-        working = true;
+		working = true;
-    } else {
+	} else {
-        // ERROR!
+		// ERROR!
-        // 1 = initial memory load failed
+		// 1 = initial memory load failed
-        // 2 = DMP configuration updates failed
+		// 2 = DMP configuration updates failed
-        // (if it's going to break, usually the code will be 1)
+		// (if it's going to break, usually the code will be 1)
-        m_Logger.error("DMP Initialization failed (code %d)", devStatus);
+		m_Logger.error("DMP Initialization failed (code %d)", devStatus);
-    }
+	}
 #else
-    // NOTE: could probably combine these into less total writes, but this should work, and isn't time critical.
+	// NOTE: could probably combine these into less total writes, but this should work,
-    imu.setAccelFIFOEnabled(true);
+	// and isn't time critical.
-    imu.setXGyroFIFOEnabled(true);
+	imu.setAccelFIFOEnabled(true);
-    imu.setYGyroFIFOEnabled(true);
+	imu.setXGyroFIFOEnabled(true);
-    imu.setZGyroFIFOEnabled(true);
+	imu.setYGyroFIFOEnabled(true);
-    imu.setSlave0FIFOEnabled(true);
+	imu.setZGyroFIFOEnabled(true);
+	imu.setSlave0FIFOEnabled(true);
 
-    // Set a rate we prefer
+	// Set a rate we prefer
-    imu.setRate(MPU9250_SAMPLE_DIV); // 8khz / (1 + MPU9250_SAMPLE_DIV)
+	imu.setRate(MPU9250_SAMPLE_DIV);  // 8khz / (1 + MPU9250_SAMPLE_DIV)
 
-    imu.resetFIFO();
+	imu.resetFIFO();
-    imu.setFIFOEnabled(true);
+	imu.setFIFOEnabled(true);
 
-    working = true;
+	working = true;
 #endif
 }
 
 void MPU9250Sensor::motionLoop() {
 #if ENABLE_INSPECTION
-    {
+	{
-        int16_t rX, rY, rZ, aX, aY, aZ, mX, mY, mZ;
+		int16_t rX, rY, rZ, aX, aY, aZ, mX, mY, mZ;
-        imu.getRotation(&rX, &rY, &rZ);
+		imu.getRotation(&rX, &rY, &rZ);
-        imu.getAcceleration(&aX, &aY, &aZ);
+		imu.getAcceleration(&aX, &aY, &aZ);
-        imu.getMagnetometer(&mX, &mY, &mZ);
+		imu.getMagnetometer(&mX, &mY, &mZ);
 
-        networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255);
+		networkConnection.sendInspectionRawIMUData(
-    }
+			sensorId,
+			rX,
+			rY,
+			rZ,
+			255,
+			aX,
+			aY,
+			aZ,
+			255,
+			mX,
+			mY,
+			mZ,
+			255
+		);
+	}
 #endif
 
 #if MPU_USE_DMPMAG
-    // Update quaternion
+	// Update quaternion
-    if(!dmpReady)
+	if (!dmpReady) {
-        return;
+		return;
-    Quaternion rawQuat{};
+	}
-    uint8_t dmpPacket[packetSize];
+	Quaternion rawQuat{};
-    if(!imu.GetCurrentFIFOPacket(dmpPacket, packetSize)) return;
+	uint8_t dmpPacket[packetSize];
-    if(imu.dmpGetQuaternion(&rawQuat, dmpPacket)) return; // FIFO CORRUPTED
+	if (!imu.GetCurrentFIFOPacket(dmpPacket, packetSize)) {
-    hadData = true;
+		return;
+	}
+	if (imu.dmpGetQuaternion(&rawQuat, dmpPacket)) {
+		return;  // FIFO CORRUPTED
+	}
+	hadData = true;
 
-    sfusion.updateQuaternion(rawQuat);
+	sfusion.updateQuaternion(rawQuat);
 
-    int16_t temp[3];
+	int16_t temp[3];
-    imu.getMagnetometer(&temp[0], &temp[1], &temp[2]);
+	imu.getMagnetometer(&temp[0], &temp[1], &temp[2]);
-    parseMagData(temp);
+	parseMagData(temp);
 
-    sfusion.updateMag(Mxyz);
+	sfusion.updateMag(Mxyz);
-    #if SEND_ACCELERATION
+#if SEND_ACCELERATION
-    {
+	{
-        int16_t atemp[3];
+		int16_t atemp[3];
-        this->imu.dmpGetAccel(atemp, dmpPacket);
+		this->imu.dmpGetAccel(atemp, dmpPacket);
-        parseAccelData(atemp);
+		parseAccelData(atemp);
 
-        sfusion.updateAcc(Axyz);
+		sfusion.updateAcc(Axyz);
-    }
+	}
-    #endif
-#else
-    union fifo_sample_raw buf;
-    uint16_t remaining_samples;
-    // TODO: would it be faster to read multiple samples at once
-    while (getNextSample (&buf, &remaining_samples)) {
-        parseAccelData(buf.sample.accel);
-        parseGyroData(buf.sample.gyro);
-        parseMagData(buf.sample.mag);
-
-        // TODO: monitor magnetometer status
-        // buf.sample.mag_status;
-        // TODO: monitor interrupts
-        // imu.getIntStatus();
-        // TODO: monitor remaining_samples to ensure that the number is going down, not up.
-        // remaining_samples
-
-        sfusion.update9D(Axyz, Gxyz, Mxyz);
-    }
 #endif
-    setFusedRotation(sfusion.getQuaternionQuat());
+#else
-    #if SEND_ACCELERATION
+	union fifo_sample_raw buf;
+	uint16_t remaining_samples;
+	// TODO: would it be faster to read multiple samples at once
+	while (getNextSample(&buf, &remaining_samples)) {
+		parseAccelData(buf.sample.accel);
+		parseGyroData(buf.sample.gyro);
+		parseMagData(buf.sample.mag);
+
+		// TODO: monitor magnetometer status
+		// buf.sample.mag_status;
+		// TODO: monitor interrupts
+		// imu.getIntStatus();
+		// TODO: monitor remaining_samples to ensure that the number is going down, not
+		// up. remaining_samples
+
+		sfusion.update9D(Axyz, Gxyz, Mxyz);
+	}
+#endif
+	setFusedRotation(sfusion.getQuaternionQuat());
+#if SEND_ACCELERATION
 	setAcceleration(sfusion.getLinearAccVec());
-    #endif
+#endif
 }
 
 void MPU9250Sensor::startCalibration(int calibrationType) {
-    ledManager.on();
+	ledManager.on();
 #if MPU_USE_DMPMAG
-    // with DMP, we just need mag data
+	// with DMP, we just need mag data
-    constexpr int calibrationSamples = 300;
+	constexpr int calibrationSamples = 300;
 
-    // Blink calibrating led before user should rotate the sensor
+	// Blink calibrating led before user should rotate the sensor
-    m_Logger.info("Gently rotate the device while it's gathering magnetometer data");
+	m_Logger.info("Gently rotate the device while it's gathering magnetometer data");
-    ledManager.pattern(15, 300, 3000/310);
+	ledManager.pattern(15, 300, 3000 / 310);
-    MagnetoCalibration *magneto = new MagnetoCalibration();
+	MagnetoCalibration* magneto = new MagnetoCalibration();
-    for (int i = 0; i < calibrationSamples; i++) {
+	for (int i = 0; i < calibrationSamples; i++) {
-        ledManager.on();
+		ledManager.on();
-        int16_t mx,my,mz;
+		int16_t mx, my, mz;
-        imu.getMagnetometer(&mx, &my, &mz);
+		imu.getMagnetometer(&mx, &my, &mz);
-        magneto->sample(my, mx, -mz);
+		magneto->sample(my, mx, -mz);
 
-        ledManager.off();
+		ledManager.off();
-        delay(250);
+		delay(250);
-    }
+	}
-    m_Logger.debug("Calculating calibration data...");
+	m_Logger.debug("Calculating calibration data...");
 
-    float M_BAinv[4][3];
+	float M_BAinv[4][3];
-    magneto->current_calibration(M_BAinv);
+	magneto->current_calibration(M_BAinv);
-    delete magneto;
+	delete magneto;
 
-    m_Logger.debug("[INFO] Magnetometer calibration matrix:");
+	m_Logger.debug("[INFO] Magnetometer calibration matrix:");
-    m_Logger.debug("{");
+	m_Logger.debug("{");
-    for (int i = 0; i < 3; i++) {
+	for (int i = 0; i < 3; i++) {
-        m_Config.M_B[i] = M_BAinv[0][i];
+		m_Config.M_B[i] = M_BAinv[0][i];
-        m_Config.M_Ainv[0][i] = M_BAinv[1][i];
+		m_Config.M_Ainv[0][i] = M_BAinv[1][i];
-        m_Config.M_Ainv[1][i] = M_BAinv[2][i];
+		m_Config.M_Ainv[1][i] = M_BAinv[2][i];
-        m_Config.M_Ainv[2][i] = M_BAinv[3][i];
+		m_Config.M_Ainv[2][i] = M_BAinv[3][i];
-        m_Logger.debug("  %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]);
+		m_Logger.debug(
-    }
+			"  %f, %f, %f, %f",
-    m_Logger.debug("}");
+			M_BAinv[0][i],
+			M_BAinv[1][i],
+			M_BAinv[2][i],
+			M_BAinv[3][i]
+		);
+	}
+	m_Logger.debug("}");
 
 #else
 
-    m_Logger.debug("Gathering raw data for device calibration...");
+	m_Logger.debug("Gathering raw data for device calibration...");
-    constexpr int calibrationSamples = 300;
+	constexpr int calibrationSamples = 300;
-    // Reset values
+	// Reset values
-    Gxyz[0] = 0;
+	Gxyz[0] = 0;
-    Gxyz[1] = 0;
+	Gxyz[1] = 0;
-    Gxyz[2] = 0;
+	Gxyz[2] = 0;
 
-    // Wait for sensor to calm down before calibration
+	// Wait for sensor to calm down before calibration
-    m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
+	m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
-    delay(2000);
+	delay(2000);
 
-    union fifo_sample_raw buf;
+	union fifo_sample_raw buf;
 
-    imu.resetFIFO(); // fifo is sure to have filled up in the seconds of delay, don't try reading it.
+	imu.resetFIFO(
-    for (int i = 0; i < calibrationSamples; i++) {
+	);  // fifo is sure to have filled up in the seconds of delay, don't try reading it.
-        // wait for new sample
+	for (int i = 0; i < calibrationSamples; i++) {
-        while (!getNextSample(&buf, nullptr)) { ; }
+		// wait for new sample
+		while (!getNextSample(&buf, nullptr)) {
+			;
+		}
 
-        Gxyz[0] += float(buf.sample.gyro[0]);
+		Gxyz[0] += float(buf.sample.gyro[0]);
-        Gxyz[1] += float(buf.sample.gyro[1]);
+		Gxyz[1] += float(buf.sample.gyro[1]);
-        Gxyz[2] += float(buf.sample.gyro[2]);
+		Gxyz[2] += float(buf.sample.gyro[2]);
-    }
+	}
-    Gxyz[0] /= calibrationSamples;
+	Gxyz[0] /= calibrationSamples;
-    Gxyz[1] /= calibrationSamples;
+	Gxyz[1] /= calibrationSamples;
-    Gxyz[2] /= calibrationSamples;
+	Gxyz[2] /= calibrationSamples;
 
 #ifdef DEBUG_SENSOR
-    m_Logger.trace("Gyro calibration results: %f %f %f", Gxyz[0], Gxyz[1], Gxyz[2]);
+	m_Logger.trace("Gyro calibration results: %f %f %f", Gxyz[0], Gxyz[1], Gxyz[2]);
 #endif
 
-    // TODO: use offset registers?
+	// TODO: use offset registers?
-    m_Config.G_off[0] = Gxyz[0];
+	m_Config.G_off[0] = Gxyz[0];
-    m_Config.G_off[1] = Gxyz[1];
+	m_Config.G_off[1] = Gxyz[1];
-    m_Config.G_off[2] = Gxyz[2];
+	m_Config.G_off[2] = Gxyz[2];
 
-    // Blink calibrating led before user should rotate the sensor
+	// Blink calibrating led before user should rotate the sensor
-    m_Logger.info("Gently rotate the device while it's gathering accelerometer and magnetometer data");
+	m_Logger.info(
-    ledManager.pattern(15, 300, 3000/310);
+		"Gently rotate the device while it's gathering accelerometer and magnetometer "
+		"data"
+	);
+	ledManager.pattern(15, 300, 3000 / 310);
 
-    MagnetoCalibration *magneto_acc = new MagnetoCalibration();
+	MagnetoCalibration* magneto_acc = new MagnetoCalibration();
-    MagnetoCalibration *magneto_mag = new MagnetoCalibration();
+	MagnetoCalibration* magneto_mag = new MagnetoCalibration();
 
-    // NOTE: we don't use the FIFO here on *purpose*. This makes the difference between a calibration that takes a second or three and a calibration that takes much longer.
+	// NOTE: we don't use the FIFO here on *purpose*. This makes the difference between
-    for (int i = 0; i < calibrationSamples; i++) {
+	// a calibration that takes a second or three and a calibration that takes much
-        ledManager.on();
+	// longer.
-        int16_t ax,ay,az,gx,gy,gz,mx,my,mz;
+	for (int i = 0; i < calibrationSamples; i++) {
-        imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+		ledManager.on();
-        magneto_acc->sample(ax, ay, az);
+		int16_t ax, ay, az, gx, gy, gz, mx, my, mz;
-        magneto_mag->sample(my, mx, -mz);
+		imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+		magneto_acc->sample(ax, ay, az);
+		magneto_mag->sample(my, mx, -mz);
 
-        ledManager.off();
+		ledManager.off();
-        delay(250);
+		delay(250);
-    }
+	}
-    m_Logger.debug("Calculating calibration data...");
+	m_Logger.debug("Calculating calibration data...");
 
-    float A_BAinv[4][3];
+	float A_BAinv[4][3];
-    magneto_acc->current_calibration(A_BAinv);
+	magneto_acc->current_calibration(A_BAinv);
-    delete magneto_acc;
+	delete magneto_acc;
 
-    float M_BAinv[4][3];
+	float M_BAinv[4][3];
-    magneto_mag->current_calibration(M_BAinv);
+	magneto_mag->current_calibration(M_BAinv);
-    delete magneto_mag;
+	delete magneto_mag;
 
-    m_Logger.debug("Finished Calculate Calibration data");
+	m_Logger.debug("Finished Calculate Calibration data");
-    m_Logger.debug("Accelerometer calibration matrix:");
+	m_Logger.debug("Accelerometer calibration matrix:");
-    m_Logger.debug("{");
+	m_Logger.debug("{");
-    for (int i = 0; i < 3; i++)
+	for (int i = 0; i < 3; i++) {
-    {
+		m_Config.A_B[i] = A_BAinv[0][i];
-        m_Config.A_B[i] = A_BAinv[0][i];
+		m_Config.A_Ainv[0][i] = A_BAinv[1][i];
-        m_Config.A_Ainv[0][i] = A_BAinv[1][i];
+		m_Config.A_Ainv[1][i] = A_BAinv[2][i];
-        m_Config.A_Ainv[1][i] = A_BAinv[2][i];
+		m_Config.A_Ainv[2][i] = A_BAinv[3][i];
-        m_Config.A_Ainv[2][i] = A_BAinv[3][i];
+		m_Logger.debug(
-        m_Logger.debug("  %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]);
+			"  %f, %f, %f, %f",
-    }
+			A_BAinv[0][i],
-    m_Logger.debug("}");
+			A_BAinv[1][i],
-    m_Logger.debug("[INFO] Magnetometer calibration matrix:");
+			A_BAinv[2][i],
-    m_Logger.debug("{");
+			A_BAinv[3][i]
-    for (int i = 0; i < 3; i++) {
+		);
-        m_Config.M_B[i] = M_BAinv[0][i];
+	}
-        m_Config.M_Ainv[0][i] = M_BAinv[1][i];
+	m_Logger.debug("}");
-        m_Config.M_Ainv[1][i] = M_BAinv[2][i];
+	m_Logger.debug("[INFO] Magnetometer calibration matrix:");
-        m_Config.M_Ainv[2][i] = M_BAinv[3][i];
+	m_Logger.debug("{");
-        m_Logger.debug("  %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]);
+	for (int i = 0; i < 3; i++) {
-    }
+		m_Config.M_B[i] = M_BAinv[0][i];
-    m_Logger.debug("}");
+		m_Config.M_Ainv[0][i] = M_BAinv[1][i];
+		m_Config.M_Ainv[1][i] = M_BAinv[2][i];
+		m_Config.M_Ainv[2][i] = M_BAinv[3][i];
+		m_Logger.debug(
+			"  %f, %f, %f, %f",
+			M_BAinv[0][i],
+			M_BAinv[1][i],
+			M_BAinv[2][i],
+			M_BAinv[3][i]
+		);
+	}
+	m_Logger.debug("}");
 #endif
 
-    m_Logger.debug("Saving the calibration data");
+	m_Logger.debug("Saving the calibration data");
 
-    SlimeVR::Configuration::SensorConfig config;
+	SlimeVR::Configuration::SensorConfig config;
-    config.type = SlimeVR::Configuration::SensorConfigType::MPU9250;
+	config.type = SlimeVR::Configuration::SensorConfigType::MPU9250;
-    config.data.mpu9250 = m_Config;
+	config.data.mpu9250 = m_Config;
-    configuration.setSensor(sensorId, config);
+	configuration.setSensor(sensorId, config);
-    configuration.save();
+	configuration.save();
 
-    ledManager.off();
+	ledManager.off();
-    m_Logger.debug("Saved the calibration data");
+	m_Logger.debug("Saved the calibration data");
 
-    m_Logger.info("Calibration data gathered");
+	m_Logger.info("Calibration data gathered");
-    // fifo will certainly have overflown due to magnetometer calibration, reset it.
+	// fifo will certainly have overflown due to magnetometer calibration, reset it.
-    imu.resetFIFO();
+	imu.resetFIFO();
 }
 
 void MPU9250Sensor::parseMagData(int16_t data[3]) {
-    // reading *little* endian int16
+	// reading *little* endian int16
-    Mxyz[0] = (float)data[1]; // my
+	Mxyz[0] = (float)data[1];  // my
-    Mxyz[1] = (float)data[0]; // mx
+	Mxyz[1] = (float)data[0];  // mx
-    Mxyz[2] = -(float)data[2]; // mz
+	Mxyz[2] = -(float)data[2];  // mz
 
-    float temp[3];
+	float temp[3];
 
-    //apply offsets and scale factors from Magneto
+	// apply offsets and scale factors from Magneto
-    for (unsigned i = 0; i < 3; i++) {
+	for (unsigned i = 0; i < 3; i++) {
-        temp[i] = (Mxyz[i] - m_Config.M_B[i]);
+		temp[i] = (Mxyz[i] - m_Config.M_B[i]);
-    }
+	}
 
-    for (unsigned i = 0; i < 3; i++) {
+	for (unsigned i = 0; i < 3; i++) {
-        #if useFullCalibrationMatrix == true
+#if useFullCalibrationMatrix == true
-            Mxyz[i] = m_Config.M_Ainv[i][0] * temp[0] + m_Config.M_Ainv[i][1] * temp[1] + m_Config.M_Ainv[i][2] * temp[2];
+		Mxyz[i] = m_Config.M_Ainv[i][0] * temp[0] + m_Config.M_Ainv[i][1] * temp[1]
-        #else
+				+ m_Config.M_Ainv[i][2] * temp[2];
-            Mxyz[i] = temp[i];
+#else
-        #endif
+		Mxyz[i] = temp[i];
-    }
+#endif
+	}
 }
 
 void MPU9250Sensor::parseAccelData(int16_t data[3]) {
-    // reading big endian int16
+	// reading big endian int16
-    Axyz[0] = (float)data[0];
+	Axyz[0] = (float)data[0];
-    Axyz[1] = (float)data[1];
+	Axyz[1] = (float)data[1];
-    Axyz[2] = (float)data[2];
+	Axyz[2] = (float)data[2];
 
-    #if !MPU_USE_DMPMAG
+#if !MPU_USE_DMPMAG
-    float temp[3];
+	float temp[3];
-    #endif
+#endif
 
-    //apply offsets (bias) and scale factors from Magneto
+	// apply offsets (bias) and scale factors from Magneto
-    for (unsigned i = 0; i < 3; i++) {
+	for (unsigned i = 0; i < 3; i++) {
-        #if !MPU_USE_DMPMAG
+#if !MPU_USE_DMPMAG
-        temp[i] = (Axyz[i] - m_Config.A_B[i]);
+		temp[i] = (Axyz[i] - m_Config.A_B[i]);
-    }
+	}
 
-    for (unsigned i = 0; i < 3; i++) {
+	for (unsigned i = 0; i < 3; i++) {
-        #if useFullCalibrationMatrix == true
+#if useFullCalibrationMatrix == true
-            Axyz[i] = m_Config.A_Ainv[i][0] * temp[0] + m_Config.A_Ainv[i][1] * temp[1] + m_Config.A_Ainv[i][2] * temp[2];
+		Axyz[i] = m_Config.A_Ainv[i][0] * temp[0] + m_Config.A_Ainv[i][1] * temp[1]
-        #else
+				+ m_Config.A_Ainv[i][2] * temp[2];
-            Axyz[i] = temp[i];
+#else
-        #endif
+		Axyz[i] = temp[i];
-        #endif
+#endif
-        Axyz[i] *= ASCALE_2G;
+#endif
-    }
+		Axyz[i] *= ASCALE_2G;
+	}
 }
 
 // TODO: refactor so that calibration/conversion to float is only done in one place.
 void MPU9250Sensor::parseGyroData(int16_t data[3]) {
-    // reading big endian int16
+	// reading big endian int16
-    Gxyz[0] = ((float)data[0] - m_Config.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
+	Gxyz[0] = ((float)data[0] - m_Config.G_off[0])
-    Gxyz[1] = ((float)data[1] - m_Config.G_off[1]) * gscale;
+			* gscale;  // 250 LSB(d/s) default to radians/s
-    Gxyz[2] = ((float)data[2] - m_Config.G_off[2]) * gscale;
+	Gxyz[1] = ((float)data[1] - m_Config.G_off[1]) * gscale;
+	Gxyz[2] = ((float)data[2] - m_Config.G_off[2]) * gscale;
 }
 
 // really just an implementation detail of getNextSample...
 void MPU9250Sensor::swapFifoData(union fifo_sample_raw* sample) {
-    #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
-        // byteswap the big endian integers
+	// byteswap the big endian integers
-        for (unsigned iii = 0; iii < 12; iii += 2) {
+	for (unsigned iii = 0; iii < 12; iii += 2) {
-            uint8_t tmp = sample->raw[iii + 0];
+		uint8_t tmp = sample->raw[iii + 0];
-            sample->raw[iii + 0] = sample->raw[iii + 1];
+		sample->raw[iii + 0] = sample->raw[iii + 1];
-            sample->raw[iii + 1] = tmp;
+		sample->raw[iii + 1] = tmp;
-        }
+	}
-    #elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
-        // byteswap the little endian integers
+	// byteswap the little endian integers
-        for (unsigned iii = 12; iii < 18; iii += 2) {
+	for (unsigned iii = 12; iii < 18; iii += 2) {
-            uint8_t tmp = sample->raw[iii + 0];
+		uint8_t tmp = sample->raw[iii + 0];
-            sample->raw[iii + 0] = sample->raw[iii + 1];
+		sample->raw[iii + 0] = sample->raw[iii + 1];
-            sample->raw[iii + 1] = tmp;
+		sample->raw[iii + 1] = tmp;
-        }
+	}
-    #else
+#else
-        #error "Endian isn't Little endian or big endian, are we not using GCC or is this a PDP?"
+#error \
-    #endif
+	"Endian isn't Little endian or big endian, are we not using GCC or is this a PDP?"
+#endif
 
-    // compiler hint for the union, should be optimized away for optimization >= -O1 according to compiler explorer
+	// compiler hint for the union, should be optimized away for optimization >= -O1
-    memmove(&sample->sample, sample->raw, sensor_data_len);
+	// according to compiler explorer
+	memmove(&sample->sample, sample->raw, sensor_data_len);
 }
 
 // thought experiments:
@@ -423,22 +493,27 @@ void MPU9250Sensor::swapFifoData(union fifo_sample_raw* sample) {
 // how does that compare to the performance of a memcpy?
 // how does that compare to the performance of data fusion?
 // if we read an extra byte from the magnetometer (or otherwise did something funky)
-// we could read into a properly aligned array of fifo_samples (and not require a memcpy?)
+// we could read into a properly aligned array of fifo_samples (and not require a
-// TODO: strict aliasing might not be violated if we just read directly into a fifo_sample*.
+// memcpy?)
+// TODO: strict aliasing might not be violated if we just read directly into a
+// fifo_sample*.
 //  which means the union approach may be overcomplicated. *shrug*
-bool MPU9250Sensor::getNextSample(union fifo_sample_raw *buffer, uint16_t *remaining_count) {
+bool MPU9250Sensor::getNextSample(
-    uint16_t count = imu.getFIFOCount();
+	union fifo_sample_raw* buffer,
-    if (count < sensor_data_len) {
+	uint16_t* remaining_count
-        // no samples to read
+) {
-        remaining_count = 0;
+	uint16_t count = imu.getFIFOCount();
-        return false;
+	if (count < sensor_data_len) {
-    }
+		// no samples to read
+		remaining_count = 0;
+		return false;
+	}
 
-    if (remaining_count) {
+	if (remaining_count) {
-        *remaining_count = (count / sensor_data_len) - 1;
+		*remaining_count = (count / sensor_data_len) - 1;
-    }
+	}
 
-    imu.getFIFOBytes(buffer->raw, sensor_data_len);
+	imu.getFIFOBytes(buffer->raw, sensor_data_len);
-    swapFifoData(buffer);
+	swapFifoData(buffer);
-    return true;
+	return true;
 }

src/sensors/mpu9250sensor.h

@@ -1,37 +1,38 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #ifndef SENSORS_MPU9250SENSOR_H
 #define SENSORS_MPU9250SENSOR_H
 
-#include "sensor.h"
-#include "logging/Logger.h"
-
 #include <MPU9250_6Axis_MotionApps_V6_12.h>
 
+#include "logging/Logger.h"
+#include "sensor.h"
+
 #define MPU9250_DEFAULT_ODR_HZ 8000.0f
 #define MPU9250_SAMPLE_DIV 39
-constexpr float MPU9250_ODR_TS = ( 1.0f / MPU9250_DEFAULT_ODR_HZ) * (1+MPU9250_SAMPLE_DIV);
+constexpr float MPU9250_ODR_TS
+	= (1.0f / MPU9250_DEFAULT_ODR_HZ) * (1 + MPU9250_SAMPLE_DIV);
 
 #define MPU_USE_DMPMAG 1
 
@@ -41,71 +42,88 @@ constexpr float MPU9250_ODR_TS = ( 1.0f / MPU9250_DEFAULT_ODR_HZ) * (1+MPU9250_S
 #include "SensorFusion.h"
 #endif
 
-class MPU9250Sensor : public Sensor
+class MPU9250Sensor : public Sensor {
-{
 public:
-    static constexpr auto TypeID = ImuID::MPU9250;
+	static constexpr auto TypeID = ImuID::MPU9250;
-    static constexpr uint8_t Address = 0x68;
+	static constexpr uint8_t Address = 0x68;
 
-    MPU9250Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t)
+	MPU9250Sensor(
-        : Sensor("MPU9250Sensor", ImuID::MPU9250, id, Address+addrSuppl, rotation, sclPin, sdaPin)
+		uint8_t id,
-    #if !MPU_USE_DMPMAG
+		uint8_t addrSuppl,
-        , sfusion(MPU9250_ODR_TS)
+		float rotation,
-    #endif
+		uint8_t sclPin,
-        {};
+		uint8_t sdaPin,
-    ~MPU9250Sensor(){};
+		uint8_t
-    void motionSetup() override final;
+	)
-    void motionLoop() override final;
+		: Sensor(
-    void startCalibration(int calibrationType) override final;
+			"MPU9250Sensor",
-    void getMPUScaled();
+			ImuID::MPU9250,
+			id,
+			Address + addrSuppl,
+			rotation,
+			sclPin,
+			sdaPin
+		)
+#if !MPU_USE_DMPMAG
+		, sfusion(MPU9250_ODR_TS)
+#endif
+			  {};
+	~MPU9250Sensor(){};
+	void motionSetup() override final;
+	void motionLoop() override final;
+	void startCalibration(int calibrationType) override final;
+	void getMPUScaled();
 
 private:
-    MPU9250 imu{};
+	MPU9250 imu{};
-    bool dmpReady = false;    // set true if DMP init was successful
+	bool dmpReady = false;  // set true if DMP init was successful
-    // TODO: actually check interrupt status
+	// TODO: actually check interrupt status
-    // uint8_t mpuIntStatus;     // holds actual interrupt status byte from MPU
+	// uint8_t mpuIntStatus;     // holds actual interrupt status byte from MPU
-    uint16_t packetSize;      // expected DMP packet size (default is 42 bytes)
+	uint16_t packetSize;  // expected DMP packet size (default is 42 bytes)
 
-    #if MPU_USE_DMPMAG
+#if MPU_USE_DMPMAG
-        SlimeVR::Sensors::SensorFusionDMP sfusion;
+	SlimeVR::Sensors::SensorFusionDMP sfusion;
-    #else
+#else
-        SlimeVR::Sensors::SensorFusion sfusion;
+	SlimeVR::Sensors::SensorFusion sfusion;
-    #endif
+#endif
 
-    // raw data and scaled as vector
+	// raw data and scaled as vector
-    float Axyz[3]{};
+	float Axyz[3]{};
-    float Gxyz[3]{};
+	float Gxyz[3]{};
-    float Mxyz[3]{};
+	float Mxyz[3]{};
-    VectorInt16 rawAccel{};
+	VectorInt16 rawAccel{};
-    Quat correction{0, 0, 0, 0};
+	Quat correction{0, 0, 0, 0};
 
-    SlimeVR::Configuration::MPU9250SensorConfig m_Config = {};
+	SlimeVR::Configuration::MPU9250SensorConfig m_Config = {};
 
-    // outputs to respective member variables
+	// outputs to respective member variables
-    void parseAccelData(int16_t data[3]);
+	void parseAccelData(int16_t data[3]);
-    void parseGyroData(int16_t data[3]);
+	void parseGyroData(int16_t data[3]);
-    void parseMagData(int16_t data[3]);
+	void parseMagData(int16_t data[3]);
 
-    // 6 bytes for gyro, 6 bytes for accel, 7 bytes for magnetometer
+	// 6 bytes for gyro, 6 bytes for accel, 7 bytes for magnetometer
-    static constexpr uint16_t sensor_data_len = 19;
+	static constexpr uint16_t sensor_data_len = 19;
 
-    struct fifo_sample {
+	struct fifo_sample {
-        int16_t accel[3];
+		int16_t accel[3];
-        int16_t gyro[3];
+		int16_t gyro[3];
-        int16_t mag[3];
+		int16_t mag[3];
-        uint8_t mag_status;
+		uint8_t mag_status;
-    };
+	};
 
-    // acts as a memory space for getNextSample. upon success, can read from the sample member
+	// acts as a memory space for getNextSample. upon success, can read from the sample
-    // TODO: this may be overcomplicated, we may be able to just use fifo_sample and i misunderstood strict aliasing rules.
+	// member
-    union fifo_sample_raw {
+	// TODO: this may be overcomplicated, we may be able to just use fifo_sample and i
-        uint8_t raw[sensor_data_len];
+	// misunderstood strict aliasing rules.
-        struct fifo_sample sample;
+	union fifo_sample_raw {
-    };
+		uint8_t raw[sensor_data_len];
+		struct fifo_sample sample;
+	};
 
-    // returns true if sample was read, outputs number of waiting samples in remaining_count if not null.
+	// returns true if sample was read, outputs number of waiting samples in
-    bool getNextSample(union fifo_sample_raw *buffer, uint16_t *remaining_count);
+	// remaining_count if not null.
-    static void swapFifoData(union fifo_sample_raw* sample);
+	bool getNextSample(union fifo_sample_raw* buffer, uint16_t* remaining_count);
+	static void swapFifoData(union fifo_sample_raw* sample);
 };
 
 #endif

src/sensors/sensor.cpp

@@ -1,117 +1,136 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 #include "sensor.h"
-#include "GlobalVars.h"
+
 #include <i2cscan.h>
+
+#include "GlobalVars.h"
 #include "calibration.h"
 
 SensorStatus Sensor::getSensorState() {
-    return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR;
+	return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR;
 }
 
 void Sensor::setAcceleration(Vector3 a) {
-    acceleration = a;
+	acceleration = a;
-    newAcceleration = true;
+	newAcceleration = true;
 }
 
 void Sensor::setFusedRotation(Quat r) {
-    fusedRotation = r * sensorOffset;
+	fusedRotation = r * sensorOffset;
-    bool changed = OPTIMIZE_UPDATES ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) : true;
+	bool changed = OPTIMIZE_UPDATES
-    if (ENABLE_INSPECTION || changed) {
+					 ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)
-        newFusedRotation = true;
+					 : true;
-        lastFusedRotationSent = fusedRotation;
+	if (ENABLE_INSPECTION || changed) {
-    }
+		newFusedRotation = true;
+		lastFusedRotationSent = fusedRotation;
+	}
 }
 
 void Sensor::sendData() {
-    if (newFusedRotation) {
+	if (newFusedRotation) {
-        newFusedRotation = false;
+		newFusedRotation = false;
-        networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy);
+		networkConnection.sendRotationData(
+			sensorId,
+			&fusedRotation,
+			DATA_TYPE_NORMAL,
+			calibrationAccuracy
+		);
 
 #ifdef DEBUG_SENSOR
-        m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation));
+		m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation));
 #endif
 
 #if SEND_ACCELERATION
-        if (newAcceleration) {
+		if (newAcceleration) {
-            newAcceleration = false;
+			newAcceleration = false;
-            networkConnection.sendSensorAcceleration(sensorId, acceleration);
+			networkConnection.sendSensorAcceleration(sensorId, acceleration);
-        }
+		}
 #endif
-    }
+	}
 }
 
 void Sensor::printTemperatureCalibrationUnsupported() {
-    m_Logger.error("Temperature calibration not supported for IMU %s", getIMUNameByType(sensorType));
+	m_Logger.error(
+		"Temperature calibration not supported for IMU %s",
+		getIMUNameByType(sensorType)
+	);
 }
-void Sensor::printTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); };
+void Sensor::printTemperatureCalibrationState() {
-void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); };
+	printTemperatureCalibrationUnsupported();
+};
+void Sensor::printDebugTemperatureCalibrationState() {
+	printTemperatureCalibrationUnsupported();
+};
 void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); };
-void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); };
+void Sensor::resetTemperatureCalibrationState() {
+	printTemperatureCalibrationUnsupported();
+};
 
 uint16_t Sensor::getSensorConfigData() {
-	SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId);
+	SlimeVR::Configuration::SensorConfig sensorConfig
+		= configuration.getSensor(sensorId);
 	return SlimeVR::Configuration::configDataToNumber(
 		&sensorConfig,
 		magStatus != MagnetometerStatus::MAG_NOT_SUPPORTED
-		);
+	);
 }
 
-const char * getIMUNameByType(ImuID imuType) {
+const char* getIMUNameByType(ImuID imuType) {
-    switch(imuType) {
+	switch (imuType) {
-        case ImuID::MPU9250:
+		case ImuID::MPU9250:
-            return "MPU9250";
+			return "MPU9250";
-        case ImuID::MPU6500:
+		case ImuID::MPU6500:
-            return "MPU6500";
+			return "MPU6500";
-        case ImuID::BNO080:
+		case ImuID::BNO080:
-            return "BNO080";
+			return "BNO080";
-        case ImuID::BNO085:
+		case ImuID::BNO085:
-            return "BNO085";
+			return "BNO085";
-        case ImuID::BNO055:
+		case ImuID::BNO055:
-            return "BNO055";
+			return "BNO055";
-        case ImuID::MPU6050:
+		case ImuID::MPU6050:
-            return "MPU6050";
+			return "MPU6050";
-        case ImuID::BNO086:
+		case ImuID::BNO086:
-            return "BNO086";
+			return "BNO086";
-        case ImuID::BMI160:
+		case ImuID::BMI160:
-            return "BMI160";
+			return "BMI160";
-        case ImuID::ICM20948:
+		case ImuID::ICM20948:
-            return "ICM20948";
+			return "ICM20948";
-        case ImuID::ICM42688:
+		case ImuID::ICM42688:
-            return "ICM42688";
+			return "ICM42688";
-        case ImuID::BMI270:
+		case ImuID::BMI270:
-            return "BMI270";
+			return "BMI270";
-        case ImuID::LSM6DS3TRC:
+		case ImuID::LSM6DS3TRC:
-            return "LSM6DS3TRC";
+			return "LSM6DS3TRC";
-        case ImuID::LSM6DSV:
+		case ImuID::LSM6DSV:
-            return "LSM6DSV";
+			return "LSM6DSV";
-        case ImuID::LSM6DSO:
+		case ImuID::LSM6DSO:
-            return "LSM6DSO";
+			return "LSM6DSO";
-        case ImuID::LSM6DSR:
+		case ImuID::LSM6DSR:
-            return "LSM6DSR";
+			return "LSM6DSR";
-        case ImuID::Unknown:
+		case ImuID::Unknown:
-        case ImuID::Empty:
+		case ImuID::Empty:
-            return "UNKNOWN";
+			return "UNKNOWN";
-    }
+	}
-    return "Unknown";
+	return "Unknown";
 }

src/sensors/sensor.h

@@ -27,6 +27,7 @@
 #include <Arduino.h>
 #include <quat.h>
 #include <vector3.h>
+
 #include "configuration/Configuration.h"
 #include "globals.h"
 #include "logging/Logger.h"
@@ -47,13 +48,24 @@ enum class MagnetometerStatus : uint8_t {
 	MAG_ENABLED = 2,
 };
 
-class Sensor
+class Sensor {
-{
 public:
-	Sensor(const char *sensorName, ImuID type, uint8_t id, uint8_t address, float rotation, uint8_t sclpin=0, uint8_t sdapin=0)
+	Sensor(
-		: addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName)),
+		const char* sensorName,
-			sclPin(sclpin), sdaPin(sdapin)
+		ImuID type,
-	{
+		uint8_t id,
+		uint8_t address,
+		float rotation,
+		uint8_t sclpin = 0,
+		uint8_t sdapin = 0
+	)
+		: addr(address)
+		, sensorId(id)
+		, sensorType(type)
+		, sensorOffset({Quat(Vector3(0, 0, 1), rotation)})
+		, m_Logger(SlimeVR::Logging::Logger(sensorName))
+		, sclPin(sclpin)
+		, sdaPin(sdapin) {
 		char buf[4];
 		sprintf(buf, "%u", id);
 		m_Logger.setTag(buf);
@@ -74,36 +86,16 @@ public:
 	virtual void saveTemperatureCalibration();
 	virtual void setFlag(uint16_t flagId, bool state){};
 	virtual uint16_t getSensorConfigData();
-	bool isWorking() {
+	bool isWorking() { return working; };
-		return working;
+	bool getHadData() const { return hadData; };
-	};
+	bool isValid() { return sclPin != sdaPin; };
-	bool getHadData() const {
+	bool isMagEnabled() { return magStatus == MagnetometerStatus::MAG_ENABLED; };
-		return hadData;
+	uint8_t getSensorId() { return sensorId; };
-	};
+	ImuID getSensorType() { return sensorType; };
-	bool isValid() {
+	MagnetometerStatus getMagStatus() { return magStatus; };
-		return sclPin != sdaPin;
+	const Vector3& getAcceleration() { return acceleration; };
-	};
+	const Quat& getFusedRotation() { return fusedRotation; };
-	bool isMagEnabled() {
+	bool hasNewDataToSend() { return newFusedRotation || newAcceleration; };
-		return magStatus == MagnetometerStatus::MAG_ENABLED;
-	};
-	uint8_t getSensorId() {
-		return sensorId;
-	};
-	ImuID getSensorType() {
-		return sensorType;
-	};
-	MagnetometerStatus getMagStatus() {
-		return magStatus;
-	};
-	const Vector3& getAcceleration() {
-		return acceleration;
-	};
-	const Quat& getFusedRotation() {
-		return fusedRotation;
-	};
-	bool hasNewDataToSend() {
-		return newFusedRotation || newAcceleration;
-	};
 
 protected:
 	uint8_t addr = 0;
@@ -132,6 +124,6 @@ private:
 	void printTemperatureCalibrationUnsupported();
 };
 
-const char * getIMUNameByType(ImuID imuType);
+const char* getIMUNameByType(ImuID imuType);
 
-#endif // SLIMEVR_SENSOR_H_
+#endif  // SLIMEVR_SENSOR_H_

src/sensors/sensoraddresses.h

@@ -1,5 +1,5 @@
-// those variables are used as "supplement" to base IMU address coming directly from IMU driver
+// those variables are used as "supplement" to base IMU address coming directly from IMU
-// they are remaining to keep backward compatibility with old style of defines.h
+// driver they are remaining to keep backward compatibility with old style of defines.h
 
 #define PRIMARY_IMU_ADDRESS_ONE 0
 #define PRIMARY_IMU_ADDRESS_TWO 1

src/sensors/softfusion/drivers/bmi270.h

@@ -1,36 +1,37 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
+	Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
 #include <algorithm>
+#include <array>
+#include <cstdint>
+#include <cstring>
 #include <limits>
+
 #include "bmi270fw.h"
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 // Driver uses acceleration range at 16g
 // and gyroscope range at 1000dps
@@ -38,382 +39,406 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers
 // Timestamps reading are not used
 
 template <typename I2CImpl>
-struct BMI270
+struct BMI270 {
-{
+	static constexpr uint8_t Address = 0x68;
-    static constexpr uint8_t Address = 0x68;
+	static constexpr auto Name = "BMI270";
-    static constexpr auto Name = "BMI270";
+	static constexpr auto Type = ImuID::BMI270;
-    static constexpr auto Type = ImuID::BMI270;
+
-
+	static constexpr float GyrTs = 1.0 / 400.0;
-    static constexpr float GyrTs=1.0/400.0;
+	static constexpr float AccTs = 1.0 / 100.0;
-    static constexpr float AccTs=1.0/100.0;
+
-
+	static constexpr float MagTs = 1.0 / 100;
-    static constexpr float MagTs=1.0/100;
+
-
+	static constexpr float GyroSensitivity = 32.768f;
-    static constexpr float GyroSensitivity = 32.768f;
+	static constexpr float AccelSensitivity = 2048.0f;
-    static constexpr float AccelSensitivity = 2048.0f;
+
-
+	struct MotionlessCalibrationData {
-    struct MotionlessCalibrationData
+		bool valid;
-    {
+		uint8_t x, y, z;
-        bool valid;
+	};
-        uint8_t x, y, z;
+
-    };
+	I2CImpl i2c;
-
+	SlimeVR::Logging::Logger& logger;
-    I2CImpl i2c;
+	int8_t zxFactor;
-    SlimeVR::Logging::Logger &logger;
+	BMI270(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-    int8_t zxFactor;
+		: i2c(i2c)
-    BMI270(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
+		, logger(logger)
-    : i2c(i2c), logger(logger), zxFactor(0) {}
+		, zxFactor(0) {}
-
+
-    struct Regs {
+	struct Regs {
-        struct WhoAmI {
+		struct WhoAmI {
-            static constexpr uint8_t reg = 0x00;
+			static constexpr uint8_t reg = 0x00;
-            static constexpr uint8_t value = 0x24;
+			static constexpr uint8_t value = 0x24;
-        };
+		};
-        static constexpr uint8_t TempData = 0x22;
+		static constexpr uint8_t TempData = 0x22;
-
+
-        struct Cmd {
+		struct Cmd {
-            static constexpr uint8_t reg = 0x7e;
+			static constexpr uint8_t reg = 0x7e;
-            static constexpr uint8_t valueSwReset = 0xb6;
+			static constexpr uint8_t valueSwReset = 0xb6;
-            static constexpr uint8_t valueFifoFlush = 0xb0;
+			static constexpr uint8_t valueFifoFlush = 0xb0;
-            static constexpr uint8_t valueGTrigger = 0x02;            
+			static constexpr uint8_t valueGTrigger = 0x02;
-        };
+		};
-
+
-        struct PwrConf {
+		struct PwrConf {
-            static constexpr uint8_t reg = 0x7c;
+			static constexpr uint8_t reg = 0x7c;
-            static constexpr uint8_t valueNoPowerSaving = 0x0;
+			static constexpr uint8_t valueNoPowerSaving = 0x0;
-            static constexpr uint8_t valueFifoSelfWakeup = 0x2;            
+			static constexpr uint8_t valueFifoSelfWakeup = 0x2;
-        };
+		};
-
+
-        struct PwrCtrl {
+		struct PwrCtrl {
-            static constexpr uint8_t reg = 0x7d;
+			static constexpr uint8_t reg = 0x7d;
-            static constexpr uint8_t valueOff = 0x0;
+			static constexpr uint8_t valueOff = 0x0;
-            static constexpr uint8_t valueGyrAccTempOn = 0b1110; // aux off
+			static constexpr uint8_t valueGyrAccTempOn = 0b1110;  // aux off
-            static constexpr uint8_t valueAccOn = 0b0100; // aux, gyr, temp off
+			static constexpr uint8_t valueAccOn = 0b0100;  // aux, gyr, temp off
-        };
+		};
-
+
-        struct InitCtrl {
+		struct InitCtrl {
-            static constexpr uint8_t reg = 0x59;
+			static constexpr uint8_t reg = 0x59;
-            static constexpr uint8_t valueStartInit = 0x00;
+			static constexpr uint8_t valueStartInit = 0x00;
-            static constexpr uint8_t valueEndInit = 0x01;
+			static constexpr uint8_t valueEndInit = 0x01;
-        };
+		};
-
+
-        static constexpr uint8_t InitAddr = 0x5b;
+		static constexpr uint8_t InitAddr = 0x5b;
-        static constexpr uint8_t InitData = 0x5e;        
+		static constexpr uint8_t InitData = 0x5e;
-
+
-        struct InternalStatus {
+		struct InternalStatus {
-            static constexpr uint8_t reg = 0x21;
+			static constexpr uint8_t reg = 0x21;
-            static constexpr uint8_t initializedBit = 0x01;
+			static constexpr uint8_t initializedBit = 0x01;
-        };
+		};
-
+
-        struct GyrConf {
+		struct GyrConf {
-            static constexpr uint8_t reg = 0x42;
+			static constexpr uint8_t reg = 0x42;
-
+
-            static constexpr uint8_t rate25Hz = 6;
+			static constexpr uint8_t rate25Hz = 6;
-            static constexpr uint8_t rate50Hz = 7;
+			static constexpr uint8_t rate50Hz = 7;
-            static constexpr uint8_t rate100Hz = 8;
+			static constexpr uint8_t rate100Hz = 8;
-            static constexpr uint8_t rate200Hz = 9;
+			static constexpr uint8_t rate200Hz = 9;
-            static constexpr uint8_t rate400Hz = 10;
+			static constexpr uint8_t rate400Hz = 10;
-            static constexpr uint8_t rate800Hz = 11;
+			static constexpr uint8_t rate800Hz = 11;
-            static constexpr uint8_t rate1600Hz = 12;
+			static constexpr uint8_t rate1600Hz = 12;
-            static constexpr uint8_t rate3200Hz = 13;
+			static constexpr uint8_t rate3200Hz = 13;
-
+
-            static constexpr uint8_t DLPFModeOsr4 = 0 << 4;
+			static constexpr uint8_t DLPFModeOsr4 = 0 << 4;
-            static constexpr uint8_t DLPFModeOsr2 = 1 << 4;
+			static constexpr uint8_t DLPFModeOsr2 = 1 << 4;
-            static constexpr uint8_t DLPFModeNorm = 2 << 4;
+			static constexpr uint8_t DLPFModeNorm = 2 << 4;
-
+
-            static constexpr uint8_t noisePerfMode = 1 << 6;
+			static constexpr uint8_t noisePerfMode = 1 << 6;
-            static constexpr uint8_t filterHighPerfMode = 1 << 7;
+			static constexpr uint8_t filterHighPerfMode = 1 << 7;
-
+
-            static constexpr uint8_t value = rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode;
+			static constexpr uint8_t value
-        };
+				= rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode;
-
+		};
-        struct GyrRange {
+
-            static constexpr uint8_t reg = 0x43;
+		struct GyrRange {
-
+			static constexpr uint8_t reg = 0x43;
-            static constexpr uint8_t range125dps = 4;
+
-            static constexpr uint8_t range250dps = 3;
+			static constexpr uint8_t range125dps = 4;
-            static constexpr uint8_t range500dps = 2;
+			static constexpr uint8_t range250dps = 3;
-            static constexpr uint8_t range1000dps = 1;
+			static constexpr uint8_t range500dps = 2;
-            static constexpr uint8_t range2000dps = 0;
+			static constexpr uint8_t range1000dps = 1;
-
+			static constexpr uint8_t range2000dps = 0;
-            static constexpr uint8_t value = range1000dps;
+
-        };
+			static constexpr uint8_t value = range1000dps;
-
+		};
-        struct AccConf {
+
-            static constexpr uint8_t reg = 0x40;
+		struct AccConf {
-
+			static constexpr uint8_t reg = 0x40;
-            static constexpr uint8_t rate0_78Hz = 1;
+
-            static constexpr uint8_t rate1_5Hz = 2;
+			static constexpr uint8_t rate0_78Hz = 1;
-            static constexpr uint8_t rate3_1Hz = 3;
+			static constexpr uint8_t rate1_5Hz = 2;
-            static constexpr uint8_t rate6_25Hz = 4;
+			static constexpr uint8_t rate3_1Hz = 3;
-            static constexpr uint8_t rate12_5Hz = 5;
+			static constexpr uint8_t rate6_25Hz = 4;
-            static constexpr uint8_t rate25Hz = 6;
+			static constexpr uint8_t rate12_5Hz = 5;
-            static constexpr uint8_t rate50Hz = 7;
+			static constexpr uint8_t rate25Hz = 6;
-            static constexpr uint8_t rate100Hz = 8;
+			static constexpr uint8_t rate50Hz = 7;
-            static constexpr uint8_t rate200Hz = 9;
+			static constexpr uint8_t rate100Hz = 8;
-            static constexpr uint8_t rate400Hz = 10;
+			static constexpr uint8_t rate200Hz = 9;
-            static constexpr uint8_t rate800Hz = 11;
+			static constexpr uint8_t rate400Hz = 10;
-            static constexpr uint8_t rate1600Hz = 12;
+			static constexpr uint8_t rate800Hz = 11;
-
+			static constexpr uint8_t rate1600Hz = 12;
-            static constexpr uint8_t DLPFModeAvg1 = 0 << 4;
+
-            static constexpr uint8_t DLPFModeAvg2 = 1 << 4;
+			static constexpr uint8_t DLPFModeAvg1 = 0 << 4;
-            static constexpr uint8_t DLPFModeAvg4 = 2 << 4;
+			static constexpr uint8_t DLPFModeAvg2 = 1 << 4;
-            static constexpr uint8_t DLPFModeAvg8 = 3 << 4;
+			static constexpr uint8_t DLPFModeAvg4 = 2 << 4;
-
+			static constexpr uint8_t DLPFModeAvg8 = 3 << 4;
-            static constexpr uint8_t filterHighPerfMode = 1 << 7;
+
-
+			static constexpr uint8_t filterHighPerfMode = 1 << 7;
-            static constexpr uint8_t value = rate100Hz | DLPFModeAvg4 | filterHighPerfMode;
+
-        };
+			static constexpr uint8_t value
-
+				= rate100Hz | DLPFModeAvg4 | filterHighPerfMode;
-        struct AccRange {
+		};
-            static constexpr uint8_t reg = 0x41;
+
-
+		struct AccRange {
-            static constexpr uint8_t range2G = 0;
+			static constexpr uint8_t reg = 0x41;
-            static constexpr uint8_t range4G = 1;
+
-            static constexpr uint8_t range8G = 2;
+			static constexpr uint8_t range2G = 0;
-            static constexpr uint8_t range16G = 3;
+			static constexpr uint8_t range4G = 1;
-
+			static constexpr uint8_t range8G = 2;
-            static constexpr uint8_t value = range16G;
+			static constexpr uint8_t range16G = 3;
-        };
+
-
+			static constexpr uint8_t value = range16G;
-        struct FifoConfig0 {
+		};
-            static constexpr uint8_t reg = 0x48;
+
-            static constexpr uint8_t value = 0x01; // fifo_stop_on_full=1, fifo_time_en=0
+		struct FifoConfig0 {
-        };
+			static constexpr uint8_t reg = 0x48;
-
+			static constexpr uint8_t value
-        struct FifoConfig1 {
+				= 0x01;  // fifo_stop_on_full=1, fifo_time_en=0
-            static constexpr uint8_t reg = 0x49;
+		};
-            static constexpr uint8_t value = (1 << 4) | (1 << 6) | (1 << 7); // header en, acc en, gyr en
+
-        };
+		struct FifoConfig1 {
-
+			static constexpr uint8_t reg = 0x49;
-        struct GyrCrtConf {
+			static constexpr uint8_t value
-            static constexpr uint8_t reg = 0x69;
+				= (1 << 4) | (1 << 6) | (1 << 7);  // header en, acc en, gyr en
-            static constexpr uint8_t valueRunning = (1 << 2); // crt_running = 1
+		};
-            static constexpr uint8_t valueStopped = 0x0; // crt_running = 0
+
-        };
+		struct GyrCrtConf {
-
+			static constexpr uint8_t reg = 0x69;
-        struct GTrig1 { // on feature page 1!
+			static constexpr uint8_t valueRunning = (1 << 2);  // crt_running = 1
-            static constexpr uint8_t reg = 0x32;
+			static constexpr uint8_t valueStopped = 0x0;  // crt_running = 0
-            static constexpr uint16_t valueTriggerCRT = (1 << 8); // select=crt
+		};
-        };
+
-
+		struct GTrig1 {  // on feature page 1!
-        struct GyrGainStatus { // on feature page 0!
+			static constexpr uint8_t reg = 0x32;
-            static constexpr uint8_t reg = 0x38;
+			static constexpr uint16_t valueTriggerCRT = (1 << 8);  // select=crt
-            static constexpr uint8_t statusOffset = 3;
+		};
-        };
+
-
+		struct GyrGainStatus {  // on feature page 0!
-        struct Offset6 { // on feature page 0!
+			static constexpr uint8_t reg = 0x38;
-            static constexpr uint8_t reg = 0x77;
+			static constexpr uint8_t statusOffset = 3;
-            static constexpr uint8_t value = (1 << 7); // gyr_gain_en = 1
+		};
-        };
+
-
+		struct Offset6 {  // on feature page 0!
-        static constexpr uint8_t FeatPage = 0x2f;
+			static constexpr uint8_t reg = 0x77;
-
+			static constexpr uint8_t value = (1 << 7);  // gyr_gain_en = 1
-        static constexpr uint8_t GyrUserGain = 0x78; // undocumented reg, got from official bmi270 driver
+		};
-
+
-        static constexpr uint8_t FifoCount = 0x24;
+		static constexpr uint8_t FeatPage = 0x2f;
-        static constexpr uint8_t FifoData = 0x26;
+
-        static constexpr uint8_t RaGyrCas = 0x3c; // on feature page 0!
+		static constexpr uint8_t GyrUserGain
-    };
+			= 0x78;  // undocumented reg, got from official bmi270 driver
-
+
-    struct Fifo {
+		static constexpr uint8_t FifoCount = 0x24;
-        static constexpr uint8_t ModeMask = 0b11000000;
+		static constexpr uint8_t FifoData = 0x26;
-        static constexpr uint8_t SkipFrame = 0b01000000;
+		static constexpr uint8_t RaGyrCas = 0x3c;  // on feature page 0!
-        static constexpr uint8_t DataFrame = 0b10000000;
+	};
-
+
-        static constexpr uint8_t GyrDataBit = 0b00001000;
+	struct Fifo {
-        static constexpr uint8_t AccelDataBit = 0b00000100;
+		static constexpr uint8_t ModeMask = 0b11000000;
-    };
+		static constexpr uint8_t SkipFrame = 0b01000000;
-
+		static constexpr uint8_t DataFrame = 0b10000000;
-    bool restartAndInit() {
+
-        // perform initialization step
+		static constexpr uint8_t GyrDataBit = 0b00001000;
-        i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset);
+		static constexpr uint8_t AccelDataBit = 0b00000100;
-        delay(12);
+	};
-        // disable power saving
+
-        i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving);
+	bool restartAndInit() {
-        delay(1);
+		// perform initialization step
-        
+		i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset);
-        // firmware upload
+		delay(12);
-        i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit);
+		// disable power saving
-        for (uint16_t pos=0; pos<sizeof(bmi270_firmware);)
+		i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving);
-        {
+		delay(1);
-            // tell the device current position
+
-
+		// firmware upload
-            // this thing is little endian, but it requires address in bizzare form
+		i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit);
-            // LSB register is only 4 bits, while MSB register is 8bits
+		for (uint16_t pos = 0; pos < sizeof(bmi270_firmware);) {
-            // also value requested is in words (16bit) not in bytes (8bit)
+			// tell the device current position
-
+
-            const uint16_t pos_words = pos >> 1; // convert current position to words
+			// this thing is little endian, but it requires address in bizzare form
-            const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00);
+			// LSB register is only 4 bits, while MSB register is 8bits
-            i2c.writeReg16(Regs::InitAddr, position);
+			// also value requested is in words (16bit) not in bytes (8bit)
-            // write actual payload chunk
+
-            const uint16_t burstWrite = std::min(sizeof(bmi270_firmware) - pos, I2CImpl::MaxTransactionLength);
+			const uint16_t pos_words = pos >> 1;  // convert current position to words
-            i2c.writeBytes(Regs::InitData, burstWrite, const_cast<uint8_t*>(bmi270_firmware + pos));
+			const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00);
-            pos += burstWrite;
+			i2c.writeReg16(Regs::InitAddr, position);
-        }
+			// write actual payload chunk
-        i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit);
+			const uint16_t burstWrite = std::min(
-        delay(140);
+				sizeof(bmi270_firmware) - pos,
-
+				I2CImpl::MaxTransactionLength
-        // leave fifo_self_wakeup enabled
+			);
-        i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup);
+			i2c.writeBytes(
-        // check if IMU initialized correctly
+				Regs::InitData,
-        if (!(i2c.readReg(Regs::InternalStatus::reg) & Regs::InternalStatus::initializedBit))
+				burstWrite,
-        {
+				const_cast<uint8_t*>(bmi270_firmware + pos)
-            // firmware upload fail or sensor not initialized
+			);
-            return false;
+			pos += burstWrite;
-        }
+		}
-
+		i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit);
-        // read zx factor used to reduce gyro cross-sensitivity error
+		delay(140);
-        const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas);
+
-        const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80;
+		// leave fifo_self_wakeup enabled
-        zxFactor = static_cast<int8_t>(zx_factor_reg | sign_byte);
+		i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup);
-        return true;
+		// check if IMU initialized correctly
-    }
+		if (!(i2c.readReg(Regs::InternalStatus::reg)
-
+			  & Regs::InternalStatus::initializedBit)) {
-    void setNormalConfig(MotionlessCalibrationData &gyroSensitivity)
+			// firmware upload fail or sensor not initialized
-    {
+			return false;
-        i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value);
+		}
-        i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value);
+
-
+		// read zx factor used to reduce gyro cross-sensitivity error
-        i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value);
+		const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas);
-        i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value);
+		const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80;
-
+		zxFactor = static_cast<int8_t>(zx_factor_reg | sign_byte);
-        if (gyroSensitivity.valid)
+		return true;
-        {
+	}
-            i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value);
+
-            i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x);
+	void setNormalConfig(MotionlessCalibrationData& gyroSensitivity) {
-        }
+		i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value);
-
+		i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value);
-        i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
+
-        delay(100); // power up delay
+		i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value);
-        i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
+		i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value);
-        i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
+
-
+		if (gyroSensitivity.valid) {
-        delay(4);
+			i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value);
-        i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush);
+			i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x);
-        delay(2);
+		}
-    }
+
-
+		i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
-    bool initialize(MotionlessCalibrationData &gyroSensitivity)
+		delay(100);  // power up delay
-    {
+		i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
-        if (!restartAndInit()) {
+		i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
-            return false;
+
-        }
+		delay(4);
-
+		i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush);
-        setNormalConfig(gyroSensitivity);
+		delay(2);
-
+	}
-        return true;
+
-    }
+	bool initialize(MotionlessCalibrationData& gyroSensitivity) {
-
+		if (!restartAndInit()) {
-    void motionlessCalibration(MotionlessCalibrationData &gyroSensitivity)
+			return false;
-    {
+		}
-        // perfrom gyroscope motionless sensitivity calibration (CRT)
+
-        // need to start from clean state according to spec
+		setNormalConfig(gyroSensitivity);
-        restartAndInit();
+
-        // only Accel ON
+		return true;
-        i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn);
+	}
-        delay(100);
+
-        i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning);
+	void motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) {
-        i2c.writeReg(Regs::FeatPage, 1);
+		// perfrom gyroscope motionless sensitivity calibration (CRT)
-        i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT);
+		// need to start from clean state according to spec
-        i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger);
+		restartAndInit();
-        delay(200);
+		// only Accel ON
-
+		i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn);
-        while(i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) {
+		delay(100);
-            logger.info("CRT running. Do not move tracker!");
+		i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning);
-            delay(200);
+		i2c.writeReg(Regs::FeatPage, 1);
-        }
+		i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT);
-
+		i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger);
-        i2c.writeReg(Regs::FeatPage, 0);
+		delay(200);
-
+
-        uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg) >> Regs::GyrGainStatus::statusOffset;
+		while (i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) {
-        // turn gyroscope back on
+			logger.info("CRT running. Do not move tracker!");
-        i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
+			delay(200);
-        delay(100);
+		}
-
+
-        if (status != 0) {
+		i2c.writeReg(Regs::FeatPage, 0);
-            logger.error("CRT failed with status 0x%x. Recalibrate again to enable CRT.", status);
+
-            if (status == 0x03) {
+		uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg)
-                logger.error("Reason: tracker was moved during CRT!");
+					  >> Regs::GyrGainStatus::statusOffset;
-            }
+		// turn gyroscope back on
-        }
+		i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
-        else {
+		delay(100);
-            std::array<uint8_t, 3> crt_values;
+
-            i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data());
+		if (status != 0) {
-            logger.debug("CRT finished successfully, result 0x%x, 0x%x, 0x%x", crt_values[0], crt_values[1], crt_values[2]);
+			logger.error(
-            gyroSensitivity.valid = true;
+				"CRT failed with status 0x%x. Recalibrate again to enable CRT.",
-            gyroSensitivity.x = crt_values[0];
+				status
-            gyroSensitivity.y = crt_values[1];
+			);
-            gyroSensitivity.z = crt_values[2];
+			if (status == 0x03) {
-        }
+				logger.error("Reason: tracker was moved during CRT!");
-
+			}
-        setNormalConfig(gyroSensitivity);
+		} else {
-    }
+			std::array<uint8_t, 3> crt_values;
-
+			i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data());
-    float getDirectTemp() const
+			logger.debug(
-    {
+				"CRT finished successfully, result 0x%x, 0x%x, 0x%x",
-        // middle value is 23 degrees C (0x0000)
+				crt_values[0],
-        // temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 degrees C (0x7FFF)
+				crt_values[1],
-        constexpr float TempStep = 128. / 65535;
+				crt_values[2]
-        const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
+			);
-        return static_cast<float>(value) * TempStep + 23.0f;
+			gyroSensitivity.valid = true;
-    }
+			gyroSensitivity.x = crt_values[0];
-
+			gyroSensitivity.y = crt_values[1];
-    using FifoBuffer = std::array<uint8_t, I2CImpl::MaxTransactionLength>;
+			gyroSensitivity.z = crt_values[2];
-    FifoBuffer read_buffer; 
+		}
-
+
-    template<typename T>
+		setNormalConfig(gyroSensitivity);
-    inline T getFromFifo(uint32_t &position, FifoBuffer& fifo) {
+	}
-        T to_ret;
+
-        std::memcpy(&to_ret, &fifo[position], sizeof(T));
+	float getDirectTemp() const {
-        position += sizeof(T);
+		// middle value is 23 degrees C (0x0000)
-        return to_ret;
+		// temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9
-    }
+		// degrees C (0x7FFF)
-
+		constexpr float TempStep = 128. / 65535;
-    template <typename AccelCall, typename GyroCall>
+		const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
-    void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
+		return static_cast<float>(value) * TempStep + 23.0f;
-        const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
+	}
-        
+
-        const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
+	using FifoBuffer = std::array<uint8_t, I2CImpl::MaxTransactionLength>;
-            static_cast<size_t>(fifo_bytes));
+	FifoBuffer read_buffer;
-        i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+
-
+	template <typename T>
-        for (uint32_t i=0u; i<bytes_to_read;) {
+	inline T getFromFifo(uint32_t& position, FifoBuffer& fifo) {
-            const uint8_t header = getFromFifo<uint8_t>(i, read_buffer);
+		T to_ret;
-            if ((header & Fifo::ModeMask) == Fifo::SkipFrame && (i - bytes_to_read) >= 1) {
+		std::memcpy(&to_ret, &fifo[position], sizeof(T));
-                getFromFifo<uint8_t>(i, read_buffer); // skip 1 byte
+		position += sizeof(T);
-            }
+		return to_ret;
-            else if ((header & Fifo::ModeMask) == Fifo::DataFrame) {
+	}
-                const uint8_t required_length = 
+
-                    (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) +
+	template <typename AccelCall, typename GyroCall>
-                    ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit)) * 6;
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
-                if (i - bytes_to_read < required_length) {
+		const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
-                    // incomplete frame, will be re-read next time
+
-                    break;
+		const auto bytes_to_read = std::min(
-                }
+			static_cast<size_t>(read_buffer.size()),
-                if (header & Fifo::GyrDataBit) {
+			static_cast<size_t>(fifo_bytes)
-                    int16_t gyro[3];
+		);
-                    gyro[0] = getFromFifo<uint16_t>(i, read_buffer);
+		i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
-                    gyro[1] = getFromFifo<uint16_t>(i, read_buffer);
+
-                    gyro[2] = getFromFifo<uint16_t>(i, read_buffer);
+		for (uint32_t i = 0u; i < bytes_to_read;) {
-                    using ShortLimit = std::numeric_limits<int16_t>;
+			const uint8_t header = getFromFifo<uint8_t>(i, read_buffer);
-                    // apply zx factor, todo: this awful line should be simplified and validated
+			if ((header & Fifo::ModeMask) == Fifo::SkipFrame
-                    gyro[0] = std::clamp(static_cast<int32_t>(gyro[0]) - static_cast<int16_t>((static_cast<int32_t>(zxFactor) * gyro[2]) / 512),
+				&& (i - bytes_to_read) >= 1) {
-                                         static_cast<int32_t>(ShortLimit::min()), static_cast<int32_t>(ShortLimit::max()));
+				getFromFifo<uint8_t>(i, read_buffer);  // skip 1 byte
-                    processGyroSample(gyro, GyrTs);
+			} else if ((header & Fifo::ModeMask) == Fifo::DataFrame) {
-                }
+				const uint8_t required_length
-
+					= (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit)
-                if (header & Fifo::AccelDataBit) {
+					   + ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit))
-                    int16_t accel[3];
+					* 6;
-                    accel[0] = getFromFifo<uint16_t>(i, read_buffer);
+				if (i - bytes_to_read < required_length) {
-                    accel[1] = getFromFifo<uint16_t>(i, read_buffer);
+					// incomplete frame, will be re-read next time
-                    accel[2] = getFromFifo<uint16_t>(i, read_buffer);
+					break;
-                    processAccelSample(accel, AccTs);
+				}
-                }
+				if (header & Fifo::GyrDataBit) {
-            }
+					int16_t gyro[3];
-        }
+					gyro[0] = getFromFifo<uint16_t>(i, read_buffer);
-    }
+					gyro[1] = getFromFifo<uint16_t>(i, read_buffer);
-
+					gyro[2] = getFromFifo<uint16_t>(i, read_buffer);
+					using ShortLimit = std::numeric_limits<int16_t>;
+					// apply zx factor, todo: this awful line should be simplified and
+					// validated
+					gyro[0] = std::clamp(
+						static_cast<int32_t>(gyro[0])
+							- static_cast<int16_t>(
+								(static_cast<int32_t>(zxFactor) * gyro[2]) / 512
+							),
+						static_cast<int32_t>(ShortLimit::min()),
+						static_cast<int32_t>(ShortLimit::max())
+					);
+					processGyroSample(gyro, GyrTs);
+				}
+
+				if (header & Fifo::AccelDataBit) {
+					int16_t accel[3];
+					accel[0] = getFromFifo<uint16_t>(i, read_buffer);
+					accel[1] = getFromFifo<uint16_t>(i, read_buffer);
+					accel[2] = getFromFifo<uint16_t>(i, read_buffer);
+					processAccelSample(accel, AccTs);
+				}
+			}
+		}
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/icm42688.h

@@ -1,34 +1,33 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
+	Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
 #include <algorithm>
+#include <array>
+#include <cstdint>
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 // Driver uses acceleration range at 8g
 // and gyroscope range at 1000dps
@@ -36,129 +35,139 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
 template <typename I2CImpl>
-struct ICM42688
+struct ICM42688 {
-{
+	static constexpr uint8_t Address = 0x68;
-    static constexpr uint8_t Address = 0x68;
+	static constexpr auto Name = "ICM-42688";
-    static constexpr auto Name = "ICM-42688";
+	static constexpr auto Type = ImuID::ICM42688;
-    static constexpr auto Type = ImuID::ICM42688;
 
-    static constexpr float GyrTs=1.0/500.0;
+	static constexpr float GyrTs = 1.0 / 500.0;
-    static constexpr float AccTs=1.0/100.0;
+	static constexpr float AccTs = 1.0 / 100.0;
 
-    static constexpr float MagTs=1.0/100;
+	static constexpr float MagTs = 1.0 / 100;
 
-    static constexpr float GyroSensitivity = 32.8f;
+	static constexpr float GyroSensitivity = 32.8f;
-    static constexpr float AccelSensitivity = 4096.0f;
+	static constexpr float AccelSensitivity = 4096.0f;
 
-    I2CImpl i2c;
+	I2CImpl i2c;
-    SlimeVR::Logging::Logger &logger;
+	SlimeVR::Logging::Logger& logger;
-    ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
+	ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-    : i2c(i2c), logger(logger) {}
+		: i2c(i2c)
+		, logger(logger) {}
 
-    struct Regs {
+	struct Regs {
-        struct WhoAmI {
+		struct WhoAmI {
-            static constexpr uint8_t reg = 0x75;
+			static constexpr uint8_t reg = 0x75;
-            static constexpr uint8_t value = 0x47;
+			static constexpr uint8_t value = 0x47;
-        };
+		};
-        static constexpr uint8_t TempData = 0x1d;
+		static constexpr uint8_t TempData = 0x1d;
 
-        struct DeviceConfig {
+		struct DeviceConfig {
-            static constexpr uint8_t reg = 0x11;
+			static constexpr uint8_t reg = 0x11;
-            static constexpr uint8_t valueSwReset = 1;
+			static constexpr uint8_t valueSwReset = 1;
-        };
+		};
-        struct IntfConfig0 {
+		struct IntfConfig0 {
-            static constexpr uint8_t reg = 0x4c;
+			static constexpr uint8_t reg = 0x4c;
-            static constexpr uint8_t value = (0 << 4) | (0 << 5) | (0 << 6); //fifo count in LE, sensor data in LE, fifo size in bytes
+			static constexpr uint8_t value
-        };
+				= (0 << 4) | (0 << 5)
-        struct FifoConfig0 {
+				| (0 << 6);  // fifo count in LE, sensor data in LE, fifo size in bytes
-            static constexpr uint8_t reg = 0x16;
+		};
-            static constexpr uint8_t value = (0b01 << 6); //stream to FIFO mode
+		struct FifoConfig0 {
-        };
+			static constexpr uint8_t reg = 0x16;
-        struct FifoConfig1 {
+			static constexpr uint8_t value = (0b01 << 6);  // stream to FIFO mode
-            static constexpr uint8_t reg = 0x5f;
+		};
-            static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2); //fifo accel en=1, gyro=1, temp=0 todo: fsync, hires 
+		struct FifoConfig1 {
-        };
+			static constexpr uint8_t reg = 0x5f;
-        struct GyroConfig {
+			static constexpr uint8_t value
-            static constexpr uint8_t reg = 0x4f;
+				= 0b1 | (0b1 << 1)
-            static constexpr uint8_t value = (0b001 << 5) | 0b1111; //1000dps, odr=500Hz
+				| (0b0 << 2);  // fifo accel en=1, gyro=1, temp=0 todo: fsync, hires
-        };
+		};
-        struct AccelConfig {
+		struct GyroConfig {
-            static constexpr uint8_t reg = 0x50;
+			static constexpr uint8_t reg = 0x4f;
-            static constexpr uint8_t value = (0b001 << 5) | 0b1000; //8g, odr = 100Hz
+			static constexpr uint8_t value
-        };
+				= (0b001 << 5) | 0b1111;  // 1000dps, odr=500Hz
-        struct PwrMgmt {
+		};
-            static constexpr uint8_t reg = 0x4e;
+		struct AccelConfig {
-            static constexpr uint8_t value = 0b11 | (0b11 << 2); //accel in low noise mode, gyro in low noise
+			static constexpr uint8_t reg = 0x50;
-        };
+			static constexpr uint8_t value = (0b001 << 5) | 0b1000;  // 8g, odr = 100Hz
+		};
+		struct PwrMgmt {
+			static constexpr uint8_t reg = 0x4e;
+			static constexpr uint8_t value
+				= 0b11 | (0b11 << 2);  // accel in low noise mode, gyro in low noise
+		};
 
-        // TODO: might be worth checking
+		// TODO: might be worth checking
-        //GYRO_CONFIG1
+		// GYRO_CONFIG1
-        //GYRO_ACCEL_CONFIG0
+		// GYRO_ACCEL_CONFIG0
-        //ACCEL_CONFIG1
+		// ACCEL_CONFIG1
 
-        static constexpr uint8_t FifoCount = 0x2e;
+		static constexpr uint8_t FifoCount = 0x2e;
-        static constexpr uint8_t FifoData = 0x30;
+		static constexpr uint8_t FifoData = 0x30;
-    };
+	};
 
-    #pragma pack(push, 1)
+#pragma pack(push, 1)
-    struct FifoEntryAligned {
+	struct FifoEntryAligned {
-        union {
+		union {
-            struct {
+			struct {
-                int16_t accel[3];
+				int16_t accel[3];
-                int16_t gyro[3];
+				int16_t gyro[3];
-                uint8_t temp;
+				uint8_t temp;
-                uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned
+				uint8_t timestamp[2];  // cannot do uint16_t because it's unaligned
-            } part;
+			} part;
-            uint8_t raw[15];
+			uint8_t raw[15];
-        };
+		};
-    };
+	};
-    #pragma pack(pop)
+#pragma pack(pop)
 
-    static constexpr size_t FullFifoEntrySize = 16;
+	static constexpr size_t FullFifoEntrySize = 16;
 
-    bool initialize()
+	bool initialize() {
-    {
+		// perform initialization step
-        // perform initialization step
+		i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset);
-        i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset);
+		delay(20);
-        delay(20);
 
-        i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value);
+		i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value);
-        i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
+		i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
-        i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
+		i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
-        i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
+		i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
-        i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
+		i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
-        i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value);
+		i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value);
-        delay(1);
+		delay(1);
 
-        return true;
+		return true;
-    }
+	}
 
-    float getDirectTemp() const
+	float getDirectTemp() const {
-    {
+		const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
-        const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
+		float result = ((float)value / 132.48f) + 25.0f;
-        float result = ((float)value / 132.48f) + 25.0f;
+		return result;
-        return result;
+	}
-    }
 
-    template <typename AccelCall, typename GyroCall>
+	template <typename AccelCall, typename GyroCall>
-    void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
-        const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
+		const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
-        
-        std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
-        const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
-            static_cast<size_t>(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize;
-        i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
-        for (auto i=0u; i<bytes_to_read; i+=FullFifoEntrySize) {
-            FifoEntryAligned entry;
-            memcpy(entry.raw, &read_buffer[i+0x1], sizeof(FifoEntryAligned)); // skip fifo header
-            processGyroSample(entry.part.gyro, GyrTs);
 
-            if (entry.part.accel[0] != -32768) {
+		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
-                processAccelSample(entry.part.accel, AccTs);
+		const auto bytes_to_read = std::min(
-            }            
+									   static_cast<size_t>(read_buffer.size()),
-        }      
+									   static_cast<size_t>(fifo_bytes)
-    }
+								   )
+								 / FullFifoEntrySize * FullFifoEntrySize;
+		i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
+			FifoEntryAligned entry;
+			memcpy(
+				entry.raw,
+				&read_buffer[i + 0x1],
+				sizeof(FifoEntryAligned)
+			);  // skip fifo header
+			processGyroSample(entry.part.gyro, GyrTs);
 
+			if (entry.part.accel[0] != -32768) {
+				processAccelSample(entry.part.accel, AccTs);
+			}
+		}
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

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

@@ -1,99 +1,107 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
 #include <algorithm>
+#include <array>
+#include <cstdint>
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 template <typename I2CImpl>
-struct LSM6DSOutputHandler
+struct LSM6DSOutputHandler {
-{
+	LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-    LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger &logger) 
+		: i2c(i2c)
-        : i2c(i2c), logger(logger)
+		, logger(logger) {}
-    {}
 
-    I2CImpl i2c;
+	I2CImpl i2c;
-    SlimeVR::Logging::Logger &logger;
+	SlimeVR::Logging::Logger& logger;
 
-    template<typename Regs>
+	template <typename Regs>
-    float getDirectTemp() const
+	float getDirectTemp() const {
-    {
+		const auto value = static_cast<int16_t>(i2c.readReg16(Regs::OutTemp));
-        const auto value = static_cast<int16_t>(i2c.readReg16(Regs::OutTemp));
+		float result = ((float)value / 256.0f) + 25.0f;
-        float result = ((float)value / 256.0f) + 25.0f;
 
-        return result;
+		return result;
-    }
+	}
 
-    #pragma pack(push, 1)
+#pragma pack(push, 1)
-    struct FifoEntryAligned {
+	struct FifoEntryAligned {
-        union {
+		union {
-            int16_t xyz[3];
+			int16_t xyz[3];
-            uint8_t raw[6];
+			uint8_t raw[6];
-        };
+		};
-    };
+	};
-    #pragma pack(pop)
+#pragma pack(pop)
 
-    static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
+	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
 
-    template <typename AccelCall, typename GyroCall, typename Regs>
+	template <typename AccelCall, typename GyroCall, typename Regs>
-    void bulkRead(AccelCall &processAccelSample, GyroCall &processGyroSample, float GyrTs, float AccTs) {
+	void bulkRead(
-        constexpr auto FIFO_SAMPLES_MASK = 0x3ff;
+		AccelCall& processAccelSample,
-        constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800;
+		GyroCall& processGyroSample,
-        
+		float GyrTs,
-        const auto fifo_status = i2c.readReg16(Regs::FifoStatus);
+		float AccTs
-        const auto available_axes = fifo_status & FIFO_SAMPLES_MASK;
+	) {
-        const auto fifo_bytes = available_axes * FullFifoEntrySize;
+		constexpr auto FIFO_SAMPLES_MASK = 0x3ff;
-        if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) {
+		constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800;
-            // FIFO overrun is expected to happen during startup and calibration
-            logger.error("FIFO OVERRUN! This occuring during normal usage is an issue.");
-        }
-        
-        std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
-        const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
-            static_cast<size_t>(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize;
-        i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
-        for (auto i=0u; i<bytes_to_read; i+=FullFifoEntrySize) {
-            FifoEntryAligned entry;
-            uint8_t tag = read_buffer[i] >> 3;
-            memcpy(entry.raw, &read_buffer[i+0x1], sizeof(FifoEntryAligned)); // skip fifo header
 
-            switch (tag) {
+		const auto fifo_status = i2c.readReg16(Regs::FifoStatus);
-                case 0x01: // Gyro NC
+		const auto available_axes = fifo_status & FIFO_SAMPLES_MASK;
-                    processGyroSample(entry.xyz, GyrTs);
+		const auto fifo_bytes = available_axes * FullFifoEntrySize;
-                    break;
+		if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) {
-                case 0x02: // Accel NC
+			// FIFO overrun is expected to happen during startup and calibration
-                    processAccelSample(entry.xyz, AccTs);
+			logger.error("FIFO OVERRUN! This occuring during normal usage is an issue."
-                    break;
+			);
-            }
+		}
-        }      
-    }
 
+		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
+		const auto bytes_to_read = std::min(
+									   static_cast<size_t>(read_buffer.size()),
+									   static_cast<size_t>(fifo_bytes)
+								   )
+								 / FullFifoEntrySize * FullFifoEntrySize;
+		i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
+			FifoEntryAligned entry;
+			uint8_t tag = read_buffer[i] >> 3;
+			memcpy(
+				entry.raw,
+				&read_buffer[i + 0x1],
+				sizeof(FifoEntryAligned)
+			);  // skip fifo header
 
+			switch (tag) {
+				case 0x01:  // Gyro NC
+					processGyroSample(entry.xyz, GyrTs);
+					break;
+				case 0x02:  // Accel NC
+					processAccelSample(entry.xyz, AccTs);
+					break;
+			}
+		}
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/lsm6ds3trc.h

@@ -1,139 +1,152 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
+	Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
 #include <algorithm>
+#include <array>
+#include <cstdint>
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 // Driver uses acceleration range at 8g
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 416Hz, accel ODR = 416Hz
 
 template <typename I2CImpl>
-struct LSM6DS3TRC
+struct LSM6DS3TRC {
-{
+	static constexpr uint8_t Address = 0x6a;
-    static constexpr uint8_t Address = 0x6a;
+	static constexpr auto Name = "LSM6DS3TR-C";
-    static constexpr auto Name = "LSM6DS3TR-C";
+	static constexpr auto Type = ImuID::LSM6DS3TRC;
-    static constexpr auto Type = ImuID::LSM6DS3TRC;
 
-    static constexpr float Freq = 416;
+	static constexpr float Freq = 416;
 
-    static constexpr float GyrTs=1.0/Freq;
+	static constexpr float GyrTs = 1.0 / Freq;
-    static constexpr float AccTs=1.0/Freq;
+	static constexpr float AccTs = 1.0 / Freq;
-    static constexpr float MagTs=1.0/Freq;
+	static constexpr float MagTs = 1.0 / Freq;
 
-    static constexpr float GyroSensitivity = 28.571428571f;
+	static constexpr float GyroSensitivity = 28.571428571f;
-    static constexpr float AccelSensitivity = 4098.360655738f;
+	static constexpr float AccelSensitivity = 4098.360655738f;
 
-    I2CImpl i2c;
+	I2CImpl i2c;
-    SlimeVR::Logging::Logger logger;
+	SlimeVR::Logging::Logger logger;
-    LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
+	LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-    : i2c(i2c), logger(logger) {}
+		: i2c(i2c)
+		, logger(logger) {}
 
-    struct Regs {
+	struct Regs {
-        struct WhoAmI {
+		struct WhoAmI {
-            static constexpr uint8_t reg = 0x0f;
+			static constexpr uint8_t reg = 0x0f;
-            static constexpr uint8_t value = 0x6a;
+			static constexpr uint8_t value = 0x6a;
-        };
+		};
-        static constexpr uint8_t OutTemp = 0x20;
+		static constexpr uint8_t OutTemp = 0x20;
-        struct Ctrl1XL {
+		struct Ctrl1XL {
-            static constexpr uint8_t reg = 0x10;
+			static constexpr uint8_t reg = 0x10;
-            static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4); //8g, 416Hz
+			static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4);  // 8g, 416Hz
-        };
+		};
-        struct Ctrl2G {
+		struct Ctrl2G {
-            static constexpr uint8_t reg = 0x11;
+			static constexpr uint8_t reg = 0x11;
-            static constexpr uint8_t value = (0b10 << 2) | (0b0110 << 4); //1000dps, 416Hz
+			static constexpr uint8_t value
-        };
+				= (0b10 << 2) | (0b0110 << 4);  // 1000dps, 416Hz
-        struct Ctrl3C {
+		};
-            static constexpr uint8_t reg = 0x12;
+		struct Ctrl3C {
-            static constexpr uint8_t valueSwReset = 1;
+			static constexpr uint8_t reg = 0x12;
-            static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1
+			static constexpr uint8_t valueSwReset = 1;
-        };
+			static constexpr uint8_t value = (1 << 6) | (1 << 2);  // BDU = 1, IF_INC =
-        struct FifoCtrl3 {
+																   // 1
-            static constexpr uint8_t reg = 0x08;
+		};
-            static constexpr uint8_t value = 0b001 | (0b001 << 3); //accel no decimation, gyro no decimation
+		struct FifoCtrl3 {
-        };
+			static constexpr uint8_t reg = 0x08;
-        struct FifoCtrl5 {
+			static constexpr uint8_t value
-            static constexpr uint8_t reg = 0x0a;
+				= 0b001 | (0b001 << 3);  // accel no decimation, gyro no decimation
-            static constexpr uint8_t value = 0b110 | (0b0111 << 3); //continuous mode, odr = 833Hz
+		};
-        };
+		struct FifoCtrl5 {
+			static constexpr uint8_t reg = 0x0a;
+			static constexpr uint8_t value
+				= 0b110 | (0b0111 << 3);  // continuous mode, odr = 833Hz
+		};
 
-        static constexpr uint8_t FifoStatus = 0x3a;
+		static constexpr uint8_t FifoStatus = 0x3a;
-        static constexpr uint8_t FifoData = 0x3e;
+		static constexpr uint8_t FifoData = 0x3e;
-    };
+	};
 
-    bool initialize()
+	bool initialize() {
-    {
+		// perform initialization step
-        // perform initialization step
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		delay(20);
-        delay(20);
+		i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
-        i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
+		i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value);
-        i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value);
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value);
-        i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value);
+		i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
-        i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
+		return true;
-        return true;
+	}
-    }
 
-    float getDirectTemp() const
+	float getDirectTemp() const {
-    {
+		const auto value = static_cast<int16_t>(i2c.readReg16(Regs::OutTemp));
-        const auto value = static_cast<int16_t>(i2c.readReg16(Regs::OutTemp));
+		float result = ((float)value / 256.0f) + 25.0f;
-        float result = ((float)value / 256.0f) + 25.0f;
 
-        return result;
+		return result;
-    }
+	}
 
-    template <typename AccelCall, typename GyroCall>
+	template <typename AccelCall, typename GyroCall>
-    void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
-        const auto read_result = i2c.readReg16(Regs::FifoStatus);
+		const auto read_result = i2c.readReg16(Regs::FifoStatus);
-        if (read_result & 0x4000) { // overrun!
+		if (read_result & 0x4000) {  // overrun!
-            // disable and re-enable fifo to clear it
+			// disable and re-enable fifo to clear it
-            logger.debug("Fifo overrun, resetting...");
+			logger.debug("Fifo overrun, resetting...");
-            i2c.writeReg(Regs::FifoCtrl5::reg, 0);
+			i2c.writeReg(Regs::FifoCtrl5::reg, 0);
-            i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
+			i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
-            return;
+			return;
-        }
+		}
-        const auto unread_entries = read_result & 0x7ff;
+		const auto unread_entries = read_result & 0x7ff;
-        constexpr auto single_measurement_words = 6;
+		constexpr auto single_measurement_words = 6;
-        constexpr auto single_measurement_bytes = sizeof(uint16_t) * single_measurement_words;
+		constexpr auto single_measurement_bytes
-        
+			= sizeof(uint16_t) * single_measurement_words;
-        std::array<int16_t, 60> read_buffer; // max 10 packages of 6 16bit values of data form fifo
-        const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()), static_cast<size_t>(unread_entries)) \
-                     * sizeof(uint16_t) / single_measurement_bytes * single_measurement_bytes;
-
-        i2c.readBytes(Regs::FifoData, bytes_to_read, reinterpret_cast<uint8_t *>(read_buffer.data()));
-        for (uint16_t i=0; i<bytes_to_read/sizeof(uint16_t); i+=single_measurement_words) {
-            processGyroSample(reinterpret_cast<const int16_t *>(&read_buffer[i]), GyrTs);
-            processAccelSample(reinterpret_cast<const int16_t *>(&read_buffer[i+3]), AccTs);
-        }
-    }
 
+		std::array<int16_t, 60>
+			read_buffer;  // max 10 packages of 6 16bit values of data form fifo
+		const auto bytes_to_read = std::min(
+									   static_cast<size_t>(read_buffer.size()),
+									   static_cast<size_t>(unread_entries)
+								   )
+								 * sizeof(uint16_t) / single_measurement_bytes
+								 * single_measurement_bytes;
 
+		i2c.readBytes(
+			Regs::FifoData,
+			bytes_to_read,
+			reinterpret_cast<uint8_t*>(read_buffer.data())
+		);
+		for (uint16_t i = 0; i < bytes_to_read / sizeof(uint16_t);
+			 i += single_measurement_words) {
+			processGyroSample(reinterpret_cast<const int16_t*>(&read_buffer[i]), GyrTs);
+			processAccelSample(
+				reinterpret_cast<const int16_t*>(&read_buffer[i + 3]),
+				AccTs
+			);
+		}
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/lsm6dso.h

@@ -1,120 +1,121 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
 #include <algorithm>
+#include <array>
+#include <cstdint>
 
 #include "lsm6ds-common.h"
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 // Driver uses acceleration range at 8g
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 416Hz, accel ODR = 104Hz
 
 template <typename I2CImpl>
-struct LSM6DSO : LSM6DSOutputHandler<I2CImpl>
+struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
-{
+	static constexpr uint8_t Address = 0x6a;
-    static constexpr uint8_t Address = 0x6a;
+	static constexpr auto Name = "LSM6DSO";
-    static constexpr auto Name = "LSM6DSO";
+	static constexpr auto Type = ImuID::LSM6DSO;
-    static constexpr auto Type = ImuID::LSM6DSO;
 
-    static constexpr float GyrFreq = 416;
+	static constexpr float GyrFreq = 416;
-    static constexpr float AccFreq = 104;
+	static constexpr float AccFreq = 104;
-    static constexpr float MagFreq = 120;
+	static constexpr float MagFreq = 120;
 
-    static constexpr float GyrTs=1.0/GyrFreq;
+	static constexpr float GyrTs = 1.0 / GyrFreq;
-    static constexpr float AccTs=1.0/AccFreq;
+	static constexpr float AccTs = 1.0 / AccFreq;
-    static constexpr float MagTs=1.0/MagFreq;
+	static constexpr float MagTs = 1.0 / MagFreq;
 
-    static constexpr float GyroSensitivity = 1000 / 35.0f;
+	static constexpr float GyroSensitivity = 1000 / 35.0f;
-    static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.244f;
 
-    using LSM6DSOutputHandler<I2CImpl>::i2c;
+	using LSM6DSOutputHandler<I2CImpl>::i2c;
 
-    struct Regs {
+	struct Regs {
-        struct WhoAmI {
+		struct WhoAmI {
-            static constexpr uint8_t reg = 0x0f;
+			static constexpr uint8_t reg = 0x0f;
-            static constexpr uint8_t value = 0x6c;
+			static constexpr uint8_t value = 0x6c;
-        };
+		};
-        static constexpr uint8_t OutTemp = 0x20;
+		static constexpr uint8_t OutTemp = 0x20;
-        struct Ctrl1XL {
+		struct Ctrl1XL {
-            static constexpr uint8_t reg = 0x10;
+			static constexpr uint8_t reg = 0x10;
-            static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS
+			static constexpr uint8_t value = (0b01001100);  // XL at 104 Hz, 8g FS
-        };
+		};
-        struct Ctrl2GY {
+		struct Ctrl2GY {
-            static constexpr uint8_t reg = 0x11;
+			static constexpr uint8_t reg = 0x11;
-            static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS
+			static constexpr uint8_t value = (0b01101000);  // GY at 416 Hz, 1000dps FS
-        };
+		};
-        struct Ctrl3C {
+		struct Ctrl3C {
-            static constexpr uint8_t reg = 0x12;
+			static constexpr uint8_t reg = 0x12;
-            static constexpr uint8_t valueSwReset = 1;
+			static constexpr uint8_t valueSwReset = 1;
-            static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1
+			static constexpr uint8_t value = (1 << 6) | (1 << 2);  // BDU = 1, IF_INC =
-        };
+																   // 1
-        struct FifoCtrl3BDR {
+		};
-            static constexpr uint8_t reg = 0x09;
+		struct FifoCtrl3BDR {
-            static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz
+			static constexpr uint8_t reg = 0x09;
-        };
+			static constexpr uint8_t value
-        struct FifoCtrl4Mode {
+				= (0b0110) | (0b0110 << 4);  // gyro and accel batched at 417Hz
-            static constexpr uint8_t reg = 0x0a;
+		};
-            static constexpr uint8_t value = (0b110); //continuous mode
+		struct FifoCtrl4Mode {
-        };
+			static constexpr uint8_t reg = 0x0a;
+			static constexpr uint8_t value = (0b110);  // continuous mode
+		};
 
-        static constexpr uint8_t FifoStatus = 0x3a;
+		static constexpr uint8_t FifoStatus = 0x3a;
-        static constexpr uint8_t FifoData = 0x78;
+		static constexpr uint8_t FifoData = 0x78;
-    };
+	};
 
-    LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
+	LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-        : LSM6DSOutputHandler<I2CImpl>(i2c, logger) {
+		: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
-    }
 
-    bool initialize()
+	bool initialize() {
-    {
+		// perform initialization step
-        // perform initialization step
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		delay(20);
-        delay(20);
+		i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
-        i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
+		i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
-        i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
-        i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
+		i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
-        i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
+		return true;
-        return true;
+	}
-    }
 
-    float getDirectTemp() const
+	float getDirectTemp() const {
-    {
+		return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
-        return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
+	}
-    }
-
-    template <typename AccelCall, typename GyroCall>
-    void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
-        LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(processAccelSample, processGyroSample, GyrTs, AccTs);
-    }
 
+	template <typename AccelCall, typename GyroCall>
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
+		LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(
+			processAccelSample,
+			processGyroSample,
+			GyrTs,
+			AccTs
+		);
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/lsm6dsr.h

@@ -1,120 +1,121 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
 #include <algorithm>
+#include <array>
+#include <cstdint>
 
 #include "lsm6ds-common.h"
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 // Driver uses acceleration range at 8g
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 416Hz, accel ODR = 104Hz
 
 template <typename I2CImpl>
-struct LSM6DSR : LSM6DSOutputHandler<I2CImpl>
+struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
-{
+	static constexpr uint8_t Address = 0x6a;
-    static constexpr uint8_t Address = 0x6a;
+	static constexpr auto Name = "LSM6DSR";
-    static constexpr auto Name = "LSM6DSR";
+	static constexpr auto Type = ImuID::LSM6DSR;
-    static constexpr auto Type = ImuID::LSM6DSR;
 
-    static constexpr float GyrFreq = 416;
+	static constexpr float GyrFreq = 416;
-    static constexpr float AccFreq = 104;
+	static constexpr float AccFreq = 104;
-    static constexpr float MagFreq = 120;
+	static constexpr float MagFreq = 120;
 
-    static constexpr float GyrTs=1.0/GyrFreq;
+	static constexpr float GyrTs = 1.0 / GyrFreq;
-    static constexpr float AccTs=1.0/AccFreq;
+	static constexpr float AccTs = 1.0 / AccFreq;
-    static constexpr float MagTs=1.0/MagFreq;
+	static constexpr float MagTs = 1.0 / MagFreq;
 
-    static constexpr float GyroSensitivity = 1000 / 35.0f;
+	static constexpr float GyroSensitivity = 1000 / 35.0f;
-    static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.244f;
 
-    using LSM6DSOutputHandler<I2CImpl>::i2c;
+	using LSM6DSOutputHandler<I2CImpl>::i2c;
 
-    struct Regs {
+	struct Regs {
-        struct WhoAmI {
+		struct WhoAmI {
-            static constexpr uint8_t reg = 0x0f;
+			static constexpr uint8_t reg = 0x0f;
-            static constexpr uint8_t value = 0x6b;
+			static constexpr uint8_t value = 0x6b;
-        };
+		};
-        static constexpr uint8_t OutTemp = 0x20;
+		static constexpr uint8_t OutTemp = 0x20;
-        struct Ctrl1XL {
+		struct Ctrl1XL {
-            static constexpr uint8_t reg = 0x10;
+			static constexpr uint8_t reg = 0x10;
-            static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS
+			static constexpr uint8_t value = (0b01001100);  // XL at 104 Hz, 8g FS
-        };
+		};
-        struct Ctrl2GY {
+		struct Ctrl2GY {
-            static constexpr uint8_t reg = 0x11;
+			static constexpr uint8_t reg = 0x11;
-            static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS
+			static constexpr uint8_t value = (0b01101000);  // GY at 416 Hz, 1000dps FS
-        };
+		};
-        struct Ctrl3C {
+		struct Ctrl3C {
-            static constexpr uint8_t reg = 0x12;
+			static constexpr uint8_t reg = 0x12;
-            static constexpr uint8_t valueSwReset = 1;
+			static constexpr uint8_t valueSwReset = 1;
-            static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1
+			static constexpr uint8_t value = (1 << 6) | (1 << 2);  // BDU = 1, IF_INC =
-        };
+																   // 1
-        struct FifoCtrl3BDR {
+		};
-            static constexpr uint8_t reg = 0x09;
+		struct FifoCtrl3BDR {
-            static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz
+			static constexpr uint8_t reg = 0x09;
-        };
+			static constexpr uint8_t value
-        struct FifoCtrl4Mode {
+				= (0b0110) | (0b0110 << 4);  // gyro and accel batched at 417Hz
-            static constexpr uint8_t reg = 0x0a;
+		};
-            static constexpr uint8_t value = (0b110); //continuous mode
+		struct FifoCtrl4Mode {
-        };
+			static constexpr uint8_t reg = 0x0a;
+			static constexpr uint8_t value = (0b110);  // continuous mode
+		};
 
-        static constexpr uint8_t FifoStatus = 0x3a;
+		static constexpr uint8_t FifoStatus = 0x3a;
-        static constexpr uint8_t FifoData = 0x78;
+		static constexpr uint8_t FifoData = 0x78;
-    };
+	};
 
-    LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
+	LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-        : LSM6DSOutputHandler<I2CImpl>(i2c, logger) {
+		: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
-    }
 
-    bool initialize()
+	bool initialize() {
-    {
+		// perform initialization step
-        // perform initialization step
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		delay(20);
-        delay(20);
+		i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
-        i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
+		i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
-        i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
-        i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
+		i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
-        i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
+		return true;
-        return true;
+	}
-    }
 
-    float getDirectTemp() const
+	float getDirectTemp() const {
-    {
+		return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
-        return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
+	}
-    }
-
-    template <typename AccelCall, typename GyroCall>
-    void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
-        LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(processAccelSample, processGyroSample, GyrTs, AccTs);
-    }
 
+	template <typename AccelCall, typename GyroCall>
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
+		LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(
+			processAccelSample,
+			processGyroSample,
+			GyrTs,
+			AccTs
+		);
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/lsm6dsv.h

@@ -1,135 +1,136 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
 #include <algorithm>
+#include <array>
+#include <cstdint>
 
 #include "lsm6ds-common.h"
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 // Driver uses acceleration range at 8g
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 480Hz, accel ODR = 120Hz
 
 template <typename I2CImpl>
-struct LSM6DSV : LSM6DSOutputHandler<I2CImpl>
+struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
-{
+	static constexpr uint8_t Address = 0x6a;
-    static constexpr uint8_t Address = 0x6a;
+	static constexpr auto Name = "LSM6DSV";
-    static constexpr auto Name = "LSM6DSV";
+	static constexpr auto Type = ImuID::LSM6DSV;
-    static constexpr auto Type = ImuID::LSM6DSV;
 
-    static constexpr float GyrFreq = 480;
+	static constexpr float GyrFreq = 480;
-    static constexpr float AccFreq = 120;
+	static constexpr float AccFreq = 120;
-    static constexpr float MagFreq = 120;
+	static constexpr float MagFreq = 120;
 
-    static constexpr float GyrTs=1.0/GyrFreq;
+	static constexpr float GyrTs = 1.0 / GyrFreq;
-    static constexpr float AccTs=1.0/AccFreq;
+	static constexpr float AccTs = 1.0 / AccFreq;
-    static constexpr float MagTs=1.0/MagFreq;
+	static constexpr float MagTs = 1.0 / MagFreq;
 
-    static constexpr float GyroSensitivity = 1000 / 35.0f;
+	static constexpr float GyroSensitivity = 1000 / 35.0f;
-    static constexpr float AccelSensitivity = 1000 / 0.244f;
+	static constexpr float AccelSensitivity = 1000 / 0.244f;
 
-    using LSM6DSOutputHandler<I2CImpl>::i2c;
+	using LSM6DSOutputHandler<I2CImpl>::i2c;
 
-    struct Regs {
+	struct Regs {
-        struct WhoAmI {
+		struct WhoAmI {
-            static constexpr uint8_t reg = 0x0f;
+			static constexpr uint8_t reg = 0x0f;
-            static constexpr uint8_t value = 0x70;
+			static constexpr uint8_t value = 0x70;
-        };
+		};
-        static constexpr uint8_t OutTemp = 0x20;
+		static constexpr uint8_t OutTemp = 0x20;
-        struct HAODRCFG {
+		struct HAODRCFG {
-            static constexpr uint8_t reg = 0x62;
+			static constexpr uint8_t reg = 0x62;
-            static constexpr uint8_t value = (0b00); //1st ODR table
+			static constexpr uint8_t value = (0b00);  // 1st ODR table
-        };
+		};
-        struct Ctrl1XLODR {
+		struct Ctrl1XLODR {
-            static constexpr uint8_t reg = 0x10;
+			static constexpr uint8_t reg = 0x10;
-            static constexpr uint8_t value = (0b0010110); //120Hz, HAODR
+			static constexpr uint8_t value = (0b0010110);  // 120Hz, HAODR
-        };
+		};
-        struct Ctrl2GODR {
+		struct Ctrl2GODR {
-            static constexpr uint8_t reg = 0x11;
+			static constexpr uint8_t reg = 0x11;
-            static constexpr uint8_t value = (0b0011000); //480Hz, HAODR
+			static constexpr uint8_t value = (0b0011000);  // 480Hz, HAODR
-        };
+		};
-        struct Ctrl3C {
+		struct Ctrl3C {
-            static constexpr uint8_t reg = 0x12;
+			static constexpr uint8_t reg = 0x12;
-            static constexpr uint8_t valueSwReset = 1;
+			static constexpr uint8_t valueSwReset = 1;
-            static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1
+			static constexpr uint8_t value = (1 << 6) | (1 << 2);  // BDU = 1, IF_INC =
-        };
+																   // 1
-        struct Ctrl6GFS {
+		};
-            static constexpr uint8_t reg = 0x15;
+		struct Ctrl6GFS {
-            static constexpr uint8_t value = (0b0011); //1000dps
+			static constexpr uint8_t reg = 0x15;
-        };
+			static constexpr uint8_t value = (0b0011);  // 1000dps
-        struct Ctrl8XLFS {
+		};
-            static constexpr uint8_t reg = 0x17;
+		struct Ctrl8XLFS {
-            static constexpr uint8_t value = (0b10); //8g
+			static constexpr uint8_t reg = 0x17;
-        };
+			static constexpr uint8_t value = (0b10);  // 8g
-        struct FifoCtrl3BDR {
+		};
-            static constexpr uint8_t reg = 0x09;
+		struct FifoCtrl3BDR {
-            static constexpr uint8_t value = (0b1000) | (0b1000 << 4); //gyro and accel batched at 480Hz
+			static constexpr uint8_t reg = 0x09;
-        };
+			static constexpr uint8_t value
-        struct FifoCtrl4Mode {
+				= (0b1000) | (0b1000 << 4);  // gyro and accel batched at 480Hz
-            static constexpr uint8_t reg = 0x0a;
+		};
-            static constexpr uint8_t value = (0b110); //continuous mode
+		struct FifoCtrl4Mode {
-        };
+			static constexpr uint8_t reg = 0x0a;
+			static constexpr uint8_t value = (0b110);  // continuous mode
+		};
 
-        static constexpr uint8_t FifoStatus = 0x1b;
+		static constexpr uint8_t FifoStatus = 0x1b;
-        static constexpr uint8_t FifoData = 0x78;
+		static constexpr uint8_t FifoData = 0x78;
-    };
+	};
 
-    LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
+	LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-        : LSM6DSOutputHandler<I2CImpl>(i2c, logger) {
+		: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
-    }
 
-    bool initialize()
+	bool initialize() {
-    {
+		// perform initialization step
-        // perform initialization step
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		delay(20);
-        delay(20);
+		i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value);
-        i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value);
+		i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value);
-        i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value);
+		i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value);
-        i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value);
+		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-        i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value);
-        i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value);
+		i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value);
-        i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value);
+		i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
-        i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
+		i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
-        i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
+		return true;
-        return true;
+	}
-    }
 
-    float getDirectTemp() const
+	float getDirectTemp() const {
-    {
+		return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
-        return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
+	}
-    }
-
-    template <typename AccelCall, typename GyroCall>
-    void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
-        LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(processAccelSample, processGyroSample, GyrTs, AccTs);
-    }
 
+	template <typename AccelCall, typename GyroCall>
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
+		LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(
+			processAccelSample,
+			processGyroSample,
+			GyrTs,
+			AccTs
+		);
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/mpu6050.h

@@ -1,185 +1,210 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2024 furrycoding & SlimeVR Contributors
+	Copyright (c) 2024 furrycoding & SlimeVR Contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #pragma once
 
-#include <cstdint>
-#include <array>
-#include <algorithm>
-
 #include <MPU6050.h>
 
+#include <algorithm>
+#include <array>
+#include <cstdint>
 
-namespace SlimeVR::Sensors::SoftFusion::Drivers
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
-{
 
 // Driver uses acceleration range at 8g
 // and gyroscope range at 1000dps
 // Gyroscope ODR = accel ODR = 250Hz
 
 template <typename I2CImpl>
-struct MPU6050
+struct MPU6050 {
-{
+	struct FifoSample {
-    struct FifoSample {
+		uint8_t accel_x_h, accel_x_l;
-        uint8_t accel_x_h, accel_x_l;
+		uint8_t accel_y_h, accel_y_l;
-        uint8_t accel_y_h, accel_y_l;
+		uint8_t accel_z_h, accel_z_l;
-        uint8_t accel_z_h, accel_z_l;
 
-        // We don't need temperature in FIFO
+		// We don't need temperature in FIFO
-        // uint8_t temp_h, temp_l;
+		// uint8_t temp_h, temp_l;
 
-        uint8_t gyro_x_h, gyro_x_l;
+		uint8_t gyro_x_h, gyro_x_l;
-        uint8_t gyro_y_h, gyro_y_l;
+		uint8_t gyro_y_h, gyro_y_l;
-        uint8_t gyro_z_h, gyro_z_l;
+		uint8_t gyro_z_h, gyro_z_l;
-    };
+	};
-    #define MPU6050_FIFO_VALUE(fifo, name) (((int16_t)fifo->name##_h << 8) | ((int16_t)fifo->name##_l))
+#define MPU6050_FIFO_VALUE(fifo, name) \
+	(((int16_t)fifo->name##_h << 8) | ((int16_t)fifo->name##_l))
 
-    static constexpr uint8_t Address = 0x68;
+	static constexpr uint8_t Address = 0x68;
-    static constexpr auto Name = "MPU-6050";
+	static constexpr auto Name = "MPU-6050";
-    static constexpr auto Type = ImuID::MPU6050;
+	static constexpr auto Type = ImuID::MPU6050;
 
-    static constexpr float Freq = 250;
+	static constexpr float Freq = 250;
 
-    static constexpr float GyrTs = 1.0 / Freq;
+	static constexpr float GyrTs = 1.0 / Freq;
-    static constexpr float AccTs = 1.0 / Freq;
+	static constexpr float AccTs = 1.0 / Freq;
-    static constexpr float MagTs = 1.0 / Freq;
+	static constexpr float MagTs = 1.0 / Freq;
 
-    static constexpr float GyroSensitivity = 32.8f;
+	static constexpr float GyroSensitivity = 32.8f;
-    static constexpr float AccelSensitivity = 4096.0f;
+	static constexpr float AccelSensitivity = 4096.0f;
 
-    I2CImpl i2c;
+	I2CImpl i2c;
-    SlimeVR::Logging::Logger &logger;
+	SlimeVR::Logging::Logger& logger;
-    MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
+	MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-    : i2c(i2c), logger(logger) {}
+		: i2c(i2c)
+		, logger(logger) {}
 
-    struct Regs {
+	struct Regs {
-        struct WhoAmI {
+		struct WhoAmI {
-            static constexpr uint8_t reg = 0x75;
+			static constexpr uint8_t reg = 0x75;
-            static constexpr uint8_t value = 0x68;
+			static constexpr uint8_t value = 0x68;
-        };
+		};
 
-        struct UserCtrl {
+		struct UserCtrl {
-            static constexpr uint8_t reg = 0x6A;
+			static constexpr uint8_t reg = 0x6A;
-            static constexpr uint8_t fifoResetValue = (1 << MPU6050_USERCTRL_FIFO_EN_BIT) | (1 << MPU6050_USERCTRL_FIFO_RESET_BIT);
+			static constexpr uint8_t fifoResetValue
-        };
+				= (1 << MPU6050_USERCTRL_FIFO_EN_BIT)
+				| (1 << MPU6050_USERCTRL_FIFO_RESET_BIT);
+		};
 
-        struct GyroConfig {
+		struct GyroConfig {
-            static constexpr uint8_t reg = 0x1b;
+			static constexpr uint8_t reg = 0x1b;
-            static constexpr uint8_t value = 0b10 << 3; // 1000dps
+			static constexpr uint8_t value = 0b10 << 3;  // 1000dps
-        };
+		};
 
-        struct AccelConfig {
+		struct AccelConfig {
-            static constexpr uint8_t reg = 0x1c;
+			static constexpr uint8_t reg = 0x1c;
-            static constexpr uint8_t value = 0b10 << 3; // 8g
+			static constexpr uint8_t value = 0b10 << 3;  // 8g
-        };
+		};
 
-        static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H;
+		static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H;
 
-        static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS;
+		static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS;
 
-        static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH;
+		static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH;
-        static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W;
+		static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W;
-    };
+	};
 
-    inline uint16_t byteSwap(uint16_t value) const {
+	inline uint16_t byteSwap(uint16_t value) const {
-        // Swap bytes because MPU is big-endian
+		// Swap bytes because MPU is big-endian
-        return (value >> 8) | (value << 8);
+		return (value >> 8) | (value << 8);
-    }
+	}
 
-    void resetFIFO() {
+	void resetFIFO() {
-        i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue);
+		i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue);
-    }
+	}
 
-    bool initialize()
+	bool initialize() {
-    {
+		// Reset
-        // Reset
+		i2c.writeReg(
-        i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1: reset with 100ms delay (also disables sleep)
+			MPU6050_RA_PWR_MGMT_1,
-        delay(100);
+			0x80
-        i2c.writeReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x07); // full SIGNAL_PATH_RESET: with another 100ms delay
+		);  // PWR_MGMT_1: reset with 100ms delay (also disables sleep)
-        delay(100);
+		delay(100);
-        
+		i2c.writeReg(
-        // Configure
+			MPU6050_RA_SIGNAL_PATH_RESET,
-        i2c.writeReg(MPU6050_RA_PWR_MGMT_1,   0x01); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
+			0x07
-        i2c.writeReg(MPU6050_RA_USER_CTRL,    0x00); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP
+		);  // full SIGNAL_PATH_RESET: with another 100ms delay
-        i2c.writeReg(MPU6050_RA_INT_ENABLE,   0x10); // 0001 0000 INT_ENABLE: only FIFO overflow interrupt
+		delay(100);
-        i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
-        i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
-        i2c.writeReg(MPU6050_RA_CONFIG,       0x02); // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro output rate to 1KHz)
-        i2c.writeReg(MPU6050_RA_SMPLRT_DIV,   0x03); // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
 
-        i2c.writeReg(MPU6050_RA_FIFO_EN,      0x78); // 0111 1000 FIFO_EN: All gyro axes + Accel
+		// Configure
+		i2c.writeReg(
+			MPU6050_RA_PWR_MGMT_1,
+			0x01
+		);  // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
+		i2c.writeReg(
+			MPU6050_RA_USER_CTRL,
+			0x00
+		);  // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP
+		i2c.writeReg(
+			MPU6050_RA_INT_ENABLE,
+			0x10
+		);  // 0001 0000 INT_ENABLE: only FIFO overflow interrupt
+		i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
+		i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
+		i2c.writeReg(
+			MPU6050_RA_CONFIG,
+			0x02
+		);  // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro
+			// output rate to 1KHz)
+		i2c.writeReg(
+			MPU6050_RA_SMPLRT_DIV,
+			0x03
+		);  // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate
+			// = Gyroscope Output Rate / (1 + SMPLRT_DIV))
 
-        resetFIFO();
+		i2c.writeReg(
+			MPU6050_RA_FIFO_EN,
+			0x78
+		);  // 0111 1000 FIFO_EN: All gyro axes + Accel
 
-        return true;
+		resetFIFO();
-    }
 
-    float getDirectTemp() const
+		return true;
-    {
+	}
-        auto value = byteSwap(i2c.readReg16(Regs::OutTemp));
-        float result = (static_cast<int16_t>(value) / 340.0f) + 36.53f;
-        return result;
-    }
 
-    template <typename AccelCall, typename GyroCall>
+	float getDirectTemp() const {
-    void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
+		auto value = byteSwap(i2c.readReg16(Regs::OutTemp));
-        const auto status = i2c.readReg(Regs::IntStatus);
+		float result = (static_cast<int16_t>(value) / 340.0f) + 36.53f;
+		return result;
+	}
 
-        if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) {
+	template <typename AccelCall, typename GyroCall>
-            // Overflows make it so we lose track of which packet is which
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
-            // This necessitates a reset
+		const auto status = i2c.readReg(Regs::IntStatus);
-            logger.debug("Fifo overrun, resetting...");
-            resetFIFO();
-            return;
-        }
 
-        std::array<uint8_t, 12 * 10> readBuffer; // max 10 packages of 12byte values (sample) of data form fifo
+		if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) {
-        auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount));
+			// Overflows make it so we lose track of which packet is which
+			// This necessitates a reset
+			logger.debug("Fifo overrun, resetting...");
+			resetFIFO();
+			return;
+		}
 
-        auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size()) / sizeof(FifoSample) * sizeof(FifoSample);
+		std::array<uint8_t, 12 * 10>
-        if (!readBytes) {
+			readBuffer;  // max 10 packages of 12byte values (sample) of data form fifo
-            return;
+		auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount));
-        }
 
-        i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data());
+		auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size())
-        for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) {
+					   / sizeof(FifoSample) * sizeof(FifoSample);
-            const FifoSample *sample = reinterpret_cast<FifoSample *>(&readBuffer[i]);
+		if (!readBytes) {
-            
+			return;
-            int16_t xyz[3];
+		}
 
-            xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x);
+		i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data());
-            xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y);
+		for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) {
-            xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z);
+			const FifoSample* sample = reinterpret_cast<FifoSample*>(&readBuffer[i]);
-            processAccelSample(xyz, AccTs);
 
-            xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x);
+			int16_t xyz[3];
-            xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y);
-            xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z);
-            processGyroSample(xyz, GyrTs);
-        }
-    }
 
+			xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x);
+			xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y);
+			xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z);
+			processAccelSample(xyz, AccTs);
 
+			xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x);
+			xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y);
+			xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z);
+			processGyroSample(xyz, GyrTs);
+		}
+	}
 };
 
-} // namespace
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/i2cimpl.h

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

src/serial/serialcommands.cpp

@@ -1,342 +1,376 @@
 /*
-    SlimeVR Code is placed under the MIT license
+	SlimeVR Code is placed under the MIT license
-    Copyright (c) 2021 Eiren Rain & SlimeVR contributors
+	Copyright (c) 2021 Eiren Rain & SlimeVR contributors
 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
+	Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
+	of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
+	in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
+	copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+	furnished to do so, subject to the following conditions:
 
-    The above copyright notice and this permission notice shall be included in
+	The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
+	all copies or substantial portions of the Software.
 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	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
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
+	THE SOFTWARE.
 */
 
 #include "serialcommands.h"
-#include "logging/Logger.h"
+
 #include <CmdCallback.hpp>
-#include "base64.hpp"
+
 #include "GlobalVars.h"
+#include "base64.hpp"
 #include "batterymonitor.h"
+#include "logging/Logger.h"
 #include "utils.h"
 
 #if ESP32
-    #include "nvs_flash.h"
+#include "nvs_flash.h"
 #endif
 
 namespace SerialCommands {
-    SlimeVR::Logging::Logger logger("SerialCommands");
+SlimeVR::Logging::Logger logger("SerialCommands");
 
-    CmdCallback<6> cmdCallbacks;
+CmdCallback<6> cmdCallbacks;
-    CmdParser cmdParser;
+CmdParser cmdParser;
-    CmdBuffer<256> cmdBuffer;
+CmdBuffer<256> cmdBuffer;
 
-
+bool lengthCheck(
-	bool lengthCheck (const char* const text, unsigned int length, const char* const cmd, const char* const name)
+	const char* const text,
-	{
+	unsigned int length,
-		size_t l = text != nullptr ? strlen(text) : 0;
+	const char* const cmd,
-		if ((l > length)) {
+	const char* const name
-			logger.error("%s ERROR: %s is longer than %d bytes / Characters", cmd, name, length);
+) {
-			return false;
+	size_t l = text != nullptr ? strlen(text) : 0;
-		}
+	if ((l > length)) {
-		return true;
+		logger.error(
+			"%s ERROR: %s is longer than %d bytes / Characters",
+			cmd,
+			name,
+			length
+		);
+		return false;
 	}
+	return true;
+}
 
-	unsigned int decode_base64_length_null(const char* const b64char, unsigned int* b64ssidlength)
+unsigned int
-	{
+decode_base64_length_null(const char* const b64char, unsigned int* b64ssidlength) {
-		if (b64char==NULL) {
+	if (b64char == NULL) {
-			return 0;
+		return 0;
-		}
-		*b64ssidlength = (unsigned int)strlen(b64char);
-		return decode_base64_length((unsigned char *)b64char, *b64ssidlength);
 	}
+	*b64ssidlength = (unsigned int)strlen(b64char);
+	return decode_base64_length((unsigned char*)b64char, *b64ssidlength);
+}
 
-	void cmdSet(CmdParser * parser) {
+void cmdSet(CmdParser* parser) {
-		if(parser->getParamCount() != 1) {
+	if (parser->getParamCount() != 1) {
-			if (parser->equalCmdParam(1, "WIFI")) {
+		if (parser->equalCmdParam(1, "WIFI")) {
-				if(parser->getParamCount() < 3) {
+			if (parser->getParamCount() < 3) {
-					logger.error("CMD SET WIFI ERROR: Too few arguments");
+				logger.error("CMD SET WIFI ERROR: Too few arguments");
-					logger.info("Syntax: SET WIFI \"<SSID>\" \"<PASSWORD>\"");
+				logger.info("Syntax: SET WIFI \"<SSID>\" \"<PASSWORD>\"");
-				} else {
-					const char *sc_ssid = parser->getCmdParam(2);
-					const char *sc_pw = parser->getCmdParam(3);
-
-					if (!lengthCheck(sc_ssid, 32, "CMD SET WIFI", "SSID") &&
-						!lengthCheck(sc_pw, 64, "CMD SET WIFI", "Password")) {
-						return;
-					}
-
-					WiFiNetwork::setWiFiCredentials(sc_ssid, sc_pw);
-					logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting");
-				}
-			} else if (parser->equalCmdParam(1, "BWIFI")) {
-				if(parser->getParamCount() < 3) {
-					logger.error("CMD SET BWIFI ERROR: Too few arguments");
-					logger.info("Syntax: SET BWIFI <B64SSID> <B64PASSWORD>");
-				} else {
-					const char * b64ssid = parser->getCmdParam(2);
-					const char * b64pass = parser->getCmdParam(3);
-					unsigned int b64ssidlength = 0;
-					unsigned int b64passlength = 0;
-					unsigned int ssidlength = decode_base64_length_null(b64ssid, &b64ssidlength);
-					unsigned int passlength = decode_base64_length_null(b64pass, &b64passlength);
-
-					// alloc the strings and set them to 0 (null terminating)
-					char ssid[ssidlength+1];
-					memset(ssid, 0, ssidlength+1);
-					char pass[passlength+1];
-					memset(pass, 0, passlength+1);
-					// make a pointer to pass
-					char *ppass = pass;
-					decode_base64((const unsigned char *)b64ssid, b64ssidlength, (unsigned char*)ssid);
-					if (!lengthCheck(ssid, 32, "CMD SET BWIFI", "SSID")) {
-						return;
-					}
-
-					if ((b64pass!=NULL) && (b64passlength > 0)) {
-						decode_base64((const unsigned char *)b64pass, b64passlength, (unsigned char*)pass);
-						if (!lengthCheck(pass, 64, "CMD SET BWIFI", "Password")) {
-							return;
-						}
-					} else {
-						// set the pointer for pass to null for no password
-						ppass = NULL;
-					}
-					WiFiNetwork::setWiFiCredentials(ssid, ppass);
-					logger.info("CMD SET BWIFI OK: New wifi credentials set, reconnecting");
-				}
 			} else {
-				logger.error("CMD SET ERROR: Unrecognized variable to set");
+				const char* sc_ssid = parser->getCmdParam(2);
+				const char* sc_pw = parser->getCmdParam(3);
+
+				if (!lengthCheck(sc_ssid, 32, "CMD SET WIFI", "SSID")
+					&& !lengthCheck(sc_pw, 64, "CMD SET WIFI", "Password")) {
+					return;
+				}
+
+				WiFiNetwork::setWiFiCredentials(sc_ssid, sc_pw);
+				logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting");
+			}
+		} else if (parser->equalCmdParam(1, "BWIFI")) {
+			if (parser->getParamCount() < 3) {
+				logger.error("CMD SET BWIFI ERROR: Too few arguments");
+				logger.info("Syntax: SET BWIFI <B64SSID> <B64PASSWORD>");
+			} else {
+				const char* b64ssid = parser->getCmdParam(2);
+				const char* b64pass = parser->getCmdParam(3);
+				unsigned int b64ssidlength = 0;
+				unsigned int b64passlength = 0;
+				unsigned int ssidlength
+					= decode_base64_length_null(b64ssid, &b64ssidlength);
+				unsigned int passlength
+					= decode_base64_length_null(b64pass, &b64passlength);
+
+				// alloc the strings and set them to 0 (null terminating)
+				char ssid[ssidlength + 1];
+				memset(ssid, 0, ssidlength + 1);
+				char pass[passlength + 1];
+				memset(pass, 0, passlength + 1);
+				// make a pointer to pass
+				char* ppass = pass;
+				decode_base64(
+					(const unsigned char*)b64ssid,
+					b64ssidlength,
+					(unsigned char*)ssid
+				);
+				if (!lengthCheck(ssid, 32, "CMD SET BWIFI", "SSID")) {
+					return;
+				}
+
+				if ((b64pass != NULL) && (b64passlength > 0)) {
+					decode_base64(
+						(const unsigned char*)b64pass,
+						b64passlength,
+						(unsigned char*)pass
+					);
+					if (!lengthCheck(pass, 64, "CMD SET BWIFI", "Password")) {
+						return;
+					}
+				} else {
+					// set the pointer for pass to null for no password
+					ppass = NULL;
+				}
+				WiFiNetwork::setWiFiCredentials(ssid, ppass);
+				logger.info("CMD SET BWIFI OK: New wifi credentials set, reconnecting");
 			}
 		} else {
-			logger.error("CMD SET ERROR: No variable to set");
+			logger.error("CMD SET ERROR: Unrecognized variable to set");
+		}
+	} else {
+		logger.error("CMD SET ERROR: No variable to set");
+	}
+}
+
+void printState() {
+	logger.info(
+		"SlimeVR Tracker, board: %d, hardware: %d, protocol: %d, firmware: %s, "
+		"address: %s, mac: %s, status: %d, wifi state: %d",
+		BOARD,
+		HARDWARE_MCU,
+		PROTOCOL_VERSION,
+		FIRMWARE_VERSION,
+		WiFiNetwork::getAddress().toString().c_str(),
+		WiFi.macAddress().c_str(),
+		statusManager.getStatus(),
+		WiFiNetwork::getWiFiState()
+	);
+	for (auto& sensor : sensorManager.getSensors()) {
+		logger.info(
+			"Sensor[%d]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
+			sensor->getSensorId(),
+			getIMUNameByType(sensor->getSensorType()),
+			UNPACK_QUATERNION(sensor->getFusedRotation()),
+			sensor->isWorking() ? "true" : "false",
+			sensor->getHadData() ? "true" : "false"
+		);
+	}
+	logger.info(
+		"Battery voltage: %.3f, level: %.1f%%",
+		battery.getVoltage(),
+		battery.getLevel() * 100
+	);
+}
+
+void cmdGet(CmdParser* parser) {
+	if (parser->getParamCount() < 2) {
+		return;
+	}
+
+	if (parser->equalCmdParam(1, "INFO")) {
+		printState();
+
+		// We don't want to print this on every timed state output
+		logger.info("Git commit: %s", GIT_REV);
+	}
+
+	if (parser->equalCmdParam(1, "CONFIG")) {
+		String str
+			= "BOARD=%d\n"
+			  "IMU=%d\n"
+			  "SECOND_IMU=%d\n"
+			  "IMU_ROTATION=%f\n"
+			  "SECOND_IMU_ROTATION=%f\n"
+			  "BATTERY_MONITOR=%d\n"
+			  "BATTERY_SHIELD_RESISTANCE=%d\n"
+			  "BATTERY_SHIELD_R1=%d\n"
+			  "BATTERY_SHIELD_R2=%d\n"
+			  "PIN_IMU_SDA=%d\n"
+			  "PIN_IMU_SCL=%d\n"
+			  "PIN_IMU_INT=%d\n"
+			  "PIN_IMU_INT_2=%d\n"
+			  "PIN_BATTERY_LEVEL=%d\n"
+			  "LED_PIN=%d\n"
+			  "LED_INVERTED=%d\n";
+
+		Serial.printf(
+			str.c_str(),
+			BOARD,
+			static_cast<int>(sensorManager.getSensorType(0)),
+			static_cast<int>(sensorManager.getSensorType(1)),
+			IMU_ROTATION,
+			SECOND_IMU_ROTATION,
+			BATTERY_MONITOR,
+			BATTERY_SHIELD_RESISTANCE,
+			BATTERY_SHIELD_R1,
+			BATTERY_SHIELD_R2,
+			PIN_IMU_SDA,
+			PIN_IMU_SCL,
+			PIN_IMU_INT,
+			PIN_IMU_INT_2,
+			PIN_BATTERY_LEVEL,
+			LED_PIN,
+			LED_INVERTED
+		);
+	}
+
+	if (parser->equalCmdParam(1, "TEST")) {
+		logger.info(
+			"[TEST] Board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, "
+			"mac: %s, status: %d, wifi state: %d",
+			BOARD,
+			HARDWARE_MCU,
+			PROTOCOL_VERSION,
+			FIRMWARE_VERSION,
+			WiFiNetwork::getAddress().toString().c_str(),
+			WiFi.macAddress().c_str(),
+			statusManager.getStatus(),
+			WiFiNetwork::getWiFiState()
+		);
+		auto& sensor0 = sensorManager.getSensors()[0];
+		sensor0->motionLoop();
+		logger.info(
+			"[TEST] Sensor[0]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
+			getIMUNameByType(sensor0->getSensorType()),
+			UNPACK_QUATERNION(sensor0->getFusedRotation()),
+			sensor0->isWorking() ? "true" : "false",
+			sensor0->getHadData() ? "true" : "false"
+		);
+		if (!sensor0->getHadData()) {
+			logger.error("[TEST] Sensor[0] didn't send any data yet!");
+		} else {
+			logger.info("[TEST] Sensor[0] sent some data, looks working.");
 		}
 	}
 
-    void printState() {
+	if (parser->equalCmdParam(1, "WIFISCAN")) {
-        logger.info(
+		logger.info("[WSCAN] Scanning for WiFi networks...");
-            "SlimeVR Tracker, board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
-            BOARD,
-            HARDWARE_MCU,
-            PROTOCOL_VERSION,
-            FIRMWARE_VERSION,
-            WiFiNetwork::getAddress().toString().c_str(),
-            WiFi.macAddress().c_str(),
-            statusManager.getStatus(),
-            WiFiNetwork::getWiFiState()
-        );
-        for (auto &sensor : sensorManager.getSensors()) {
-            logger.info(
-                "Sensor[%d]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
-                sensor->getSensorId(),
-                getIMUNameByType(sensor->getSensorType()),
-                UNPACK_QUATERNION(sensor->getFusedRotation()),
-                sensor->isWorking() ? "true" : "false",
-                sensor->getHadData() ? "true" : "false"
-            );
-        }
-        logger.info(
-            "Battery voltage: %.3f, level: %.1f%%",
-            battery.getVoltage(),
-			battery.getLevel() * 100
-        );
-    }
 
-    void cmdGet(CmdParser * parser) {
+		// Scan would fail if connecting, stop connecting before scan
-        if (parser->getParamCount() < 2) {
+		if (WiFi.status() != WL_CONNECTED) {
-            return;
+			WiFi.disconnect();
-        }
+		}
+		if (WiFiNetwork::isProvisioning()) {
+			WiFiNetwork::stopProvisioning();
+		}
 
-        if (parser->equalCmdParam(1, "INFO")) {
+		WiFi.scanNetworks();
-            printState();
 
-            // We don't want to print this on every timed state output
+		int scanRes = WiFi.scanComplete();
-            logger.info("Git commit: %s", GIT_REV);
+		if (scanRes >= 0) {
-        }
+			logger.info("[WSCAN] Found %d networks:", scanRes);
-
+			for (int i = 0; i < scanRes; i++) {
-        if (parser->equalCmdParam(1, "CONFIG")) {
+				logger.info(
-            String str =
+					"[WSCAN] %d:\t%02d\t%s\t(%d)\t%s",
-                "BOARD=%d\n"
+					i,
-                "IMU=%d\n"
+					WiFi.SSID(i).length(),
-                "SECOND_IMU=%d\n"
+					WiFi.SSID(i).c_str(),
-                "IMU_ROTATION=%f\n"
+					WiFi.RSSI(i),
-                "SECOND_IMU_ROTATION=%f\n"
+					((WiFi.encryptionType(i) == 0) ? "OPEN" : "PASSWD")
-                "BATTERY_MONITOR=%d\n"
+				);
-                "BATTERY_SHIELD_RESISTANCE=%d\n"
-                "BATTERY_SHIELD_R1=%d\n"
-                "BATTERY_SHIELD_R2=%d\n"
-                "PIN_IMU_SDA=%d\n"
-                "PIN_IMU_SCL=%d\n"
-                "PIN_IMU_INT=%d\n"
-                "PIN_IMU_INT_2=%d\n"
-                "PIN_BATTERY_LEVEL=%d\n"
-                "LED_PIN=%d\n"
-                "LED_INVERTED=%d\n";
-
-            Serial.printf(
-                str.c_str(),
-                BOARD,
-                static_cast<int>(sensorManager.getSensorType(0)),
-                static_cast<int>(sensorManager.getSensorType(1)),
-                IMU_ROTATION,
-                SECOND_IMU_ROTATION,
-                BATTERY_MONITOR,
-                BATTERY_SHIELD_RESISTANCE,
-                BATTERY_SHIELD_R1,
-                BATTERY_SHIELD_R2,
-                PIN_IMU_SDA,
-                PIN_IMU_SCL,
-                PIN_IMU_INT,
-                PIN_IMU_INT_2,
-                PIN_BATTERY_LEVEL,
-                LED_PIN,
-                LED_INVERTED
-            );
-        }
-
-        if (parser->equalCmdParam(1, "TEST")) {
-            logger.info(
-                "[TEST] Board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
-                BOARD,
-                HARDWARE_MCU,
-                PROTOCOL_VERSION,
-                FIRMWARE_VERSION,
-                WiFiNetwork::getAddress().toString().c_str(),
-                WiFi.macAddress().c_str(),
-                statusManager.getStatus(),
-                WiFiNetwork::getWiFiState()
-            );
-            auto& sensor0 = sensorManager.getSensors()[0];
-            sensor0->motionLoop();
-            logger.info(
-                "[TEST] Sensor[0]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
-                getIMUNameByType(sensor0->getSensorType()),
-                UNPACK_QUATERNION(sensor0->getFusedRotation()),
-                sensor0->isWorking() ? "true" : "false",
-                sensor0->getHadData() ? "true" : "false"
-            );
-            if(!sensor0->getHadData()) {
-                logger.error("[TEST] Sensor[0] didn't send any data yet!");
-            } else {
-                logger.info("[TEST] Sensor[0] sent some data, looks working.");
-            }
-        }
-
-        if (parser->equalCmdParam(1, "WIFISCAN")) {
-			logger.info("[WSCAN] Scanning for WiFi networks...");
-
-			// Scan would fail if connecting, stop connecting before scan
-			if (WiFi.status() != WL_CONNECTED) {
-				WiFi.disconnect();
-			}
-			if (WiFiNetwork::isProvisioning()) {
-				WiFiNetwork::stopProvisioning();
 			}
+			WiFi.scanDelete();
+		} else {
+			logger.info("[WSCAN] Scan failed!");
+		}
 
-			WiFi.scanNetworks();
+		// Restore conencting state
-
+		if (WiFi.status() != WL_CONNECTED) {
-			int scanRes = WiFi.scanComplete();
+			WiFi.begin();
-			if (scanRes >= 0) {
+		}
-				logger.info("[WSCAN] Found %d networks:", scanRes);
+	}
-				for (int i = 0; i < scanRes; i++) {
-					logger.info("[WSCAN] %d:\t%02d\t%s\t(%d)\t%s",
-						i, WiFi.SSID(i).length(), WiFi.SSID(i).c_str(), WiFi.RSSI(i),
-						((WiFi.encryptionType(i) == 0) ? "OPEN" : "PASSWD")
-					);
-				}
-				WiFi.scanDelete();
-			} else {
-				logger.info("[WSCAN] Scan failed!");
-			}
-
-			// Restore conencting state
-			if (WiFi.status() != WL_CONNECTED) {
-				WiFi.begin();
-			}
-        }
-    }
-
-    void cmdReboot(CmdParser * parser) {
-        logger.info("REBOOT");
-        ESP.restart();
-    }
-
-    void cmdFactoryReset(CmdParser * parser) {
-        logger.info("FACTORY RESET");
-
-        configuration.reset();
-
-        WiFi.disconnect(true); // Clear WiFi credentials
-        #if ESP8266
-            ESP.eraseConfig(); // Clear ESP config
-        #elif ESP32
-            nvs_flash_erase();
-        #else
-            #warning SERIAL COMMAND FACTORY RESET NOT SUPPORTED
-            logger.info("FACTORY RESET NOT SUPPORTED");
-            return;
-        #endif
-
-        #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
-            #warning FACTORY RESET does not clear your hardcoded WiFi credentials!
-            logger.warn("FACTORY RESET does not clear your hardcoded WiFi credentials!");
-        #endif
-
-        delay(3000);
-        ESP.restart();
-    }
-
-    void cmdTemperatureCalibration(CmdParser* parser) {
-        if (parser->getParamCount() > 1) {
-            if (parser->equalCmdParam(1, "PRINT")) {
-                for (auto &sensor : sensorManager.getSensors()) {
-                    sensor->printTemperatureCalibrationState();
-                }
-                return;
-            } else if (parser->equalCmdParam(1, "DEBUG")) {
-                for (auto &sensor : sensorManager.getSensors()) {
-                    sensor->printDebugTemperatureCalibrationState();
-                }
-                return;
-            } else if (parser->equalCmdParam(1, "RESET")) {
-                for (auto &sensor : sensorManager.getSensors()) {
-                    sensor->resetTemperatureCalibrationState();
-                }
-                return;
-            } else if (parser->equalCmdParam(1, "SAVE")) {
-                for (auto &sensor : sensorManager.getSensors()) {
-                    sensor->saveTemperatureCalibration();
-                }
-                return;
-            }
-        }
-        logger.info("Usage:");
-        logger.info("  TCAL PRINT: print current temperature calibration config");
-        logger.info("  TCAL DEBUG: print debug values for the current temperature calibration profile");
-        logger.info("  TCAL RESET: reset current temperature calibration in RAM (does not delete already saved)");
-        logger.info("  TCAL SAVE: save current temperature calibration to persistent flash");
-        logger.info("Note:");
-        logger.info("  Temperature calibration config saves automatically when calibration percent is at 100%");
-    }
-
-    void setUp() {
-        cmdCallbacks.addCmd("SET", &cmdSet);
-        cmdCallbacks.addCmd("GET", &cmdGet);
-        cmdCallbacks.addCmd("FRST", &cmdFactoryReset);
-        cmdCallbacks.addCmd("REBOOT", &cmdReboot);
-        cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration);
-    }
-
-    void update() {
-        cmdCallbacks.updateCmdProcessing(&cmdParser, &cmdBuffer, &Serial);
-    }
 }
+
+void cmdReboot(CmdParser* parser) {
+	logger.info("REBOOT");
+	ESP.restart();
+}
+
+void cmdFactoryReset(CmdParser* parser) {
+	logger.info("FACTORY RESET");
+
+	configuration.reset();
+
+	WiFi.disconnect(true);  // Clear WiFi credentials
+#if ESP8266
+	ESP.eraseConfig();  // Clear ESP config
+#elif ESP32
+	nvs_flash_erase();
+#else
+#warning SERIAL COMMAND FACTORY RESET NOT SUPPORTED
+	logger.info("FACTORY RESET NOT SUPPORTED");
+	return;
+#endif
+
+#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
+#warning FACTORY RESET does not clear your hardcoded WiFi credentials!
+	logger.warn("FACTORY RESET does not clear your hardcoded WiFi credentials!");
+#endif
+
+	delay(3000);
+	ESP.restart();
+}
+
+void cmdTemperatureCalibration(CmdParser* parser) {
+	if (parser->getParamCount() > 1) {
+		if (parser->equalCmdParam(1, "PRINT")) {
+			for (auto& sensor : sensorManager.getSensors()) {
+				sensor->printTemperatureCalibrationState();
+			}
+			return;
+		} else if (parser->equalCmdParam(1, "DEBUG")) {
+			for (auto& sensor : sensorManager.getSensors()) {
+				sensor->printDebugTemperatureCalibrationState();
+			}
+			return;
+		} else if (parser->equalCmdParam(1, "RESET")) {
+			for (auto& sensor : sensorManager.getSensors()) {
+				sensor->resetTemperatureCalibrationState();
+			}
+			return;
+		} else if (parser->equalCmdParam(1, "SAVE")) {
+			for (auto& sensor : sensorManager.getSensors()) {
+				sensor->saveTemperatureCalibration();
+			}
+			return;
+		}
+	}
+	logger.info("Usage:");
+	logger.info("  TCAL PRINT: print current temperature calibration config");
+	logger.info(
+		"  TCAL DEBUG: print debug values for the current temperature calibration "
+		"profile"
+	);
+	logger.info(
+		"  TCAL RESET: reset current temperature calibration in RAM (does not delete "
+		"already saved)"
+	);
+	logger.info("  TCAL SAVE: save current temperature calibration to persistent flash"
+	);
+	logger.info("Note:");
+	logger.info(
+		"  Temperature calibration config saves automatically when calibration percent "
+		"is at 100%"
+	);
+}
+
+void setUp() {
+	cmdCallbacks.addCmd("SET", &cmdSet);
+	cmdCallbacks.addCmd("GET", &cmdGet);
+	cmdCallbacks.addCmd("FRST", &cmdFactoryReset);
+	cmdCallbacks.addCmd("REBOOT", &cmdReboot);
+	cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration);
+}
+
+void update() { cmdCallbacks.updateCmdProcessing(&cmdParser, &cmdBuffer, &Serial); }
+}  // namespace SerialCommands

src/serial/serialcommands.h

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

src/status/Status.cpp

@@ -1,26 +1,22 @@
 #include "Status.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Status {
-    namespace Status
+const char* statusToString(Status status) {
-    {
+	switch (status) {
-        const char *statusToString(Status status)
+		case LOADING:
-        {
+			return "LOADING";
-            switch (status)
+		case LOW_BATTERY:
-            {
+			return "LOW_BATTERY";
-            case LOADING:
+		case IMU_ERROR:
-                return "LOADING";
+			return "IMU_ERROR";
-            case LOW_BATTERY:
+		case WIFI_CONNECTING:
-                return "LOW_BATTERY";
+			return "WIFI_CONNECTING";
-            case IMU_ERROR:
+		case SERVER_CONNECTING:
-                return "IMU_ERROR";
+			return "SERVER_CONNECTING";
-            case WIFI_CONNECTING:
+		default:
-                return "WIFI_CONNECTING";
+			return "UNKNOWN";
-            case SERVER_CONNECTING:
+	}
-                return "SERVER_CONNECTING";
-            default:
-                return "UNKNOWN";
-            }
-        }
-    }
 }
+}  // namespace Status
+}  // namespace SlimeVR

src/status/Status.h

@@ -1,21 +1,18 @@
 #ifndef STATUS_STATUS_H
 #define STATUS_STATUS_H
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Status {
-    namespace Status
+enum Status {
-    {
+	LOADING = 1 << 0,
-        enum Status
+	LOW_BATTERY = 1 << 1,
-        {
+	IMU_ERROR = 1 << 2,
-            LOADING = 1 << 0,
+	WIFI_CONNECTING = 1 << 3,
-            LOW_BATTERY = 1 << 1,
+	SERVER_CONNECTING = 1 << 4
-            IMU_ERROR = 1 << 2,
+};
-            WIFI_CONNECTING = 1 << 3,
-            SERVER_CONNECTING = 1 << 4
-        };
 
-        const char *statusToString(Status status);
+const char* statusToString(Status status);
-    }
+}  // namespace Status
-}
+}  // namespace SlimeVR
 
 #endif

src/status/StatusManager.cpp

@@ -1,38 +1,27 @@
 #include "StatusManager.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Status {
-    namespace Status
+void StatusManager::setStatus(Status status, bool value) {
-    {
+	if (value) {
-        void StatusManager::setStatus(Status status, bool value)
+		if (m_Status & status) {
-        {
+			return;
-            if (value)
+		}
-            {
-                if (m_Status & status)
-                {
-                    return;
-                }
 
-                m_Logger.trace("Added status %s", statusToString(status));
+		m_Logger.trace("Added status %s", statusToString(status));
 
-                m_Status |= status;
+		m_Status |= status;
-            }
+	} else {
-            else
+		if (!(m_Status & status)) {
-            {
+			return;
-                if (!(m_Status & status))
+		}
-                {
-                    return;
-                }
 
-                m_Logger.trace("Removed status %s", statusToString(status));
+		m_Logger.trace("Removed status %s", statusToString(status));
 
-                m_Status &= ~status;
+		m_Status &= ~status;
-            }
+	}
-        }
-
-        bool StatusManager::hasStatus(Status status)
-        {
-            return (m_Status & status) == status;
-        }
-    }
 }
+
+bool StatusManager::hasStatus(Status status) { return (m_Status & status) == status; }
+}  // namespace Status
+}  // namespace SlimeVR

src/status/StatusManager.h

@@ -4,25 +4,20 @@
 #include "Status.h"
 #include "logging/Logger.h"
 
-namespace SlimeVR
+namespace SlimeVR {
-{
+namespace Status {
-    namespace Status
+class StatusManager {
-    {
+public:
-        class StatusManager
+	void setStatus(Status status, bool value);
-        {
+	bool hasStatus(Status status);
-        public:
+	uint32_t getStatus() { return m_Status; };
-            void setStatus(Status status, bool value);
-            bool hasStatus(Status status);
-            uint32_t getStatus() {
-                return m_Status;
-            };
 
-        private:
+private:
-            uint32_t m_Status;
+	uint32_t m_Status;
 
-            Logging::Logger m_Logger = Logging::Logger("StatusManager");
+	Logging::Logger m_Logger = Logging::Logger("StatusManager");
-        };
+};
-    }
+}  // namespace Status
-}
+}  // namespace SlimeVR
 
 #endif

src/utils.h

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

test/i2cscan.cpp

@@ -1,119 +1,124 @@
-/* 
+/*
-* i2c_port_address_scanner
+ * i2c_port_address_scanner
-* Scans ports D0 to D7 on an ESP8266 and searches for I2C device. based on the original code
+ * Scans ports D0 to D7 on an ESP8266 and searches for I2C device. based on the original
-* available on Arduino.cc and later improved by user Krodal and Nick Gammon (www.gammon.com.au/forum/?id=10896)
+ * code available on Arduino.cc and later improved by user Krodal and Nick Gammon
-* D8 throws exceptions thus it has been left out
+ * (www.gammon.com.au/forum/?id=10896) D8 throws exceptions thus it has been left out
-*
+ *
-*/
+ */
 #include <Arduino.h>
 #include <Wire.h>
 
-#include "ota.h"
 #include "configuration/Configuration.h"
-#include "helper_3dmath.h"
-#include "udpclient.h"
 #include "credentials.h"
+#include "helper_3dmath.h"
+#include "ota.h"
+#include "udpclient.h"
 
 SlimeVR::Configuration::DeviceConfig config;
 
 uint8_t portArray[] = {16, 5, 4, 0, 2, 14, 12, 13};
-//String portMap[] = {"D0", "D1", "D2", "D3", "D4", "D5", "D6", "D7"}; //for Wemos
+// String portMap[] = {"D0", "D1", "D2", "D3", "D4", "D5", "D6", "D7"}; //for Wemos
-String portMap[] = {"GPIO16", "GPIO5", "GPIO4", "GPIO0", "GPIO2", "GPIO14", "GPIO12", "GPIO13"};
+String portMap[]
-
+	= {"GPIO16", "GPIO5", "GPIO4", "GPIO0", "GPIO2", "GPIO14", "GPIO12", "GPIO13"};
 
 void check_if_exist_I2C() {
-  byte error, address;
+	byte error, address;
-  int nDevices;
+	int nDevices;
-  nDevices = 0;
+	nDevices = 0;
-  for (address = 1; address < 127; address++ )  {
+	for (address = 1; address < 127; address++) {
-    // The i2c_scanner uses the return value of
+		// The i2c_scanner uses the return value of
-    // the Write.endTransmisstion to see if
+		// the Write.endTransmisstion to see if
-    // a device did acknowledge to the address.
+		// a device did acknowledge to the address.
-    Wire.beginTransmission(address);
+		Wire.beginTransmission(address);
-    error = Wire.endTransmission();
+		error = Wire.endTransmission();
 
-    if (error == 0){
+		if (error == 0) {
-      Serial.print("I2C device found at address 0x");
+			Serial.print("I2C device found at address 0x");
-      if (address < 16)
+			if (address < 16) {
-        Serial.print("0");
+				Serial.print("0");
-      Serial.print(address, HEX);
+			}
-      Serial.println("  !");
+			Serial.print(address, HEX);
+			Serial.println("  !");
 
-      nDevices++;
+			nDevices++;
-    } else if (error == 4) {
+		} else if (error == 4) {
-      Serial.print("Unknow error at address 0x");
+			Serial.print("Unknow error at address 0x");
-      if (address < 16)
+			if (address < 16) {
-        Serial.print("0");
+				Serial.print("0");
-      Serial.println(address, HEX);
+			}
-    }
+			Serial.println(address, HEX);
-  } //for loop
+		}
-  if (nDevices == 0)
+	}  // for loop
-    Serial.println("No I2C devices found");
+	if (nDevices == 0) {
-  else
+		Serial.println("No I2C devices found");
-    Serial.println("Scan finished\n");
+	} else {
+		Serial.println("Scan finished\n");
+	}
 }
 
-void scanPorts() { 
+void scanPorts() {
-  for (uint8_t i = 0; i < sizeof(portArray); i++) {
+	for (uint8_t i = 0; i < sizeof(portArray); i++) {
-    for (uint8_t j = 0; j < sizeof(portArray); j++) {
+		for (uint8_t j = 0; j < sizeof(portArray); j++) {
-      if (i != j){
+			if (i != j) {
-        Serial.print("Scanning (SDA : SCL) - " + portMap[i] + " : " + portMap[j] + " - ");
+				Serial.print(
-        Wire.begin(portArray[i], portArray[j]);
+					"Scanning (SDA : SCL) - " + portMap[i] + " : " + portMap[j] + " - "
-        check_if_exist_I2C();
+				);
-      }
+				Wire.begin(portArray[i], portArray[j]);
-    }
+				check_if_exist_I2C();
-  }
+			}
+		}
+	}
 }
 
 void I2C_recovery() {
- Serial.println("Starting I2C bus recovery");
+	Serial.println("Starting I2C bus recovery");
-  delay(2000);
+	delay(2000);
-  int SDAPIN = 70;
+	int SDAPIN = 70;
-  int CLKPIN = 71;
+	int CLKPIN = 71;
-  //try i2c bus recovery at 100kHz = 5uS high, 5uS low
+	// try i2c bus recovery at 100kHz = 5uS high, 5uS low
-  pinMode(SDAPIN, OUTPUT);//keeping SDA high during recovery
+	pinMode(SDAPIN, OUTPUT);  // keeping SDA high during recovery
-  digitalWrite(SDAPIN, HIGH);
+	digitalWrite(SDAPIN, HIGH);
-  pinMode(CLKPIN, OUTPUT);
+	pinMode(CLKPIN, OUTPUT);
-  for (int i = 0; i < 10; i++) { //9nth cycle acts as NACK
+	for (int i = 0; i < 10; i++) {  // 9nth cycle acts as NACK
-    digitalWrite(CLKPIN, HIGH);
+		digitalWrite(CLKPIN, HIGH);
-    delayMicroseconds(5);
+		delayMicroseconds(5);
-    digitalWrite(CLKPIN, LOW);
+		digitalWrite(CLKPIN, LOW);
-    delayMicroseconds(5);
+		delayMicroseconds(5);
-  }
+	}
 
-  //a STOP signal (SDA from low to high while CLK is high)
+	// a STOP signal (SDA from low to high while CLK is high)
-  digitalWrite(SDAPIN, LOW);
+	digitalWrite(SDAPIN, LOW);
-  delayMicroseconds(5);
+	delayMicroseconds(5);
-  digitalWrite(CLKPIN, HIGH);
+	digitalWrite(CLKPIN, HIGH);
-  delayMicroseconds(2);
+	delayMicroseconds(2);
-  digitalWrite(SDAPIN, HIGH);
+	digitalWrite(SDAPIN, HIGH);
-  delayMicroseconds(2);
+	delayMicroseconds(2);
-  //bus status is now : FREE
+	// bus status is now : FREE
 
-  Serial.println("bus recovery done, starting scan in 2 secs");
+	Serial.println("bus recovery done, starting scan in 2 secs");
-  //return to power up mode
+	// return to power up mode
-  pinMode(SDAPIN, INPUT);
+	pinMode(SDAPIN, INPUT);
-  pinMode(CLKPIN, INPUT);
+	pinMode(CLKPIN, INPUT);
-  delay(2000);
+	delay(2000);
-  //pins + begin advised in https://github.com/esp8266/Arduino/issues/452
+	// pins + begin advised in https://github.com/esp8266/Arduino/issues/452
-  //Wire1.pins(SDAPIN, CLKPIN); //this changes default values for sda and clock as well
+	// Wire1.pins(SDAPIN, CLKPIN); //this changes default values for sda and clock as
-  Wire.begin();
+	// well
-  //only pins: no signal on clk and sda
+	Wire.begin();
-  //only begin: no signal on clk, no signal on sda
+	// only pins: no signal on clk and sda
+	// only begin: no signal on clk, no signal on sda
 }
 
-void loop() {
+void loop() { otaUpdate(); }
-  otaUpdate();
-}
 
 void setup() {
-  Serial.begin(115200);
+	Serial.begin(115200);
-  while (!Serial);             // Leonardo: wait for serial monitor
+	while (!Serial)
-  Serial.println("\n\nI2C Scanner to scan for devices on each port pair D0 to D7");
+		;  // Leonardo: wait for serial monitor
-  scanPorts();
+	Serial.println("\n\nI2C Scanner to scan for devices on each port pair D0 to D7");
-  I2C_recovery();
+	scanPorts();
-  scanPorts();
+	I2C_recovery();
+	scanPorts();
 
-  setUpWiFi(&config);
+	setUpWiFi(&config);
-  otaSetup(otaPassword);
+	otaSetup(otaPassword);
 }