diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 000000000..0e2493a4f --- /dev/null +++ b/.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/).)*$' diff --git a/.github/workflows/actions.yml b/.github/workflows/actions.yml index d2714f081..ae247f3ee 100644 --- a/.github/workflows/actions.yml +++ b/.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 diff --git a/src/GlobalVars.h b/src/GlobalVars.h index 66bab0a8c..1d2f191d3 100644 --- a/src/GlobalVars.h +++ b/src/GlobalVars.h @@ -27,12 +27,12 @@ #include #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; diff --git a/src/LEDManager.cpp b/src/LEDManager.cpp index f184d55b3..51ef864f1 100644 --- a/src/LEDManager.cpp +++ b/src/LEDManager.cpp @@ -1,212 +1,183 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "LEDManager.h" + #include "GlobalVars.h" #include "status/Status.h" -namespace SlimeVR -{ - void LEDManager::setup() - { +namespace SlimeVR { +void LEDManager::setup() { #if ENABLE_LEDS - pinMode(m_Pin, OUTPUT); + pinMode(m_Pin, OUTPUT); #endif - // Do the initial pull of the state - update(); - } + // Do the initial pull of the state + update(); +} - void LEDManager::on() - { +void LEDManager::on() { #if ENABLE_LEDS - digitalWrite(m_Pin, LED__ON); + digitalWrite(m_Pin, LED__ON); #endif - } +} - void LEDManager::off() - { +void LEDManager::off() { #if ENABLE_LEDS - digitalWrite(m_Pin, LED__OFF); + 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 - { +} + +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; - } + 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; + 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; - } - } + } + + 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 diff --git a/src/LEDManager.h b/src/LEDManager.h index 696caa746..1d30cb2bd 100644 --- a/src/LEDManager.h +++ b/src/LEDManager.h @@ -1,29 +1,30 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_LEDMANAGER_H #define SLIMEVR_LEDMANAGER_H #include + #include "globals.h" #include "logging/Logger.h" @@ -45,59 +46,52 @@ #define SERVER_CONNECTING_INTERVAL 3000 #define SERVER_CONNECTING_COUNT 2 -namespace SlimeVR -{ - enum LEDStage - { - OFF, - ON, - GAP, - INTERVAL - }; - - class LEDManager - { - public: - LEDManager(uint8_t pin) : m_Pin(pin) {} - - void setup(); - - /*! - * @brief Turns the LED on - */ - void on(); - - /*! - * @brief Turns the LED off - */ - void off(); - - /*! - * @brief Blink the LED for [time]ms. *Can* cause lag - * @param time Amount of ms to turn the LED on - */ - void blink(unsigned long time); - - /*! - * @brief Show a pattern on the LED. *Can* cause lag - * @param timeon Amount of ms to turn the LED on - * @param timeoff Amount of ms to turn the LED off - * @param times Amount of times to display the pattern - */ - void pattern(unsigned long timeon, unsigned long timeoff, int times); - - void update(); - - private: - uint8_t m_CurrentCount = 0; - unsigned long m_Timer = 0; - LEDStage m_CurrentStage = OFF; - unsigned long m_LastUpdate = millis(); - - uint8_t m_Pin; - - Logging::Logger m_Logger = Logging::Logger("LEDManager"); - }; -} +namespace SlimeVR { +enum LEDStage { OFF, ON, GAP, INTERVAL }; + +class LEDManager { +public: + LEDManager(uint8_t pin) + : m_Pin(pin) {} + + void setup(); + + /*! + * @brief Turns the LED on + */ + void on(); + + /*! + * @brief Turns the LED off + */ + void off(); + + /*! + * @brief Blink the LED for [time]ms. *Can* cause lag + * @param time Amount of ms to turn the LED on + */ + void blink(unsigned long time); + + /*! + * @brief Show a pattern on the LED. *Can* cause lag + * @param timeon Amount of ms to turn the LED on + * @param timeoff Amount of ms to turn the LED off + * @param times Amount of times to display the pattern + */ + void pattern(unsigned long timeon, unsigned long timeoff, int times); + + void update(); + +private: + uint8_t m_CurrentCount = 0; + unsigned long m_Timer = 0; + LEDStage m_CurrentStage = OFF; + unsigned long m_LastUpdate = millis(); + + uint8_t m_Pin; + + Logging::Logger m_Logger = Logging::Logger("LEDManager"); +}; +} // namespace SlimeVR #endif diff --git a/src/batterymonitor.cpp b/src/batterymonitor.cpp index 45066a2cc..f6dcdddfe 100644 --- a/src/batterymonitor.cpp +++ b/src/batterymonitor.cpp @@ -1,138 +1,132 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "batterymonitor.h" + #include "GlobalVars.h" -#if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021) +#if ESP8266 \ + && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021) ADC_MODE(ADC_VCC); #endif -void BatteryMonitor::Setup() -{ +void BatteryMonitor::Setup() { #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 - for (uint8_t i = 0x48; i < 0x4F; i++) - { - if (I2CSCAN::hasDevOnBus(i)) - { - address = i; - break; - } - } - if (address == 0) - { - m_Logger.error("MCP3021 not found on I2C bus"); - } + for (uint8_t i = 0x48; i < 0x4F; i++) { + if (I2CSCAN::hasDevOnBus(i)) { + address = i; + break; + } + } + if (address == 0) { + m_Logger.error("MCP3021 not found on I2C bus"); + } #endif } -void BatteryMonitor::Loop() -{ - #if BATTERY_MONITOR == BAT_EXTERNAL || BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 - auto now_ms = millis(); - if (now_ms - last_battery_sample >= batterySampleRate) - { - last_battery_sample = now_ms; - voltage = -1; - #if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021) - // Find out what your max measurement is (voltage_3_3). - // Take the max measurement and check if it was less than 50mV - // if yes output 5.0V - // if no output 3.3V - dropvoltage + 0.1V - auto ESPmV = ESP.getVcc(); - if (ESPmV > voltage_3_3) - { - voltage_3_3 = ESPmV; - } - else - { - //Calculate drop in mV - ESPmV = voltage_3_3 - ESPmV; - if (ESPmV < 50) - { - voltage = 5.0F; - } - else - { - voltage = 3.3F - ((float)ESPmV / 1000.0F) + 0.1F; //we assume 100mV drop on the linear converter - } - } - #endif - #if ESP8266 && BATTERY_MONITOR == BAT_EXTERNAL - voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * ADCVoltageMax / ADCResolution * ADCMultiplier; - #endif - #if ESP32 && BATTERY_MONITOR == BAT_EXTERNAL - voltage = ((float)analogReadMilliVolts(PIN_BATTERY_LEVEL)) / 1000 * ADCMultiplier; - #endif - #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 - if (address > 0) - { - Wire.beginTransmission(address); - Wire.requestFrom(address, (uint8_t)2); - auto MSB = Wire.read(); - auto LSB = Wire.read(); - auto status = Wire.endTransmission(); - if (status == 0) - { - float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2)); - v *= ADCMultiplier; - voltage = (voltage > 0) ? min(voltage, v) : v; - } - } - #endif - if (voltage > 0) //valid measurement - { - // Estimate battery level, 3.2V is 0%, 4.17V is 100% (1.0) - if (voltage > 3.975f) - level = (voltage - 2.920f) * 0.8f; - else if (voltage > 3.678f) - level = (voltage - 3.300f) * 1.25f; - else if (voltage > 3.489f) - level = (voltage - 3.400f) * 1.7f; - else if (voltage > 3.360f) - level = (voltage - 3.300f) * 0.8f; - else - level = (voltage - 3.200f) * 0.3f; +void BatteryMonitor::Loop() { +#if BATTERY_MONITOR == BAT_EXTERNAL || BATTERY_MONITOR == BAT_INTERNAL \ + || BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 + auto now_ms = millis(); + if (now_ms - last_battery_sample >= batterySampleRate) { + last_battery_sample = now_ms; + voltage = -1; +#if ESP8266 \ + && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021) + // Find out what your max measurement is (voltage_3_3). + // Take the max measurement and check if it was less than 50mV + // if yes output 5.0V + // if no output 3.3V - dropvoltage + 0.1V + auto ESPmV = ESP.getVcc(); + if (ESPmV > voltage_3_3) { + voltage_3_3 = ESPmV; + } else { + // Calculate drop in mV + ESPmV = voltage_3_3 - ESPmV; + if (ESPmV < 50) { + voltage = 5.0F; + } else { + voltage = 3.3F - ((float)ESPmV / 1000.0F) + + 0.1F; // we assume 100mV drop on the linear converter + } + } +#endif +#if ESP8266 && BATTERY_MONITOR == BAT_EXTERNAL + voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * ADCVoltageMax / ADCResolution + * ADCMultiplier; +#endif +#if ESP32 && BATTERY_MONITOR == BAT_EXTERNAL + voltage + = ((float)analogReadMilliVolts(PIN_BATTERY_LEVEL)) / 1000 * ADCMultiplier; +#endif +#if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 + if (address > 0) { + Wire.beginTransmission(address); + Wire.requestFrom(address, (uint8_t)2); + auto MSB = Wire.read(); + auto LSB = Wire.read(); + auto status = Wire.endTransmission(); + if (status == 0) { + float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2)); + v *= ADCMultiplier; + voltage = (voltage > 0) ? min(voltage, v) : v; + } + } +#endif + if (voltage > 0) // valid measurement + { + // Estimate battery level, 3.2V is 0%, 4.17V is 100% (1.0) + if (voltage > 3.975f) { + level = (voltage - 2.920f) * 0.8f; + } else if (voltage > 3.678f) { + level = (voltage - 3.300f) * 1.25f; + } else if (voltage > 3.489f) { + level = (voltage - 3.400f) * 1.7f; + } else if (voltage > 3.360f) { + level = (voltage - 3.300f) * 0.8f; + } else { + level = (voltage - 3.200f) * 0.3f; + } - level = (level - 0.05f) / 0.95f; // Cut off the last 5% (3.36V) + level = (level - 0.05f) / 0.95f; // Cut off the last 5% (3.36V) - if (level > 1) - level = 1; - else if (level < 0) - level = 0; - networkConnection.sendBatteryLevel(voltage, level); - #ifdef BATTERY_LOW_POWER_VOLTAGE - if (voltage < BATTERY_LOW_POWER_VOLTAGE) - { - #if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP - ESP.deepSleep(0); - #else - statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true); - #endif - } else { - statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false); - } - #endif - } - } - #endif + if (level > 1) { + level = 1; + } else if (level < 0) { + level = 0; + } + networkConnection.sendBatteryLevel(voltage, level); +#ifdef BATTERY_LOW_POWER_VOLTAGE + if (voltage < BATTERY_LOW_POWER_VOLTAGE) { +#if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP + ESP.deepSleep(0); +#else + statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true); +#endif + } else { + statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false); + } +#endif + } + } +#endif } diff --git a/src/batterymonitor.h b/src/batterymonitor.h index 7ae8140f2..e6a38b06e 100644 --- a/src/batterymonitor.h +++ b/src/batterymonitor.h @@ -1,92 +1,98 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_BATTERYMONITOR_H_ #define SLIMEVR_BATTERYMONITOR_H_ #include -#include "globals.h" -#include #include +#include + +#include "globals.h" #include "logging/Logger.h" #if ESP8266 - #define ADCResolution 1023.0 // ESP8266 has 10bit ADC - #define ADCVoltageMax 1.0 // ESP8266 input is 1.0 V = 1023.0 +#define ADCResolution 1023.0 // ESP8266 has 10bit ADC +#define ADCVoltageMax 1.0 // ESP8266 input is 1.0 V = 1023.0 #endif #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 - #error Internal ADC enabled without pin! Please select a pin. - #endif - // Wemos D1 Mini has an internal Voltage Divider with R1=100K and R2=220K > this means, 3.3V analogRead input voltage results in 1023.0 - // Wemos D1 Mini with Wemos Battery Shield v1.2.0 or higher: Battery Shield with J2 closed, has an additional 130K resistor. So the resulting Voltage Divider is R1=220K+100K=320K and R2=100K > this means, 4.5V analogRead input voltage results in 1023.0 - // ESP32 Boards may have not the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can be changed. - // Diagramm: - // (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2] ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND) - // SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0 - #define ADCMultiplier (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) / BATTERY_SHIELD_R1 +#ifndef PIN_BATTERY_LEVEL +#error Internal ADC enabled without pin! Please select a pin. +#endif +// Wemos D1 Mini has an internal Voltage Divider with R1=100K and R2=220K > this +// means, 3.3V analogRead input voltage results in 1023.0 Wemos D1 Mini with Wemos +// Battery Shield v1.2.0 or higher: Battery Shield with J2 closed, has an additional +// 130K resistor. So the resulting Voltage Divider is R1=220K+100K=320K and R2=100K > +// this means, 4.5V analogRead input voltage results in 1023.0 ESP32 Boards may have not +// the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can +// be changed. Diagramm: +// (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2] +// ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND) +// SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0 +#define ADCMultiplier \ + (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) \ + / BATTERY_SHIELD_R1 #elif BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 - // Default recommended resistors are 9.1k and 5.1k - #define ADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1 +// Default recommended resistors are 9.1k and 5.1k +#define ADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1 #endif -class BatteryMonitor -{ +class BatteryMonitor { public: - void Setup(); - void Loop(); + void Setup(); + void Loop(); - float getVoltage() const { return voltage; } - float getLevel() const { return level; } + float getVoltage() const { return voltage; } + float getLevel() const { return level; } private: - unsigned long last_battery_sample = 0; + unsigned long last_battery_sample = 0; #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 - uint8_t address = 0; + uint8_t address = 0; #endif #if BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 - uint16_t voltage_3_3 = 3000; + uint16_t voltage_3_3 = 3000; #endif - float voltage = -1; - float level = -1; + float voltage = -1; + float level = -1; - SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor"); + SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor"); }; -#endif // SLIMEVR_BATTERYMONITOR_H_ +#endif // SLIMEVR_BATTERYMONITOR_H_ diff --git a/src/calibration.h b/src/calibration.h index f9c7ddea5..43b752eb2 100644 --- a/src/calibration.h +++ b/src/calibration.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_CALIBRATION_H_ #define SLIMEVR_CALIBRATION_H_ @@ -31,4 +31,4 @@ #define CALIBRATION_TYPE_EXTERNAL_ACCEL 6 #define CALIBRATION_TYPE_EXTERNAL_MAG 7 -#endif // SLIMEVR_CALIBRATION_H_ \ No newline at end of file +#endif // SLIMEVR_CALIBRATION_H_ \ No newline at end of file diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 482dd7204..c4ca47c1e 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -1,327 +1,384 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ +#include "Configuration.h" + #include -#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 { - void Configuration::setup() { - if (m_Loaded) { - return; - } - - bool status = LittleFS.begin(); - if (!status) { - this->m_Logger.warn("Could not mount LittleFS, formatting"); - - status = LittleFS.format(); - if (!status) { - this->m_Logger.warn("Could not format LittleFS, aborting"); - return; - } - - status = LittleFS.begin(); - if (!status) { - this->m_Logger.error("Could not mount LittleFS, aborting"); - return; - } - } - - if (LittleFS.exists("/config.bin")) { - m_Logger.trace("Found configuration file"); - - auto file = LittleFS.open("/config.bin", "r"); - - file.read((uint8_t*)&m_Config.version, sizeof(int32_t)); - - if (m_Config.version < CURRENT_CONFIGURATION_VERSION) { - m_Logger.debug("Configuration is outdated: v%d < v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION); - - if (!runMigrations(m_Config.version)) { - m_Logger.error("Failed to migrate configuration from v%d to v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION); - return; - } - } else { - m_Logger.info("Found up-to-date configuration v%d", m_Config.version); - } - - file.seek(0); - file.read((uint8_t*)&m_Config, sizeof(DeviceConfig)); - file.close(); - } else { - m_Logger.info("No configuration file found, creating new one"); - m_Config.version = CURRENT_CONFIGURATION_VERSION; - save(); - } - - loadSensors(); - - m_Loaded = true; - - m_Logger.info("Loaded configuration"); - -#ifdef DEBUG_CONFIGURATION - print(); -#endif - } +namespace Configuration { +void Configuration::setup() { + if (m_Loaded) { + return; + } + + bool status = LittleFS.begin(); + if (!status) { + this->m_Logger.warn("Could not mount LittleFS, formatting"); + + status = LittleFS.format(); + if (!status) { + this->m_Logger.warn("Could not format LittleFS, aborting"); + return; + } - void Configuration::save() { - for (size_t i = 0; i < m_Sensors.size(); i++) { - SensorConfig config = m_Sensors[i]; - if (config.type == SensorConfigType::NONE) { - continue; - } + status = LittleFS.begin(); + if (!status) { + this->m_Logger.error("Could not mount LittleFS, aborting"); + return; + } + } - char path[17]; - sprintf(path, DIR_CALIBRATIONS"/%d", i); + if (LittleFS.exists("/config.bin")) { + m_Logger.trace("Found configuration file"); - m_Logger.trace("Saving sensor config data for %d", i); + auto file = LittleFS.open("/config.bin", "r"); - File file = LittleFS.open(path, "w"); - file.write((uint8_t*)&config, sizeof(SensorConfig)); - file.close(); - } + file.read((uint8_t*)&m_Config.version, sizeof(int32_t)); - { - File file = LittleFS.open("/config.bin", "w"); - file.write((uint8_t*)&m_Config, sizeof(DeviceConfig)); - file.close(); - } + 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("Saved configuration"); - } + if (!runMigrations(m_Config.version)) { + m_Logger.error( + "Failed to migrate configuration from v%d to v%d", + m_Config.version, + CURRENT_CONFIGURATION_VERSION + ); + return; + } + } else { + m_Logger.info("Found up-to-date configuration v%d", m_Config.version); + } - void Configuration::reset() { - LittleFS.format(); + file.seek(0); + file.read((uint8_t*)&m_Config, sizeof(DeviceConfig)); + file.close(); + } else { + m_Logger.info("No configuration file found, creating new one"); + m_Config.version = CURRENT_CONFIGURATION_VERSION; + save(); + } - m_Sensors.clear(); - m_Config.version = 1; - save(); + loadSensors(); - m_Logger.debug("Reset configuration"); - } + m_Loaded = true; - int32_t Configuration::getVersion() const { - return m_Config.version; - } + m_Logger.info("Loaded configuration"); - size_t Configuration::getSensorCount() const { - return m_Sensors.size(); - } +#ifdef DEBUG_CONFIGURATION + print(); +#endif +} - SensorConfig Configuration::getSensor(size_t sensorID) const { - if (sensorID >= m_Sensors.size()) { - return {}; - } +void Configuration::save() { + for (size_t i = 0; i < m_Sensors.size(); i++) { + SensorConfig config = m_Sensors[i]; + if (config.type == SensorConfigType::NONE) { + continue; + } - return m_Sensors.at(sensorID); - } + char path[17]; + sprintf(path, DIR_CALIBRATIONS "/%d", i); - void Configuration::setSensor(size_t sensorID, const SensorConfig& config) { - size_t currentSensors = m_Sensors.size(); + m_Logger.trace("Saving sensor config data for %d", i); - if (sensorID >= currentSensors) { - m_Sensors.resize(sensorID + 1); - } + File file = LittleFS.open(path, "w"); + file.write((uint8_t*)&config, sizeof(SensorConfig)); + file.close(); + } - m_Sensors[sensorID] = config; - } + { + File file = LittleFS.open("/config.bin", "w"); + file.write((uint8_t*)&m_Config, sizeof(DeviceConfig)); + file.close(); + } - void Configuration::loadSensors() { - SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) { - SensorConfig sensorConfig; - f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig)); + m_Logger.debug("Saved configuration"); +} - uint8_t sensorId = strtoul(f.name(), nullptr, 10); - m_Logger.debug( - "Found sensor calibration for %s at index %d", - calibrationConfigTypeToString(sensorConfig.type), - sensorId - ); +void Configuration::reset() { + LittleFS.format(); - setSensor(sensorId, sensorConfig); - }); - } + m_Sensors.clear(); + m_Config.version = 1; + save(); - bool Configuration::loadTemperatureCalibration( - uint8_t sensorId, - GyroTemperatureCalibrationConfig& config - ) { - if (!SlimeVR::Utils::ensureDirectory(DIR_TEMPERATURE_CALIBRATIONS)) { - return false; - } + m_Logger.debug("Reset configuration"); +} - char path[32]; - sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId); +int32_t Configuration::getVersion() const { return m_Config.version; } - if (!LittleFS.exists(path)) { - return false; - } +size_t Configuration::getSensorCount() const { return m_Sensors.size(); } - auto f = SlimeVR::Utils::openFile(path, "r"); - if (f.isDirectory()) { - return false; - } +SensorConfig Configuration::getSensor(size_t sensorID) const { + if (sensorID >= m_Sensors.size()) { + return {}; + } - if (f.size() != sizeof(GyroTemperatureCalibrationConfig)) { - m_Logger.debug( - "Found incompatible sensor temperature calibration (size mismatch) " - "sensorId:%d, skipping", - sensorId - ); - return false; - } + return m_Sensors.at(sensorID); +} - SensorConfigType storedConfigType; - f.read((uint8_t*)&storedConfigType, sizeof(SensorConfigType)); +void Configuration::setSensor(size_t sensorID, const SensorConfig& config) { + size_t currentSensors = m_Sensors.size(); - 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; - } + if (sensorID >= currentSensors) { + m_Sensors.resize(sensorID + 1); + } - 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; - } + m_Sensors[sensorID] = config; +} - bool Configuration::saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config) { - if (config.type == SensorConfigType::NONE) { - return false; - } +void Configuration::loadSensors() { + SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) { + SensorConfig sensorConfig; + f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig)); - char path[32]; - sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId); + uint8_t sensorId = strtoul(f.name(), nullptr, 10); + m_Logger.debug( + "Found sensor calibration for %s at index %d", + calibrationConfigTypeToString(sensorConfig.type), + sensorId + ); - m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId); + setSensor(sensorId, sensorConfig); + }); +} - File file = LittleFS.open(path, "w"); - file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig)); - file.close(); +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; +} - m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId); - return true; - } +bool Configuration::saveTemperatureCalibration( + uint8_t sensorId, + const GyroTemperatureCalibrationConfig& config +) { + if (config.type == SensorConfigType::NONE) { + return false; + } - bool Configuration::runMigrations(int32_t version) { - return true; - } + char path[32]; + sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId); - void Configuration::print() { - m_Logger.info("Configuration:"); - m_Logger.info(" Version: %d", m_Config.version); - m_Logger.info(" %d Sensors:", m_Sensors.size()); + m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId); - 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)); + File file = LittleFS.open(path, "w"); + file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig)); + file.close(); - switch (c.type) { - case SensorConfigType::NONE: - break; + m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId); + return true; +} - case SensorConfigType::BMI160: - m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B)); +bool Configuration::runMigrations(int32_t version) { return true; } - 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])); - } +void Configuration::print() { + m_Logger.info("Configuration:"); + m_Logger.info(" Version: %d", m_Config.version); + m_Logger.info(" %d Sensors:", m_Sensors.size()); - 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); + 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)); - break; + switch (c.type) { + case SensorConfigType::NONE: + break; - case SensorConfigType::SFUSION: - m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B)); + 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.sfusion.A_Ainv[i])); - } + 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); - 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; + 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)); + case SensorConfigType::SFUSION: + m_Logger.info( + " A_B : %f, %f, %f", + UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B) + ); - break; + 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::MPU9250: - m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B)); + 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) + ); - 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])); - } + break; - m_Logger.info(" M_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B)); + case SensorConfigType::MPU9250: + m_Logger.info( + " A_B : %f, %f, %f", + UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_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(" 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(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off)); + 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; + 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)); + 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; + break; - case SensorConfigType::BNO0XX: - m_Logger.info(" magEnabled: %d", c.data.bno0XX.magEnabled); + case SensorConfigType::BNO0XX: + m_Logger.info(" magEnabled: %d", c.data.bno0XX.magEnabled); - break; - } - } - } - } + break; + } + } } +} // namespace Configuration +} // namespace SlimeVR diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h index c54ca0168..00cd4c78b 100644 --- a/src/configuration/Configuration.h +++ b/src/configuration/Configuration.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_CONFIGURATION_CONFIGURATION_H @@ -26,42 +26,48 @@ #include +#include "../motionprocessing/GyroTemperatureCalibrator.h" #include "DeviceConfig.h" #include "logging/Logger.h" -#include "../motionprocessing/GyroTemperatureCalibrator.h" namespace SlimeVR { - namespace Configuration { - class Configuration { - public: - void setup(); +namespace Configuration { +class Configuration { +public: + void setup(); - void save(); - void reset(); + void save(); + void reset(); - void print(); + void print(); - int32_t getVersion() const; + int32_t getVersion() const; - size_t getSensorCount() const; - SensorConfig getSensor(size_t sensorID) const; - void setSensor(size_t sensorID, const SensorConfig& config); + size_t getSensorCount() const; + SensorConfig getSensor(size_t sensorID) const; + void setSensor(size_t sensorID, const SensorConfig& config); - bool loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config); - bool saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config); + bool loadTemperatureCalibration( + uint8_t sensorId, + GyroTemperatureCalibrationConfig& config + ); + bool saveTemperatureCalibration( + uint8_t sensorId, + const GyroTemperatureCalibrationConfig& config + ); - private: - void loadSensors(); - bool runMigrations(int32_t version); +private: + void loadSensors(); + bool runMigrations(int32_t version); - bool m_Loaded = false; + bool m_Loaded = false; - DeviceConfig m_Config{}; - std::vector m_Sensors; + DeviceConfig m_Config{}; + std::vector m_Sensors; - Logging::Logger m_Logger = Logging::Logger("Configuration"); - }; - } -} + Logging::Logger m_Logger = Logging::Logger("Configuration"); +}; +} // namespace Configuration +} // namespace SlimeVR #endif diff --git a/src/consts.h b/src/consts.h index a6640879a..04e57698d 100644 --- a/src/consts.h +++ b/src/consts.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_CONSTS_H_ #define SLIMEVR_CONSTS_H_ @@ -26,23 +26,23 @@ // List of constants used in other places enum class ImuID { - Unknown = 0, - MPU9250, - MPU6500, - BNO080, - BNO085, - BNO055, - MPU6050, - BNO086, - BMI160, - ICM20948, - ICM42688, - BMI270, - LSM6DS3TRC, - LSM6DSV, - LSM6DSO, - LSM6DSR, - Empty = 255 + Unknown = 0, + MPU9250, + MPU6500, + BNO080, + BNO085, + BNO055, + MPU6050, + BNO086, + BMI160, + ICM20948, + ICM42688, + BMI270, + LSM6DS3TRC, + LSM6DSV, + LSM6DSO, + LSM6DSR, + 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_WRANGLER 14 // Only used by wrangler app -#define BOARD_MOCOPI 15 // Used by mocopi/moslime +#define BOARD_OWOTRACK 13 // Only used by owoTrack mobile app +#define BOARD_WRANGLER 14 // Only used by wrangler app +#define BOARD_MOCOPI 15 // Used by mocopi/moslime #define BOARD_WEMOSWROOM02 16 #define BOARD_XIAO_ESP32C3 17 -#define BOARD_HARITORA 18 // Used by Haritora/SlimeTora -#define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware +#define BOARD_HARITORA 18 // Used by Haritora/SlimeTora +#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_NONE 1 // No sleeping, no PS => for connection issues -#define POWER_SAVING_MINIMUM 2 // Sleeping and PS => default -#define POWER_SAVING_MODERATE 3 // Sleeping and better PS => might miss broadcasts, use at own risk -#define POWER_SAVING_MAXIMUM 4 // Actual CPU sleeping, currently has issues with disconnecting +#define POWER_SAVING_LEGACY 0 // No sleeping, but PS enabled +#define POWER_SAVING_NONE 1 // No sleeping, no PS => for connection issues +#define POWER_SAVING_MINIMUM 2 // Sleeping and PS => default +#define POWER_SAVING_MODERATE \ + 3 // Sleeping and better PS => might miss broadcasts, use at own risk +#define POWER_SAVING_MAXIMUM \ + 4 // Actual CPU sleeping, currently has issues with disconnecting // Send rotation/acceleration data as separate frames. // PPS: 1470 @ 5+1, 1960 @ 5+3 #define PACKET_BUNDLING_DISABLED 0 // Less packets. Pack data per sensor and send asap. -// Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3 setups. -// PPS: 680 @ 5+1, 740 @ 5+3 +// Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3 +// setups. PPS: 680 @ 5+1, 740 @ 5+3 #define PACKET_BUNDLING_LOWLATENCY 1 -// Even less packets, if more than 1 sensor - wait for data from all sensors or until timeout, then send. -// Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for 5+1, by ~15% for 5+3 setups. -// PPS: 650 @ 5+1, 650 @ 5+3 +// Even less packets, if more than 1 sensor - wait for data from all sensors or until +// timeout, then send. Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for +// 5+1, by ~15% for 5+3 setups. PPS: 650 @ 5+1, 650 @ 5+3 #define PACKET_BUNDLING_BUFFERED 2 -// Get radian for a given angle from 0° to 360° (2*PI*r, solve for r given an angle, range -180° to 180°) +// 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_WRANGLER 4 // Only used by wrangler app -#define MCU_OWOTRACK_IOS 5 // 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_OWOTRACK_IOS 5 // Only used by owoTrack mobile app #define MCU_ESP32_C3 6 -#define MCU_MOCOPI 7 // Used by mocopi/moslime -#define MCU_HARITORA 8 // Used by Haritora/SlimeTora -#define MCU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware +#define MCU_MOCOPI 7 // Used by mocopi/moslime +#define MCU_HARITORA 8 // Used by Haritora/SlimeTora +#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_ diff --git a/src/credentials.h b/src/credentials.h index f724df37c..ce1a0c0e3 100644 --- a/src/credentials.h +++ b/src/credentials.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_CREDENTIALS_H_ #define SLIMEVR_CREDENTIALS_H_ @@ -30,6 +30,7 @@ // firmware. We don't have any hardware buttons for the user to confirm // OTA update, so this is the best way we have. // OTA is allowed only for the first 60 seconds after device startup. -const char *otaPassword = "SlimeVR-OTA"; // YOUR OTA PASSWORD HERE, LEAVE EMPTY TO DISABLE OTA UPDATES +const char* otaPassword + = "SlimeVR-OTA"; // YOUR OTA PASSWORD HERE, LEAVE EMPTY TO DISABLE OTA UPDATES -#endif // SLIMEVR_CREDENTIALS_H_ +#endif // SLIMEVR_CREDENTIALS_H_ diff --git a/src/debug.h b/src/debug.h index 5e794f26b..c2f05425f 100644 --- a/src/debug.h +++ b/src/debug.h @@ -1,50 +1,55 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_DEBUG_H_ #define SLIMEVR_DEBUG_H_ #include "consts.h" #include "logging/Level.h" -#define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration -#define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs -#define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently) -#define LOAD_BIAS true // Loads the bias values from NVS on start -#define SAVE_BIAS true // Periodically saves bias calibration data to NVS -#define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only) -#define ENABLE_TAP false // monitor accel for (triple) tap events and send them. Uses more cpu, disable if problems. Server does nothing with value so disabled atm -#define SEND_ACCELERATION true // send linear acceleration to the server - -//Debug information +#define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to + // startup/traditional-calibration +#define BNO_USE_ARVR_STABILIZATION \ + true // Set to false to disable stabilization for BNO085+ IMUs +#define USE_6_AXIS \ + true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently) +#define LOAD_BIAS true // Loads the bias values from NVS on start +#define SAVE_BIAS true // Periodically saves bias calibration data to NVS +#define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only) +#define ENABLE_TAP \ + false // monitor accel for (triple) tap events and send them. Uses more cpu, + // disable if problems. Server does nothing with value so disabled atm +#define SEND_ACCELERATION true // send linear acceleration to the server + +// Debug information #define LOG_LEVEL LOG_LEVEL_DEBUG #if LOG_LEVEL == LOG_LEVEL_TRACE - #define DEBUG_SENSOR - #define DEBUG_NETWORK - #define DEBUG_CONFIGURATION +#define DEBUG_SENSOR +#define DEBUG_NETWORK +#define DEBUG_CONFIGURATION #endif -#define serialDebug false // Set to true to get Serial output for debugging +#define serialDebug false // Set to true to get Serial output for debugging #define serialBaudRate 115200 #define LED_INTERVAL_STANDBY 10000 #define PRINT_STATE_EVERY_MS 60000 @@ -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_ diff --git a/src/defines.h b/src/defines.h index 33296454a..34da04b99 100644 --- a/src/defines.h +++ b/src/defines.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ // ================================================ // See docs for configuration options and examples: @@ -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, \ - AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z) +#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN, +AXIS_REMAP_USE_Z, \ AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z) -IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ +IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, +PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ */ #ifndef IMU_DESC_LIST -#define IMU_DESC_LIST \ - IMU_DESC_ENTRY(IMU, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, PIN_IMU_INT) \ - IMU_DESC_ENTRY(SECOND_IMU, SECONDARY_IMU_ADDRESS_TWO, SECOND_IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, SECONDARY_IMU_OPTIONAL, PIN_IMU_INT_2) +#define IMU_DESC_LIST \ + IMU_DESC_ENTRY( \ + IMU, \ + PRIMARY_IMU_ADDRESS_ONE, \ + IMU_ROTATION, \ + PIN_IMU_SCL, \ + PIN_IMU_SDA, \ + PRIMARY_IMU_OPTIONAL, \ + PIN_IMU_INT \ + ) \ + IMU_DESC_ENTRY( \ + SECOND_IMU, \ + SECONDARY_IMU_ADDRESS_TWO, \ + SECOND_IMU_ROTATION, \ + PIN_IMU_SCL, \ + PIN_IMU_SDA, \ + SECONDARY_IMU_OPTIONAL, \ + PIN_IMU_INT_2 \ + ) #endif // 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. -// For other boards you can now adjust the other resistor values. -// The diagram looks like this: -// (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2] ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND) -// #define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in external resistor value in kOhm -// #define BATTERY_SHIELD_R1 100 // Board voltage divider resistor Ain to GND in kOhm -// #define BATTERY_SHIELD_R2 220 // Board voltage divider resistor Ain to INPUT_BOARD in kOhm +// D1 Mini boards with ESP8266 have internal resistors. For these boards you only have +// to adjust BATTERY_SHIELD_RESISTANCE. For other boards you can now adjust the other +// resistor values. The diagram looks like this: +// (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2] +// ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND) +// #define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in +// external resistor value in kOhm #define BATTERY_SHIELD_R1 100 // Board voltage +// divider resistor Ain to GND in kOhm #define BATTERY_SHIELD_R2 220 // Board voltage +// divider resistor Ain to INPUT_BOARD in kOhm // LED configuration: // Configuration Priority 1 = Highest: @@ -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_SCL 12 - #define PIN_IMU_INT 16 - #define PIN_IMU_INT_2 13 - #define PIN_BATTERY_LEVEL 17 - #define LED_PIN 2 - #define LED_INVERTED true - #ifndef BATTERY_SHIELD_RESISTANCE - #define BATTERY_SHIELD_RESISTANCE 0 - #endif - #ifndef BATTERY_SHIELD_R1 - #define BATTERY_SHIELD_R1 10 - #endif - #ifndef BATTERY_SHIELD_R2 - #define BATTERY_SHIELD_R2 40.2 - #endif +#define PIN_IMU_SDA 14 +#define PIN_IMU_SCL 12 +#define PIN_IMU_INT 16 +#define PIN_IMU_INT_2 13 +#define PIN_BATTERY_LEVEL 17 +#define LED_PIN 2 +#define LED_INVERTED true +#ifndef BATTERY_SHIELD_RESISTANCE +#define BATTERY_SHIELD_RESISTANCE 0 +#endif +#ifndef BATTERY_SHIELD_R1 +#define BATTERY_SHIELD_R1 10 +#endif +#ifndef BATTERY_SHIELD_R2 +#define BATTERY_SHIELD_R2 40.2 +#endif #elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV - #define PIN_IMU_SDA 4 - #define PIN_IMU_SCL 5 - #define PIN_IMU_INT 10 - #define PIN_IMU_INT_2 13 - #define PIN_BATTERY_LEVEL 17 - #define LED_PIN 2 - #define LED_INVERTED true - #ifndef BATTERY_SHIELD_RESISTANCE - #define BATTERY_SHIELD_RESISTANCE 0 - #endif - #ifndef BATTERY_SHIELD_R1 - #define BATTERY_SHIELD_R1 10 - #endif - #ifndef BATTERY_SHIELD_R2 - #define BATTERY_SHIELD_R2 40.2 - #endif +#define PIN_IMU_SDA 4 +#define PIN_IMU_SCL 5 +#define PIN_IMU_INT 10 +#define PIN_IMU_INT_2 13 +#define PIN_BATTERY_LEVEL 17 +#define LED_PIN 2 +#define LED_INVERTED true +#ifndef BATTERY_SHIELD_RESISTANCE +#define BATTERY_SHIELD_RESISTANCE 0 +#endif +#ifndef BATTERY_SHIELD_R1 +#define BATTERY_SHIELD_R1 10 +#endif +#ifndef BATTERY_SHIELD_R2 +#define BATTERY_SHIELD_R2 40.2 +#endif #elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI - #define PIN_IMU_SDA D2 - #define PIN_IMU_SCL D1 - #define PIN_IMU_INT D5 - #define PIN_IMU_INT_2 D6 - #define PIN_BATTERY_LEVEL A0 +#define PIN_IMU_SDA D2 +#define PIN_IMU_SCL D1 +#define PIN_IMU_INT D5 +#define PIN_IMU_INT_2 D6 +#define PIN_BATTERY_LEVEL A0 // #define LED_PIN 2 // #define LED_INVERTED true - #ifndef BATTERY_SHIELD_RESISTANCE - #define BATTERY_SHIELD_RESISTANCE 180 - #endif - #ifndef BATTERY_SHIELD_R1 - #define BATTERY_SHIELD_R1 100 - #endif - #ifndef BATTERY_SHIELD_R2 - #define BATTERY_SHIELD_R2 220 - #endif +#ifndef BATTERY_SHIELD_RESISTANCE +#define BATTERY_SHIELD_RESISTANCE 180 +#endif +#ifndef BATTERY_SHIELD_R1 +#define BATTERY_SHIELD_R1 100 +#endif +#ifndef BATTERY_SHIELD_R2 +#define BATTERY_SHIELD_R2 220 +#endif #elif BOARD == BOARD_ESP01 - #define PIN_IMU_SDA 2 - #define PIN_IMU_SCL 0 - #define PIN_IMU_INT 255 - #define PIN_IMU_INT_2 255 - #define PIN_BATTERY_LEVEL 255 - #define LED_PIN LED_OFF - #define LED_INVERTED false +#define PIN_IMU_SDA 2 +#define PIN_IMU_SCL 0 +#define PIN_IMU_INT 255 +#define PIN_IMU_INT_2 255 +#define PIN_BATTERY_LEVEL 255 +#define LED_PIN LED_OFF +#define LED_INVERTED false #elif BOARD == BOARD_TTGO_TBASE - #define PIN_IMU_SDA 5 - #define PIN_IMU_SCL 4 - #define PIN_IMU_INT 14 - #define PIN_IMU_INT_2 13 - #define PIN_BATTERY_LEVEL A0 +#define PIN_IMU_SDA 5 +#define PIN_IMU_SCL 4 +#define PIN_IMU_INT 14 +#define PIN_IMU_INT_2 13 +#define PIN_BATTERY_LEVEL A0 // #define LED_PIN 2 // #define LED_INVERTED false #elif BOARD == BOARD_CUSTOM - // Define pins by the examples above +// Define pins by the examples above #elif BOARD == BOARD_WROOM32 - #define PIN_IMU_SDA 21 - #define PIN_IMU_SCL 22 - #define PIN_IMU_INT 23 - #define PIN_IMU_INT_2 25 - #define PIN_BATTERY_LEVEL 36 +#define PIN_IMU_SDA 21 +#define PIN_IMU_SCL 22 +#define PIN_IMU_INT 23 +#define PIN_IMU_INT_2 25 +#define PIN_BATTERY_LEVEL 36 // #define LED_PIN 2 // #define LED_INVERTED false #elif BOARD == BOARD_LOLIN_C3_MINI - #define PIN_IMU_SDA 5 - #define PIN_IMU_SCL 4 - #define PIN_IMU_INT 6 - #define PIN_IMU_INT_2 8 - #define PIN_BATTERY_LEVEL 3 - #define LED_PIN 7 +#define PIN_IMU_SDA 5 +#define PIN_IMU_SCL 4 +#define PIN_IMU_INT 6 +#define PIN_IMU_INT_2 8 +#define PIN_BATTERY_LEVEL 3 +#define LED_PIN 7 // #define LED_INVERTED false #elif BOARD == BOARD_BEETLE32C3 - #define PIN_IMU_SDA 8 - #define PIN_IMU_SCL 9 - #define PIN_IMU_INT 6 - #define PIN_IMU_INT_2 7 - #define PIN_BATTERY_LEVEL 3 - #define LED_PIN 10 - #define LED_INVERTED false +#define PIN_IMU_SDA 8 +#define PIN_IMU_SCL 9 +#define PIN_IMU_INT 6 +#define PIN_IMU_INT_2 7 +#define PIN_BATTERY_LEVEL 3 +#define LED_PIN 10 +#define LED_INVERTED false #elif BOARD == BOARD_ES32C3DEVKITM1 - #define PIN_IMU_SDA 5 - #define PIN_IMU_SCL 4 - #define PIN_IMU_INT 6 - #define PIN_IMU_INT_2 7 - #define PIN_BATTERY_LEVEL 3 - #define LED_PIN LED_OFF // RGB LED Protocol would need to be implementetet did not brother for the test, because the board ideal for tracker ifself +#define PIN_IMU_SDA 5 +#define PIN_IMU_SCL 4 +#define PIN_IMU_INT 6 +#define PIN_IMU_INT_2 7 +#define PIN_BATTERY_LEVEL 3 +#define LED_PIN \ + LED_OFF // RGB LED Protocol would need to be implementetet did not brother for the + // test, because the board ideal for tracker ifself // #define LED_INVERTED false #elif BOARD == BOARD_WEMOSWROOM02 - #define PIN_IMU_SDA 2 - #define PIN_IMU_SCL 14 - #define PIN_IMU_INT 0 - #define PIN_IMU_INT_2 4 - #define PIN_BATTERY_LEVEL A0 - #define LED_PIN 16 - #define LED_INVERTED true +#define PIN_IMU_SDA 2 +#define PIN_IMU_SCL 14 +#define PIN_IMU_INT 0 +#define PIN_IMU_INT_2 4 +#define PIN_BATTERY_LEVEL A0 +#define LED_PIN 16 +#define LED_INVERTED true #elif BOARD == BOARD_XIAO_ESP32C3 - #define PIN_IMU_SDA 6 // D4 - #define PIN_IMU_SCL 7 // D5 - #define PIN_IMU_INT 5 // D3 - #define PIN_IMU_INT_2 10 // D10 - #define LED_PIN 4 // D2 - #define LED_INVERTED false - #define PIN_BATTERY_LEVEL 2 // D0 / A0 - #ifndef BATTERY_SHIELD_RESISTANCE - #define BATTERY_SHIELD_RESISTANCE 0 - #endif - #ifndef BATTERY_SHIELD_R1 - #define BATTERY_SHIELD_R1 100 - #endif - #ifndef BATTERY_SHIELD_R2 - #define BATTERY_SHIELD_R2 100 - #endif +#define PIN_IMU_SDA 6 // D4 +#define PIN_IMU_SCL 7 // D5 +#define PIN_IMU_INT 5 // D3 +#define PIN_IMU_INT_2 10 // D10 +#define LED_PIN 4 // D2 +#define LED_INVERTED false +#define PIN_BATTERY_LEVEL 2 // D0 / A0 +#ifndef BATTERY_SHIELD_RESISTANCE +#define BATTERY_SHIELD_RESISTANCE 0 +#endif +#ifndef BATTERY_SHIELD_R1 +#define BATTERY_SHIELD_R1 100 +#endif +#ifndef BATTERY_SHIELD_R2 +#define BATTERY_SHIELD_R2 100 +#endif #endif diff --git a/src/defines_bmi160.h b/src/defines_bmi160.h index 9eb9eea91..747656c82 100644 --- a/src/defines_bmi160.h +++ b/src/defines_bmi160.h @@ -1,32 +1,31 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 SlimeVR Contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 SlimeVR Contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef BMI160_DEFINES_H #define BMI160_DEFINES_H -// BMI160 magnetometer type, applies to both main and aux trackers, mixed types are not supported currently. -// If only 1 out of 2 trackers has a mag, tracker without a mag should still function normally. -// NOT USED if USE_6_AXIS == true -// Pick one: +// BMI160 magnetometer type, applies to both main and aux trackers, mixed types are not +// supported currently. If only 1 out of 2 trackers has a mag, tracker without a mag +// should still function normally. NOT USED if USE_6_AXIS == true Pick one: #define BMI160_MAG_TYPE BMI160_MAG_TYPE_HMC // #define BMI160_MAG_TYPE BMI160_MAG_TYPE_QMC @@ -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. -// Magnetometer not be used until you calibrate it. -// Disables this calibration step if value is 0. -// NOT USED if USE_6_AXIS == true -// Default: 20 +// How long to run magnetometer calibration for, if enabled and you have added a +// magnetometer. Magnetometer not be used until you calibrate it. Disables this +// calibration step if value is 0. NOT USED if USE_6_AXIS == true Default: 20 #define BMI160_CALIBRATION_MAG_SECONDS 20 // Send temperature to the server as AXXYY, diff --git a/src/defines_sensitivity.h b/src/defines_sensitivity.h index a46189aef..ca947f459 100644 --- a/src/defines_sensitivity.h +++ b/src/defines_sensitivity.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef DEFINES_SENSITIVITY_H @@ -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 - { .mac = "A4:E5:7C:B6:00:01", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 }, - { .mac = "A4:E5:7C:B6:00:02", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 }, - { .mac = "A4:E5:7C:B6:00:03", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 11, .y = 2.2, .z = -1 }, - { .mac = "A4:E5:7C:B6:00:04", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 }, - { .mac = "A4:E5:7C:B6:00:05", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -10.63, .y = -8.25, .z = -18.6 }, + // example values + {.mac = "A4:E5:7C:B6:00:01", + .sensorId = SENSORID_PRIMARY, + .spins = 10, + .x = 2.63, + .y = 37.82, + .z = 31.11}, + {.mac = "A4:E5:7C:B6:00:02", + .sensorId = SENSORID_PRIMARY, + .spins = 10, + .x = -2.38, + .y = -26.8, + .z = -42.78}, + {.mac = "A4:E5:7C:B6:00:03", + .sensorId = SENSORID_PRIMARY, + .spins = 10, + .x = 11, + .y = 2.2, + .z = -1}, + {.mac = "A4:E5:7C:B6:00:04", + .sensorId = SENSORID_PRIMARY, + .spins = 10, + .x = -7, + .y = -53.7, + .z = -57}, + {.mac = "A4:E5:7C:B6:00:05", + .sensorId = SENSORID_PRIMARY, + .spins = 10, + .x = -10.63, + .y = -8.25, + .z = -18.6}, }; #endif \ No newline at end of file diff --git a/src/globals.h b/src/globals.h index 8667ef193..67eaac12e 100644 --- a/src/globals.h +++ b/src/globals.h @@ -1,29 +1,30 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_GLOBALS_H_ #define SLIMEVR_GLOBALS_H_ #include + #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 there is no pin defined for the board, use LED_PIN 255 and disable LED +// If LED_PIN is not defined in "defines.h" take the default pin from "pins_arduino.h" +// framework. If there is no pin defined for the board, use LED_PIN 255 and disable LED #if defined(LED_PIN) - // LED_PIN is defined - #if (LED_PIN < 0) || (LED_PIN >= LED_OFF) - #define ENABLE_LEDS false - #else - #define ENABLE_LEDS true - #endif +// LED_PIN is defined +#if (LED_PIN < 0) || (LED_PIN >= LED_OFF) +#define ENABLE_LEDS false +#else +#define ENABLE_LEDS true +#endif #else - // LED_PIN is not defined - #if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0) - #define LED_PIN LED_BUILTIN - #define ENABLE_LEDS true - #else - #define LED_PIN LED_OFF - #define ENABLE_LEDS false - #endif +// LED_PIN is not defined +#if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0) +#define LED_PIN LED_BUILTIN +#define ENABLE_LEDS true +#else +#define LED_PIN LED_OFF +#define ENABLE_LEDS false +#endif #endif #if !defined(LED_INVERTED) - // default is inverted for SlimeVR / ESP-12E - #define LED_INVERTED true +// default is inverted for SlimeVR / ESP-12E +#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_ diff --git a/src/logging/Level.cpp b/src/logging/Level.cpp index 71bce7865..8c9b67702 100644 --- a/src/logging/Level.cpp +++ b/src/logging/Level.cpp @@ -1,28 +1,24 @@ #include "Level.h" -namespace SlimeVR -{ - namespace Logging - { - const char *levelToString(Level level) - { - switch (level) - { - case TRACE: - return "TRACE"; - case DEBUG: - return "DEBUG"; - case INFO: - return "INFO"; - case WARN: - return "WARN"; - case ERROR: - return "ERROR"; - case FATAL: - return "FATAL"; - default: - return "UNKNOWN"; - } - } - } +namespace SlimeVR { +namespace Logging { +const char* levelToString(Level level) { + switch (level) { + case TRACE: + return "TRACE"; + case DEBUG: + return "DEBUG"; + case INFO: + return "INFO"; + case WARN: + return "WARN"; + case ERROR: + return "ERROR"; + case FATAL: + return "FATAL"; + default: + return "UNKNOWN"; + } } +} // namespace Logging +} // namespace SlimeVR diff --git a/src/logging/Level.h b/src/logging/Level.h index 3bd8a72f5..b33ca55da 100644 --- a/src/logging/Level.h +++ b/src/logging/Level.h @@ -7,23 +7,20 @@ #define LOG_LEVEL_ERROR 4 #define LOG_LEVEL_FATAL 5 -namespace SlimeVR -{ - namespace Logging - { - enum Level - { - TRACE = LOG_LEVEL_TRACE, - DEBUG = LOG_LEVEL_DEBUG, - INFO = LOG_LEVEL_INFO, - WARN = LOG_LEVEL_WARN, - ERROR = LOG_LEVEL_ERROR, - FATAL = LOG_LEVEL_FATAL - }; +namespace SlimeVR { +namespace Logging { +enum Level { + TRACE = LOG_LEVEL_TRACE, + DEBUG = LOG_LEVEL_DEBUG, + INFO = LOG_LEVEL_INFO, + WARN = LOG_LEVEL_WARN, + ERROR = LOG_LEVEL_ERROR, + FATAL = LOG_LEVEL_FATAL +}; - const char *levelToString(Level level); - } -} +const char* levelToString(Level level); +} // namespace Logging +} // namespace SlimeVR #define LOGGING_LEVEL_H #endif diff --git a/src/logging/Logger.cpp b/src/logging/Logger.cpp index de7a5a639..be8d68b9b 100644 --- a/src/logging/Logger.cpp +++ b/src/logging/Logger.cpp @@ -1,82 +1,70 @@ #include "Logger.h" -namespace SlimeVR -{ - namespace Logging - { - void Logger::setTag(const char *tag) - { - m_Tag = (char *)malloc(strlen(tag) + 1); - strcpy(m_Tag, tag); - } +namespace SlimeVR { +namespace Logging { +void Logger::setTag(const char* tag) { + m_Tag = (char*)malloc(strlen(tag) + 1); + strcpy(m_Tag, tag); +} - void Logger::trace(const char *format, ...) - { - va_list args; - va_start(args, format); - log(TRACE, format, args); - va_end(args); - } +void Logger::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::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::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::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::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::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; - } +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 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); - } + 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); - } - } + Serial.printf("[%-5s] [%s] %s\n", levelToString(level), buf, buffer); } +} // namespace Logging +} // namespace SlimeVR diff --git a/src/logging/Logger.h b/src/logging/Logger.h index 06e46c979..09d1d2b9f 100644 --- a/src/logging/Logger.h +++ b/src/logging/Logger.h @@ -1,109 +1,98 @@ #ifndef LOGGING_LOGGER_H #define LOGGING_LOGGER_H +#include + #include "Level.h" #include "debug.h" -#include -namespace SlimeVR -{ - namespace Logging - { - class Logger - { - public: - Logger(const char *prefix) : m_Prefix(prefix), m_Tag(nullptr){}; - Logger(const char *prefix, const char *tag) : m_Prefix(prefix), m_Tag(nullptr) - { - setTag(tag); - }; - - ~Logger() - { - if (m_Tag != nullptr) - { - free(m_Tag); - } - } - - void setTag(const char *tag); - - void trace(const char *str, ...); - void debug(const char *str, ...); - void info(const char *str, ...); - void warn(const char *str, ...); - void error(const char *str, ...); - void fatal(const char *str, ...); - - template - inline void traceArray(const char *str, const T *array, size_t size) - { - logArray(TRACE, str, array, size); - } - - template - inline void debugArray(const char *str, const T *array, size_t size) - { - logArray(DEBUG, str, array, size); - } - - template - inline void infoArray(const char *str, const T *array, size_t size) - { - logArray(INFO, str, array, size); - } - - template - inline void warnArray(const char *str, const T *array, size_t size) - { - logArray(WARN, str, array, size); - } - - template - inline void errorArray(const char *str, const T *array, size_t size) - { - logArray(ERROR, str, array, size); - } - - template - inline void fatalArray(const char *str, const T *array, size_t size) - { - logArray(FATAL, str, array, size); - } - - private: - void log(Level level, const char *str, va_list args); - - template - void logArray(Level level, const char *str, const T *array, size_t size) - { - if (level < LOG_LEVEL) - { - return; - } - - 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", levelToString(level), buf, str); - - for (size_t i = 0; i < size; i++) - { - Serial.print(array[i]); - } - - Serial.println(); - } - - const char *const m_Prefix; - char *m_Tag; - }; - } -} +namespace SlimeVR { +namespace Logging { +class Logger { +public: + Logger(const char* prefix) + : m_Prefix(prefix) + , m_Tag(nullptr){}; + Logger(const char* prefix, const char* tag) + : m_Prefix(prefix) + , m_Tag(nullptr) { + setTag(tag); + }; + + ~Logger() { + if (m_Tag != nullptr) { + free(m_Tag); + } + } + + void setTag(const char* tag); + + void trace(const char* str, ...); + void debug(const char* str, ...); + void info(const char* str, ...); + void warn(const char* str, ...); + void error(const char* str, ...); + void fatal(const char* str, ...); + + template + inline void traceArray(const char* str, const T* array, size_t size) { + logArray(TRACE, str, array, size); + } + + template + inline void debugArray(const char* str, const T* array, size_t size) { + logArray(DEBUG, str, array, size); + } + + template + inline void infoArray(const char* str, const T* array, size_t size) { + logArray(INFO, str, array, size); + } + + template + inline void warnArray(const char* str, const T* array, size_t size) { + logArray(WARN, str, array, size); + } + + template + inline void errorArray(const char* str, const T* array, size_t size) { + logArray(ERROR, str, array, size); + } + + template + inline void fatalArray(const char* str, const T* array, size_t size) { + logArray(FATAL, str, array, size); + } + +private: + void log(Level level, const char* str, va_list args); + + template + void logArray(Level level, const char* str, const T* array, size_t size) { + if (level < LOG_LEVEL) { + return; + } + + 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", levelToString(level), buf, str); + + for (size_t i = 0; i < size; i++) { + Serial.print(array[i]); + } + + Serial.println(); + } + + const char* const m_Prefix; + char* m_Tag; +}; +} // namespace Logging +} // namespace SlimeVR #endif diff --git a/src/main.cpp b/src/main.cpp index 9273451eb..c57b5959c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,35 +1,36 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ -#include "Wire.h" -#include "ota.h" -#include "GlobalVars.h" -#include "globals.h" -#include "credentials.h" #include -#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() -{ - Serial.begin(serialBaudRate); - globalTimer = timer_create_default(); +void setup() { + Serial.begin(serialBaudRate); + globalTimer = timer_create_default(); #ifdef ESP32C3 - // Wait for the Computer to be able to connect. - delay(2000); + // Wait for the Computer to be able to connect. + 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(); - configuration.setup(); + ledManager.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 - // 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. + I2CSCAN::clearBus( + PIN_IMU_SDA, + 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.) - Wire.end(); + // For some unknown reason the I2C seem to be open on ESP32-C3 by default. Let's + // just close it before opening it again. (The ESP32-C3 only has 1 I2C.) + Wire.end(); #endif - // using `static_cast` here seems to be better, because there are 2 similar function signatures - Wire.begin(static_cast(PIN_IMU_SDA), static_cast(PIN_IMU_SCL)); + // using `static_cast` here seems to be better, because there are 2 similar function + // signatures + Wire.begin(static_cast(PIN_IMU_SDA), static_cast(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 - Wire.setTimeOut(150); +#ifdef ESP32 // Counterpart on ESP32 to ClockStretchLimit + Wire.setTimeOut(150); #endif - Wire.setClock(I2C_SPEED); + Wire.setClock(I2C_SPEED); - // Wait for IMU to boot - delay(500); + // Wait for IMU to boot + delay(500); - sensorManager.setup(); + sensorManager.setup(); - networkManager.setup(); - OTA::otaSetup(otaPassword); - battery.Setup(); + networkManager.setup(); + OTA::otaSetup(otaPassword); + battery.Setup(); - statusManager.setStatus(SlimeVR::Status::LOADING, false); + statusManager.setStatus(SlimeVR::Status::LOADING, false); - sensorManager.postSetup(); + sensorManager.postSetup(); - loopTime = micros(); + loopTime = micros(); } -void loop() -{ - globalTimer.tick(); - SerialCommands::update(); - OTA::otaUpdate(); - networkManager.update(); - sensorManager.update(); - battery.Loop(); - ledManager.update(); +void loop() { + globalTimer.tick(); + SerialCommands::update(); + OTA::otaUpdate(); + networkManager.update(); + sensorManager.update(); + battery.Loop(); + ledManager.update(); #ifdef TARGET_LOOPTIME_MICROS - long elapsed = (micros() - loopTime); - if (elapsed < TARGET_LOOPTIME_MICROS) - { - long sleepus = TARGET_LOOPTIME_MICROS - elapsed - 100;//µs to sleep - long sleepms = sleepus / 1000;//ms to sleep - if(sleepms > 0) // if >= 1 ms - { - delay(sleepms); // sleep ms = save power - sleepus -= sleepms * 1000; - } - if (sleepus > 100) - { - delayMicroseconds(sleepus); - } - } - loopTime = micros(); + long elapsed = (micros() - loopTime); + if (elapsed < TARGET_LOOPTIME_MICROS) { + long sleepus = TARGET_LOOPTIME_MICROS - elapsed - 100; // µs to sleep + long sleepms = sleepus / 1000; // ms to sleep + if (sleepms > 0) // if >= 1 ms + { + delay(sleepms); // sleep ms = save power + sleepus -= sleepms * 1000; + } + if (sleepus > 100) { + delayMicroseconds(sleepus); + } + } + loopTime = micros(); +#endif +#if defined(PRINT_STATE_EVERY_MS) && PRINT_STATE_EVERY_MS > 0 + unsigned long now = millis(); + if (lastStatePrint + PRINT_STATE_EVERY_MS < now) { + lastStatePrint = now; + SerialCommands::printState(); + } #endif - #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 } diff --git a/src/motionprocessing/GyroTemperatureCalibrator.cpp b/src/motionprocessing/GyroTemperatureCalibrator.cpp index 6fe8d4de0..8b4cc02eb 100644 --- a/src/motionprocessing/GyroTemperatureCalibrator.cpp +++ b/src/motionprocessing/GyroTemperatureCalibrator.cpp @@ -1,211 +1,244 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "GyroTemperatureCalibrator.h" + #include "GlobalVars.h" void GyroTemperatureCalibrator::resetCurrentTemperatureState() { - if (!state.numSamples) return; - state.numSamples = 0; - state.tSum = 0; - state.xSum = 0; - state.ySum = 0; - state.zSum = 0; + if (!state.numSamples) { + return; + } + state.numSamples = 0; + state.tSum = 0; + state.xSum = 0; + state.ySum = 0; + state.zSum = 0; } // must be called for every raw gyro sample -void GyroTemperatureCalibrator::updateGyroTemperatureCalibration(const float temperature, const bool restDetected, int16_t x, int16_t y, int16_t z) { - if (!restDetected) { - return resetCurrentTemperatureState(); - } - - if (temperature < TEMP_CALIBRATION_MIN && !calibrationRunning) { - calibrationRunning = true; - configSaved = false; - config.reset(); - } - if (temperature > TEMP_CALIBRATION_MAX && calibrationRunning) { - auto coeffs = poly.computeCoefficients(); - for (uint32_t i = 0; i < poly.numCoefficients; i++) { - config.cx[i] = coeffs[0][i]; - config.cy[i] = coeffs[1][i]; - config.cz[i] = coeffs[2][i]; - } - config.hasCoeffs = true; - bst = 0.0f; - bsx = 0; - bsy = 0; - bsz = 0; - bn = 0; - lastTemp = 0; - calibrationRunning = false; - if (!configSaveFailed && !configSaved) { - saveConfig(); - } - } - if (calibrationRunning) { - if (fabs(lastTemp - temperature) > 0.03f) { - const double avgt = (double)bst / bn; - const double avgValues[] = { - (double)bsx / bn, - (double)bsy / bn, - (double)bsz / bn, - }; - if (bn > 0) poly.update(avgt, avgValues); - bst = 0.0f; - bsx = 0; - bsy = 0; - bsz = 0; - bn = 0; - lastTemp = temperature; - } else { - bst += temperature; - bsx += x; - bsy += y; - bsz += z; - bn++; - } - } - - const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(temperature); - - if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) return; - - bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f; - if (currentTempAlreadyCalibrated) return; - - if (state.temperatureCurrentIdx != idx) { - state.temperatureCurrentIdx = idx; - resetCurrentTemperatureState(); - } - - float temperatureStepBoundsMin = TEMP_CALIBRATION_IDX_TO_TEMP(idx) - TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP; - float temperatureStepBoundsMax = TEMP_CALIBRATION_IDX_TO_TEMP(idx) + TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP; - bool isTemperatureOutOfDeviationRange = - temperature < temperatureStepBoundsMin || temperature > temperatureStepBoundsMax; - if (isTemperatureOutOfDeviationRange) { - return resetCurrentTemperatureState(); - } - - state.numSamples++; - state.tSum += temperature; - state.xSum += x; - state.ySum += y; - state.zSum += z; - if (state.numSamples > samplesPerStep) { - bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f; - if (!currentTempAlreadyCalibrated) { - config.samplesTotal++; - } - config.samples[idx].t = state.tSum / state.numSamples; - config.samples[idx].x = ((double)state.xSum / state.numSamples); - config.samples[idx].y = ((double)state.ySum / state.numSamples); - config.samples[idx].z = ((double)state.zSum / state.numSamples); - - config.minTemperatureRange = - min(config.samples[idx].t, config.minTemperatureRange); - config.maxTemperatureRange = - max(config.samples[idx].t, config.maxTemperatureRange); - config.minCalibratedIdx = - TEMP_CALIBRATION_TEMP_TO_IDX(config.minTemperatureRange); - config.maxCalibratedIdx = - TEMP_CALIBRATION_TEMP_TO_IDX(config.maxTemperatureRange); - resetCurrentTemperatureState(); - } +void GyroTemperatureCalibrator::updateGyroTemperatureCalibration( + const float temperature, + const bool restDetected, + int16_t x, + int16_t y, + int16_t z +) { + if (!restDetected) { + return resetCurrentTemperatureState(); + } + + if (temperature < TEMP_CALIBRATION_MIN && !calibrationRunning) { + calibrationRunning = true; + configSaved = false; + config.reset(); + } + if (temperature > TEMP_CALIBRATION_MAX && calibrationRunning) { + auto coeffs = poly.computeCoefficients(); + for (uint32_t i = 0; i < poly.numCoefficients; i++) { + config.cx[i] = coeffs[0][i]; + config.cy[i] = coeffs[1][i]; + config.cz[i] = coeffs[2][i]; + } + config.hasCoeffs = true; + bst = 0.0f; + bsx = 0; + bsy = 0; + bsz = 0; + bn = 0; + lastTemp = 0; + calibrationRunning = false; + if (!configSaveFailed && !configSaved) { + saveConfig(); + } + } + if (calibrationRunning) { + if (fabs(lastTemp - temperature) > 0.03f) { + const double avgt = (double)bst / bn; + const double avgValues[] = { + (double)bsx / bn, + (double)bsy / bn, + (double)bsz / bn, + }; + if (bn > 0) { + poly.update(avgt, avgValues); + } + bst = 0.0f; + bsx = 0; + bsy = 0; + bsz = 0; + bn = 0; + lastTemp = temperature; + } else { + bst += temperature; + bsx += x; + bsy += y; + bsz += z; + bn++; + } + } + + const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(temperature); + + if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) { + return; + } + + bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f; + if (currentTempAlreadyCalibrated) { + return; + } + + if (state.temperatureCurrentIdx != idx) { + state.temperatureCurrentIdx = idx; + resetCurrentTemperatureState(); + } + + float temperatureStepBoundsMin + = TEMP_CALIBRATION_IDX_TO_TEMP(idx) - TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP; + float temperatureStepBoundsMax + = TEMP_CALIBRATION_IDX_TO_TEMP(idx) + TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP; + bool isTemperatureOutOfDeviationRange = temperature < temperatureStepBoundsMin + || temperature > temperatureStepBoundsMax; + if (isTemperatureOutOfDeviationRange) { + return resetCurrentTemperatureState(); + } + + state.numSamples++; + state.tSum += temperature; + state.xSum += x; + state.ySum += y; + state.zSum += z; + if (state.numSamples > samplesPerStep) { + bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f; + if (!currentTempAlreadyCalibrated) { + config.samplesTotal++; + } + config.samples[idx].t = state.tSum / state.numSamples; + config.samples[idx].x = ((double)state.xSum / state.numSamples); + config.samples[idx].y = ((double)state.ySum / state.numSamples); + config.samples[idx].z = ((double)state.zSum / state.numSamples); + + config.minTemperatureRange + = min(config.samples[idx].t, config.minTemperatureRange); + config.maxTemperatureRange + = max(config.samples[idx].t, config.maxTemperatureRange); + config.minCalibratedIdx + = TEMP_CALIBRATION_TEMP_TO_IDX(config.minTemperatureRange); + config.maxCalibratedIdx + = TEMP_CALIBRATION_TEMP_TO_IDX(config.maxTemperatureRange); + resetCurrentTemperatureState(); + } } -bool GyroTemperatureCalibrator::approximateOffset(const float temperature, float GOxyz[3]) { - if (!config.hasData()) return false; - if (config.hasCoeffs) { - if (lastApproximatedTemperature != 0.0f && temperature == lastApproximatedTemperature) { - GOxyz[0] = lastApproximatedOffsets[0]; - GOxyz[1] = lastApproximatedOffsets[1]; - GOxyz[2] = lastApproximatedOffsets[2]; - } else { - float offsets[3] = { config.cx[3], config.cy[3], config.cz[3] }; - for (int32_t i = 2; i >= 0; i--) { - offsets[0] = offsets[0] * temperature + config.cx[i]; - offsets[1] = offsets[1] * temperature + config.cy[i]; - offsets[2] = offsets[2] * temperature + config.cz[i]; - } - lastApproximatedTemperature = temperature; - lastApproximatedOffsets[0] = GOxyz[0] = offsets[0]; - lastApproximatedOffsets[1] = GOxyz[1] = offsets[1]; - lastApproximatedOffsets[2] = GOxyz[2] = offsets[2]; - } - return true; - } - - const float constrainedTemperature = constrain(temperature, - config.minTemperatureRange, - config.maxTemperatureRange - ); - - const int16_t idx = - TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature); - - if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) return false; - - bool isCurrentTempCalibrated = config.samples[idx].t != 0.0f; - if (isCurrentTempCalibrated) { - GOxyz[0] = config.samples[idx].x; - GOxyz[1] = config.samples[idx].y; - GOxyz[2] = config.samples[idx].z; - return true; - } - - return false; +bool GyroTemperatureCalibrator::approximateOffset( + const float temperature, + float GOxyz[3] +) { + if (!config.hasData()) { + return false; + } + if (config.hasCoeffs) { + if (lastApproximatedTemperature != 0.0f + && temperature == lastApproximatedTemperature) { + GOxyz[0] = lastApproximatedOffsets[0]; + GOxyz[1] = lastApproximatedOffsets[1]; + GOxyz[2] = lastApproximatedOffsets[2]; + } else { + float offsets[3] = {config.cx[3], config.cy[3], config.cz[3]}; + for (int32_t i = 2; i >= 0; i--) { + offsets[0] = offsets[0] * temperature + config.cx[i]; + offsets[1] = offsets[1] * temperature + config.cy[i]; + offsets[2] = offsets[2] * temperature + config.cz[i]; + } + lastApproximatedTemperature = temperature; + lastApproximatedOffsets[0] = GOxyz[0] = offsets[0]; + lastApproximatedOffsets[1] = GOxyz[1] = offsets[1]; + lastApproximatedOffsets[2] = GOxyz[2] = offsets[2]; + } + return true; + } + + const float constrainedTemperature = constrain( + temperature, + config.minTemperatureRange, + config.maxTemperatureRange + ); + + const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature); + + if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) { + return false; + } + + bool isCurrentTempCalibrated = config.samples[idx].t != 0.0f; + if (isCurrentTempCalibrated) { + GOxyz[0] = config.samples[idx].x; + GOxyz[1] = config.samples[idx].y; + GOxyz[2] = config.samples[idx].z; + return true; + } + + return false; } bool GyroTemperatureCalibrator::loadConfig(float newSensitivity) { - bool ok = configuration.loadTemperatureCalibration(sensorId, config); - if (ok) { - config.rescaleSamples(newSensitivity); - if (config.fullyCalibrated()) { - configSaved = true; - } - } else { - m_Logger.warn("No temperature calibration data found for sensor %d, ignoring...", sensorId); - // m_Logger.info("Temperature calibration is advised"); - m_Logger.info("Temperature calibration is a work-in-progress feature; any changes to its parameters or updates will render the saved temperature curve invalid and unloadable."); - } - return configSaved; + bool ok = configuration.loadTemperatureCalibration(sensorId, config); + if (ok) { + config.rescaleSamples(newSensitivity); + if (config.fullyCalibrated()) { + configSaved = true; + } + } else { + m_Logger.warn( + "No temperature calibration data found for sensor %d, ignoring...", + sensorId + ); + // m_Logger.info("Temperature calibration is advised"); + m_Logger.info( + "Temperature calibration is a work-in-progress feature; any changes to its " + "parameters or updates will render the saved temperature curve invalid and " + "unloadable." + ); + } + return configSaved; } bool GyroTemperatureCalibrator::saveConfig() { - if (configuration.saveTemperatureCalibration(sensorId, config)) { - m_Logger.info("Saved temperature calibration config (%0.1f%) for sensorId:%i", - config.getCalibrationDonePercent(), - sensorId - ); - if (config.fullyCalibrated()) { - configSaved = true; - } else { - m_Logger.info("Calibration will resume from this checkpoint after reboot"); - } - } else { - configSaveFailed = true; - m_Logger.error("Something went wrong"); - } - return configSaved; + if (configuration.saveTemperatureCalibration(sensorId, config)) { + m_Logger.info( + "Saved temperature calibration config (%0.1f%) for sensorId:%i", + config.getCalibrationDonePercent(), + sensorId + ); + if (config.fullyCalibrated()) { + configSaved = true; + } else { + m_Logger.info("Calibration will resume from this checkpoint after reboot"); + } + } else { + configSaveFailed = true; + m_Logger.error("Something went wrong"); + } + return configSaved; } diff --git a/src/motionprocessing/GyroTemperatureCalibrator.h b/src/motionprocessing/GyroTemperatureCalibrator.h index 1757a0d2c..352bb0b91 100644 --- a/src/motionprocessing/GyroTemperatureCalibrator.h +++ b/src/motionprocessing/GyroTemperatureCalibrator.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef GYRO_TEMPERATURE_CALIBRATOR_H @@ -26,11 +26,11 @@ #include #include -#include "debug.h" -#include "../logging/Logger.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 - // already calibrated by DMP? +// 16 bit 333 lsb/K, ~0.00508 degrees per bit +// already calibrated by DMP? #elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050 - // 16 bit 340 lsb/K, ~0.00518 degrees per bit +// 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 = - (uint16_t)((TEMP_CALIBRATION_MAX - TEMP_CALIBRATION_MIN) * (1/TEMP_CALIBRATION_STEP)); +#define TEMP_CALIBRATION_TEMP_TO_IDX(temperature) \ + (uint16_t)( \ + (temperature + TEMP_CALIBRATION_STEP / 2.0f) * (1 / TEMP_CALIBRATION_STEP) \ + - TEMP_CALIBRATION_MIN * (1 / TEMP_CALIBRATION_STEP) \ + ) -#define TEMP_CALIBRATION_TEMP_TO_IDX(temperature) (uint16_t)( \ - (temperature + TEMP_CALIBRATION_STEP/2.0f) * (1/TEMP_CALIBRATION_STEP) - \ - TEMP_CALIBRATION_MIN * (1/TEMP_CALIBRATION_STEP) \ -) - -#define TEMP_CALIBRATION_IDX_TO_TEMP(idx) (float)( \ - ((float)idx / (1.0f/TEMP_CALIBRATION_STEP)) + \ - TEMP_CALIBRATION_MIN \ -) +#define TEMP_CALIBRATION_IDX_TO_TEMP(idx) \ + (float)(((float)idx / (1.0f / TEMP_CALIBRATION_STEP)) + TEMP_CALIBRATION_MIN) struct GyroTemperatureCalibrationState { - uint16_t temperatureCurrentIdx; - uint32_t numSamples; - float tSum; - int32_t xSum; - int32_t ySum; - int32_t zSum; - - GyroTemperatureCalibrationState(): temperatureCurrentIdx(-1), numSamples(0), tSum(0.0f), xSum(0), ySum(0), zSum(0) - { }; + uint16_t temperatureCurrentIdx; + uint32_t numSamples; + float tSum; + int32_t xSum; + int32_t ySum; + int32_t zSum; + + GyroTemperatureCalibrationState() + : temperatureCurrentIdx(-1) + , numSamples(0) + , tSum(0.0f) + , xSum(0) + , ySum(0) + , zSum(0){}; }; struct GyroTemperatureOffsetSample { - float t; - float x; - float y; - float z; - - GyroTemperatureOffsetSample(): t(0.0f), x(0), y(0), z(0) - { } + float t; + float x; + float y; + float z; + + GyroTemperatureOffsetSample() + : t(0.0f) + , x(0) + , y(0) + , z(0) {} }; struct GyroTemperatureCalibrationConfig { - SlimeVR::Configuration::SensorConfigType type; - - float sensitivityLSB; - float minTemperatureRange; - float maxTemperatureRange; - uint16_t minCalibratedIdx = 0; - uint16_t maxCalibratedIdx = 0; - GyroTemperatureOffsetSample samples[TEMP_CALIBRATION_BUFFER_SIZE]; - uint16_t samplesTotal = 0; - float cx[4] = {0.0}; - float cy[4] = {0.0}; - float cz[4] = {0.0}; - bool hasCoeffs = false; - - GyroTemperatureCalibrationConfig(SlimeVR::Configuration::SensorConfigType _type, float _sensitivityLSB) : - type(_type), - sensitivityLSB(_sensitivityLSB), - minTemperatureRange(1000), - maxTemperatureRange(-1000) - { } - - bool hasData() { - return minTemperatureRange != 1000; - } - - bool fullyCalibrated() { - return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs; - } - - float getCalibrationDonePercent() { - return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f; - } - - void rescaleSamples(float newSensitivityLSB) { - if (sensitivityLSB == newSensitivityLSB) return; - float mul = newSensitivityLSB / sensitivityLSB; - for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { - if (samples[i].t == 0) continue; - samples[i].x *= mul; - samples[i].y *= mul; - samples[i].z *= mul; - } - sensitivityLSB = newSensitivityLSB; - } - - void reset() { - minTemperatureRange = 1000; - maxTemperatureRange = -1000; - samplesTotal = 0; - for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { - samples[i].t = 0; - samples[i].x = 0; - samples[i].y = 0; - samples[i].z = 0; - } - hasCoeffs = false; - } + SlimeVR::Configuration::SensorConfigType type; + + float sensitivityLSB; + float minTemperatureRange; + float maxTemperatureRange; + uint16_t minCalibratedIdx = 0; + uint16_t maxCalibratedIdx = 0; + GyroTemperatureOffsetSample samples[TEMP_CALIBRATION_BUFFER_SIZE]; + uint16_t samplesTotal = 0; + float cx[4] = {0.0}; + float cy[4] = {0.0}; + float cz[4] = {0.0}; + bool hasCoeffs = false; + + GyroTemperatureCalibrationConfig( + SlimeVR::Configuration::SensorConfigType _type, + float _sensitivityLSB + ) + : type(_type) + , sensitivityLSB(_sensitivityLSB) + , minTemperatureRange(1000) + , maxTemperatureRange(-1000) {} + + bool hasData() { return minTemperatureRange != 1000; } + + bool fullyCalibrated() { + return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs; + } + + float getCalibrationDonePercent() { + return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f; + } + + void rescaleSamples(float newSensitivityLSB) { + if (sensitivityLSB == newSensitivityLSB) { + return; + } + float mul = newSensitivityLSB / sensitivityLSB; + for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { + if (samples[i].t == 0) { + continue; + } + samples[i].x *= mul; + samples[i].y *= mul; + samples[i].z *= mul; + } + sensitivityLSB = newSensitivityLSB; + } + + void reset() { + minTemperatureRange = 1000; + maxTemperatureRange = -1000; + samplesTotal = 0; + for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { + samples[i].t = 0; + samples[i].x = 0; + samples[i].y = 0; + samples[i].z = 0; + } + hasCoeffs = false; + } }; class GyroTemperatureCalibrator { public: - uint8_t sensorId; - GyroTemperatureCalibrationConfig config; - - // set when config is fully calibrated is saved OR on startup when loaded config is fully calibrated; - // left unset when sending saving command over serial so it can continue calibration and autosave later - bool configSaved = false; - bool configSaveFailed = false; - - GyroTemperatureCalibrator(SlimeVR::Configuration::SensorConfigType _configType, uint8_t _sensorId, float sensitivity, uint32_t _samplesPerStep): - sensorId(_sensorId), - config(_configType, sensitivity), - samplesPerStep(_samplesPerStep), - m_Logger(SlimeVR::Logging::Logger("GyroTemperatureCalibrator")) - { - char buf[4]; - sprintf(buf, "%u", _sensorId); - m_Logger.setTag(buf); - } - - void updateGyroTemperatureCalibration(const float temperature, const bool restDetected, int16_t x, int16_t y, int16_t z); - bool approximateOffset(const float temperature, float GOxyz[3]); - bool loadConfig(float newSensitivity); - bool saveConfig(); - - void reset() { - config.reset(); - configSaved = false; - configSaveFailed = false; - } - - bool isCalibrating() { - return calibrationRunning; - } + uint8_t sensorId; + GyroTemperatureCalibrationConfig config; + + // set when config is fully calibrated is saved OR on startup when loaded config is + // fully calibrated; left unset when sending saving command over serial so it can + // continue calibration and autosave later + bool configSaved = false; + bool configSaveFailed = false; + + GyroTemperatureCalibrator( + SlimeVR::Configuration::SensorConfigType _configType, + uint8_t _sensorId, + float sensitivity, + uint32_t _samplesPerStep + ) + : sensorId(_sensorId) + , config(_configType, sensitivity) + , samplesPerStep(_samplesPerStep) + , m_Logger(SlimeVR::Logging::Logger("GyroTemperatureCalibrator")) { + char buf[4]; + sprintf(buf, "%u", _sensorId); + m_Logger.setTag(buf); + } + + void updateGyroTemperatureCalibration( + const float temperature, + const bool restDetected, + int16_t x, + int16_t y, + int16_t z + ); + bool approximateOffset(const float temperature, float GOxyz[3]); + bool loadConfig(float newSensitivity); + bool saveConfig(); + + void reset() { + config.reset(); + configSaved = false; + configSaveFailed = false; + } + + bool isCalibrating() { return calibrationRunning; } private: - GyroTemperatureCalibrationState state; - uint32_t samplesPerStep; - SlimeVR::Logging::Logger m_Logger; - - float lastApproximatedTemperature = 0.0f; - float lastApproximatedOffsets[3]; - - bool calibrationRunning = false; - OnlineVectorPolyfit<3, 3, (uint64_t)1e9> poly; - float bst = 0.0f; - int32_t bsx = 0; - int32_t bsy = 0; - int32_t bsz = 0; - int32_t bn = 0; - float lastTemp = 0; - - void resetCurrentTemperatureState(); + GyroTemperatureCalibrationState state; + uint32_t samplesPerStep; + SlimeVR::Logging::Logger m_Logger; + + float lastApproximatedTemperature = 0.0f; + float lastApproximatedOffsets[3]; + + bool calibrationRunning = false; + OnlineVectorPolyfit<3, 3, (uint64_t)1e9> poly; + float bst = 0.0f; + int32_t bsx = 0; + int32_t bsy = 0; + int32_t bsz = 0; + int32_t bn = 0; + float lastTemp = 0; + + void resetCurrentTemperatureState(); }; #endif diff --git a/src/motionprocessing/OnlinePolyfit.h b/src/motionprocessing/OnlinePolyfit.h index 8ee7119b6..0c8ccd930 100644 --- a/src/motionprocessing/OnlinePolyfit.h +++ b/src/motionprocessing/OnlinePolyfit.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include @@ -28,98 +28,103 @@ template class OnlineVectorPolyfit { public: - constexpr static int32_t numDimensions = dimensions; - constexpr static int32_t numCoefficients = degree + 1; - constexpr static double forgettingFactor = std::exp(-1.0/forgettingFactorNumSamples); - - OnlineVectorPolyfit() { - reset(); - } - - void reset() { - std::fill(Rb[0], Rb[0] + rows * cols, 0.0); - std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f); - } - - // Recursive least squares update using QR decomposition by Givens transformations - void update(double xValue, const double yValues[numDimensions]) { - double xin[cols]; - xin[0] = 1; - for (int32_t i = 1; i < cols - numDimensions; i++) { - xin[i] = xin[i - 1] * xValue; - } - for (int32_t i = 0; i < numDimensions; i++) { - xin[cols - numDimensions + i] = yValues[i]; - } - - // degree = 3, dimensions = 3, yValues = [x, y, z] - // B I I I Ix Iy Iz R R R R bx by bz - // . B I I Ix Iy Iz === . R R R bx by bz - // . . B I Ix Iy Iz === . . R R bx by bz - // . . . B Ix Iy Iz . . . R bx by bz - - // https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf - - for (int32_t y = 0; y < rows; y++) { - double c = 1, s = 0; - if (xin[y] != 0.0) { - Rb[y][y] *= forgettingFactor; - const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]); - c = Rb[y][y] * (1.0 / norm); - s = xin[y] * (1.0 / norm); - Rb[y][y] = norm; - } - for (int32_t x = y + 1; x < cols; x++) { - Rb[y][x] *= forgettingFactor; - const double xout = (c * xin[x] - s * Rb[y][x]); - Rb[y][x] = (s * xin[x] + c * Rb[y][x]); - xin[x] = xout; - } - } - } - - // Back solves upper triangular system - // Returns float[numDimensions][numCoefficients] coefficients from lowest to highest power - const auto computeCoefficients() { - // https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution - for (int32_t d = 0; d < numDimensions; d++) { - for (int32_t y = rows - 1; y >= 0; y--) { - const int32_t bColumn = cols - numDimensions + d; - coeffs[d][y] = Rb[y][bColumn]; - if (Rb[y][y] == 0.0) continue; - for (int32_t x = y + 1; x < rows; x++) { - coeffs[d][y] -= coeffs[d][x] * Rb[y][x]; - } - coeffs[d][y] /= Rb[y][y]; - } - } - return coeffs; - } - - float predict(int32_t d, float x) { - if (d >= numDimensions) return 0.0; - // https://en.wikipedia.org/wiki/Horner%27s_method - float y = coeffs[d][numCoefficients - 1]; - for (int32_t i = numCoefficients - 2; i >= 0; i--) { - y = y * x + coeffs[d][i]; - } - return y; - } - - std::pair 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); - } + constexpr static int32_t numDimensions = dimensions; + constexpr static int32_t numCoefficients = degree + 1; + constexpr static double forgettingFactor + = std::exp(-1.0 / forgettingFactorNumSamples); + + OnlineVectorPolyfit() { reset(); } + + void reset() { + std::fill(Rb[0], Rb[0] + rows * cols, 0.0); + std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f); + } + + // Recursive least squares update using QR decomposition by Givens transformations + void update(double xValue, const double yValues[numDimensions]) { + double xin[cols]; + xin[0] = 1; + for (int32_t i = 1; i < cols - numDimensions; i++) { + xin[i] = xin[i - 1] * xValue; + } + for (int32_t i = 0; i < numDimensions; i++) { + xin[cols - numDimensions + i] = yValues[i]; + } + + // degree = 3, dimensions = 3, yValues = [x, y, z] + // B I I I Ix Iy Iz R R R R bx by bz + // . B I I Ix Iy Iz === . R R R bx by bz + // . . B I Ix Iy Iz === . . R R bx by bz + // . . . B Ix Iy Iz . . . R bx by bz + + // https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf + + for (int32_t y = 0; y < rows; y++) { + double c = 1, s = 0; + if (xin[y] != 0.0) { + Rb[y][y] *= forgettingFactor; + const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]); + c = Rb[y][y] * (1.0 / norm); + s = xin[y] * (1.0 / norm); + Rb[y][y] = norm; + } + for (int32_t x = y + 1; x < cols; x++) { + Rb[y][x] *= forgettingFactor; + const double xout = (c * xin[x] - s * Rb[y][x]); + Rb[y][x] = (s * xin[x] + c * Rb[y][x]); + xin[x] = xout; + } + } + } + + // Back solves upper triangular system + // Returns float[numDimensions][numCoefficients] coefficients from lowest to highest + // power + const auto computeCoefficients() { + // https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution + for (int32_t d = 0; d < numDimensions; d++) { + for (int32_t y = rows - 1; y >= 0; y--) { + const int32_t bColumn = cols - numDimensions + d; + coeffs[d][y] = Rb[y][bColumn]; + if (Rb[y][y] == 0.0) { + continue; + } + for (int32_t x = y + 1; x < rows; x++) { + coeffs[d][y] -= coeffs[d][x] * Rb[y][x]; + } + coeffs[d][y] /= Rb[y][y]; + } + } + return coeffs; + } + + float predict(int32_t d, float x) { + if (d >= numDimensions) { + return 0.0; + } + // https://en.wikipedia.org/wiki/Horner%27s_method + float y = coeffs[d][numCoefficients - 1]; + for (int32_t i = numCoefficients - 2; i >= 0; i--) { + y = y * x + coeffs[d][i]; + } + return y; + } + + std::pair tangentAt(float x) { + float intercept = coeffs[0]; + float slope = coeffs[1]; + for (uint32_t i = 2; i < degree + 1; i++) { + intercept -= coeffs[i] * (i - 1) * pow(x, i); + slope += coeffs[i] * i * pow(x, i - 1); + } + return std::make_pair(slope, intercept); + } + private: - constexpr static int32_t rows = numCoefficients; - constexpr static int32_t cols = numCoefficients + 3; - double Rb[rows][cols]; - float coeffs[numDimensions][rows]; + constexpr static int32_t rows = numCoefficients; + constexpr static int32_t cols = numCoefficients + 3; + double Rb[rows][cols]; + float coeffs[numDimensions][rows]; }; #endif \ No newline at end of file diff --git a/src/motionprocessing/RestDetection.h b/src/motionprocessing/RestDetection.h index 94d754532..33b98260d 100644 --- a/src/motionprocessing/RestDetection.h +++ b/src/motionprocessing/RestDetection.h @@ -11,252 +11,281 @@ // #define REST_DETECTION_DISABLE_LPF #include -#include #include +#include + #include "types.h" #define NaN std::numeric_limits::quiet_NaN() struct RestDetectionParams { - sensor_real_t biasClip; - sensor_real_t biasSigmaRest; - sensor_real_t restMinTime; - sensor_real_t restFilterTau; - sensor_real_t restThGyr; - sensor_real_t restThAcc; - RestDetectionParams(): - biasClip(2.0f), - biasSigmaRest(0.03f), - restMinTime(1.5), - restFilterTau(0.5f), - restThGyr(2.0f), - restThAcc(0.5f) - { } + sensor_real_t biasClip; + sensor_real_t biasSigmaRest; + sensor_real_t restMinTime; + sensor_real_t restFilterTau; + sensor_real_t restThGyr; + sensor_real_t restThAcc; + RestDetectionParams() + : biasClip(2.0f) + , biasSigmaRest(0.03f) + , restMinTime(1.5) + , restFilterTau(0.5f) + , restThGyr(2.0f) + , restThAcc(0.5f) {} }; inline sensor_real_t square(sensor_real_t x) { return x * x; } class RestDetection { public: - RestDetection(sensor_real_t gyrTs, sensor_real_t accTs) { - this->gyrTs = gyrTs; - this->accTs = accTs; - setup(); - } - RestDetection(const RestDetectionParams ¶ms, sensor_real_t gyrTs, sensor_real_t accTs) { - this->params = params; - this->gyrTs = gyrTs; - this->accTs = accTs; - setup(); - } + RestDetection(sensor_real_t gyrTs, sensor_real_t accTs) { + this->gyrTs = gyrTs; + this->accTs = accTs; + setup(); + } + RestDetection( + const RestDetectionParams& params, + sensor_real_t gyrTs, + sensor_real_t accTs + ) { + this->params = params; + this->gyrTs = gyrTs; + this->accTs = accTs; + setup(); + } #ifndef REST_DETECTION_DISABLE_LPF - void filterInitialState(sensor_real_t x0, const double b[3], const double a[2], double out[]) - { - // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter - // update equation) - out[0] = x0*(1 - b[0]); - out[1] = x0*(b[2] - a[1]); - } - - sensor_real_t filterStep(sensor_real_t x, const double b[3], const double a[2], double state[2]) - { - // difference equations based on scipy.signal.lfilter documentation - // assumes that a0 == 1.0 - double y = b[0]*x + state[0]; - state[0] = b[1]*x - a[0]*y + state[1]; - state[1] = b[2]*x - a[1]*y; - return y; - } - - - void filterVec(const sensor_real_t x[], size_t N, sensor_real_t tau, sensor_real_t Ts, const double b[3], - const double a[2], double state[], sensor_real_t out[]) - { - assert(N>=2); - - // to avoid depending on a single sample, average the first samples (for duration tau) - // and then use this average to calculate the filter initial state - if (isnan(state[0])) { // initialization phase - if (isnan(state[1])) { // first sample - state[1] = 0; // state[1] is used to store the sample count - for(size_t i = 0; i < N; i++) { - state[2+i] = 0; // state[2+i] is used to store the sum - } - } - state[1]++; - for (size_t i = 0; i < N; i++) { - state[2+i] += x[i]; - out[i] = state[2+i]/state[1]; - } - if (state[1]*Ts >= tau) { - for(size_t i = 0; i < N; i++) { - filterInitialState(out[i], b, a, state+2*i); - } - } - return; - } - - for (size_t i = 0; i < N; i++) { - out[i] = filterStep(x[i], b, a, state+2*i); - } - } + void filterInitialState( + sensor_real_t x0, + const double b[3], + const double a[2], + double out[] + ) { + // initial state for steady state (equivalent to scipy.signal.lfilter_zi, + // obtained by setting y=x=x0 in the filter update equation) + out[0] = x0 * (1 - b[0]); + out[1] = x0 * (b[2] - a[1]); + } + + sensor_real_t + filterStep(sensor_real_t x, const double b[3], const double a[2], double state[2]) { + // difference equations based on scipy.signal.lfilter documentation + // assumes that a0 == 1.0 + double y = b[0] * x + state[0]; + state[0] = b[1] * x - a[0] * y + state[1]; + state[1] = b[2] * x - a[1] * y; + return y; + } + + void filterVec( + const sensor_real_t x[], + size_t N, + sensor_real_t tau, + sensor_real_t Ts, + const double b[3], + const double a[2], + double state[], + sensor_real_t out[] + ) { + assert(N >= 2); + + // to avoid depending on a single sample, average the first samples (for + // duration tau) and then use this average to calculate the filter initial state + if (isnan(state[0])) { // initialization phase + if (isnan(state[1])) { // first sample + state[1] = 0; // state[1] is used to store the sample count + for (size_t i = 0; i < N; i++) { + state[2 + i] = 0; // state[2+i] is used to store the sum + } + } + state[1]++; + for (size_t i = 0; i < N; i++) { + state[2 + i] += x[i]; + out[i] = state[2 + i] / state[1]; + } + if (state[1] * Ts >= tau) { + for (size_t i = 0; i < N; i++) { + filterInitialState(out[i], b, a, state + 2 * i); + } + } + return; + } + + for (size_t i = 0; i < N; i++) { + out[i] = filterStep(x[i], b, a, state + 2 * i); + } + } #endif - void updateGyr(const sensor_real_t gyr[3]) { + void updateGyr(const sensor_real_t gyr[3]) { #ifdef REST_DETECTION_DISABLE_LPF - gyrLastSquaredDeviation = - square(gyr[0] - lastSample.gyr[0]) + - square(gyr[1] - lastSample.gyr[1]) + - square(gyr[2] - lastSample.gyr[2]); - - sensor_real_t biasClip = params.biasClip*sensor_real_t(M_PI/180.0); - if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0)) - || fabs(lastSample.gyr[0]) > biasClip || fabs(lastSample.gyr[1]) > biasClip - || fabs(lastSample.gyr[2]) > biasClip) { - restTime = 0; - restDetected = false; - } - - lastSample.gyr[0] = gyr[0]; - lastSample.gyr[1] = gyr[1]; - lastSample.gyr[2] = gyr[2]; + gyrLastSquaredDeviation = square(gyr[0] - lastSample.gyr[0]) + + square(gyr[1] - lastSample.gyr[1]) + + square(gyr[2] - lastSample.gyr[2]); + + sensor_real_t biasClip = params.biasClip * sensor_real_t(M_PI / 180.0); + if (gyrLastSquaredDeviation + >= square(params.restThGyr * sensor_real_t(M_PI / 180.0)) + || fabs(lastSample.gyr[0]) > biasClip || fabs(lastSample.gyr[1]) > biasClip + || fabs(lastSample.gyr[2]) > biasClip) { + restTime = 0; + restDetected = false; + } + + lastSample.gyr[0] = gyr[0]; + lastSample.gyr[1] = gyr[1]; + lastSample.gyr[2] = gyr[2]; #else - filterVec(gyr, 3, params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA, - restGyrLpState, restLastGyrLp); - - gyrLastSquaredDeviation = - square(gyr[0] - restLastGyrLp[0]) + - square(gyr[1] - restLastGyrLp[1]) + - square(gyr[2] - restLastGyrLp[2]); - - sensor_real_t biasClip = params.biasClip*sensor_real_t(M_PI/180.0); - if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0)) - || fabs(restLastGyrLp[0]) > biasClip || fabs(restLastGyrLp[1]) > biasClip - || fabs(restLastGyrLp[2]) > biasClip) { - restTime = 0; - restDetected = false; - } + filterVec( + gyr, + 3, + params.restFilterTau, + gyrTs, + restGyrLpB, + restGyrLpA, + restGyrLpState, + restLastGyrLp + ); + + gyrLastSquaredDeviation = square(gyr[0] - restLastGyrLp[0]) + + square(gyr[1] - restLastGyrLp[1]) + + square(gyr[2] - restLastGyrLp[2]); + + sensor_real_t biasClip = params.biasClip * sensor_real_t(M_PI / 180.0); + if (gyrLastSquaredDeviation + >= square(params.restThGyr * sensor_real_t(M_PI / 180.0)) + || fabs(restLastGyrLp[0]) > biasClip || fabs(restLastGyrLp[1]) > biasClip + || fabs(restLastGyrLp[2]) > biasClip) { + restTime = 0; + restDetected = false; + } #endif - } + } - void updateAcc(sensor_real_t dt, const sensor_real_t acc[3]) { - if (acc[0] == sensor_real_t(0.0) && acc[1] == sensor_real_t(0.0) && acc[2] == sensor_real_t(0.0)) { - return; - } + void updateAcc(sensor_real_t dt, const sensor_real_t acc[3]) { + if (acc[0] == sensor_real_t(0.0) && acc[1] == sensor_real_t(0.0) + && acc[2] == sensor_real_t(0.0)) { + return; + } #ifdef REST_DETECTION_DISABLE_LPF - accLastSquaredDeviation = - square(acc[0] - lastSample.acc[0]) + - square(acc[1] - lastSample.acc[1]) + - square(acc[2] - lastSample.acc[2]); - - if (accLastSquaredDeviation >= square(params.restThAcc)) { - restTime = 0; - restDetected = false; - } else { - restTime += dt; - if (restTime >= params.restMinTime) { - restDetected = true; - } - } - - lastSample.acc[0] = acc[0]; - lastSample.acc[1] = acc[1]; - lastSample.acc[2] = acc[2]; + accLastSquaredDeviation = square(acc[0] - lastSample.acc[0]) + + square(acc[1] - lastSample.acc[1]) + + square(acc[2] - lastSample.acc[2]); + + if (accLastSquaredDeviation >= square(params.restThAcc)) { + restTime = 0; + restDetected = false; + } else { + restTime += dt; + if (restTime >= params.restMinTime) { + restDetected = true; + } + } + + lastSample.acc[0] = acc[0]; + lastSample.acc[1] = acc[1]; + lastSample.acc[2] = acc[2]; #else - filterVec(acc, 3, params.restFilterTau, accTs, restAccLpB, restAccLpA, - restAccLpState, restLastAccLp); - - accLastSquaredDeviation = - square(acc[0] - restLastAccLp[0]) + - square(acc[1] - restLastAccLp[1]) + - square(acc[2] - restLastAccLp[2]); - - if (accLastSquaredDeviation >= square(params.restThAcc)) { - restTime = 0; - restDetected = false; - } else { - restTime += dt; - if (restTime >= params.restMinTime) { - restDetected = true; - } - } + filterVec( + acc, + 3, + params.restFilterTau, + accTs, + restAccLpB, + restAccLpA, + restAccLpState, + restLastAccLp + ); + + accLastSquaredDeviation = square(acc[0] - restLastAccLp[0]) + + square(acc[1] - restLastAccLp[1]) + + square(acc[2] - restLastAccLp[2]); + + if (accLastSquaredDeviation >= square(params.restThAcc)) { + restTime = 0; + restDetected = false; + } else { + restTime += dt; + if (restTime >= params.restMinTime) { + restDetected = true; + } + } #endif - } + } - bool getRestDetected() { - return restDetected; - } + bool getRestDetected() { return restDetected; } #ifndef REST_DETECTION_DISABLE_LPF - void resetState() { - restDetected = false; - - gyrLastSquaredDeviation = 0.0; - accLastSquaredDeviation = 0.0; - restTime = 0.0; - std::fill(restLastGyrLp, restLastGyrLp + 3, 0.0); - std::fill(restGyrLpState, restGyrLpState + 3*2, NaN); - std::fill(restLastAccLp, restLastAccLp + 3, 0.0); - std::fill(restAccLpState, restAccLpState + 3*2, NaN); - } - - void filterCoeffs(sensor_real_t tau, sensor_real_t Ts, double outB[], double outA[]) { - assert(tau > 0); - assert(Ts > 0); - // second order Butterworth filter based on https://stackoverflow.com/a/52764064 - double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response - double C = tan(M_PI*fc*double(Ts)); - double D = C*C + sqrt(2)*C + 1; - double b0 = C*C/D; - outB[0] = b0; - outB[1] = 2*b0; - outB[2] = b0; - // a0 = 1.0 - outA[0] = 2*(C*C-1)/D; // a1 - outA[1] = (1-sqrt(2)*C+C*C)/D; // a2 - } + void resetState() { + restDetected = false; + + gyrLastSquaredDeviation = 0.0; + accLastSquaredDeviation = 0.0; + restTime = 0.0; + std::fill(restLastGyrLp, restLastGyrLp + 3, 0.0); + std::fill(restGyrLpState, restGyrLpState + 3 * 2, NaN); + std::fill(restLastAccLp, restLastAccLp + 3, 0.0); + std::fill(restAccLpState, restAccLpState + 3 * 2, NaN); + } + + void + filterCoeffs(sensor_real_t tau, sensor_real_t Ts, double outB[], double outA[]) { + assert(tau > 0); + assert(Ts > 0); + // second order Butterworth filter based on https://stackoverflow.com/a/52764064 + double fc + = (M_SQRT2 / (2.0 * M_PI)) + / double(tau + ); // time constant of dampened, non-oscillating part of step response + double C = tan(M_PI * fc * double(Ts)); + double D = C * C + sqrt(2) * C + 1; + double b0 = C * C / D; + outB[0] = b0; + outB[1] = 2 * b0; + outB[2] = b0; + // a0 = 1.0 + outA[0] = 2 * (C * C - 1) / D; // a1 + outA[1] = (1 - sqrt(2) * C + C * C) / D; // a2 + } #endif - void setup() { + void setup() { #ifndef REST_DETECTION_DISABLE_LPF - assert(gyrTs > 0); - assert(accTs > 0); + assert(gyrTs > 0); + assert(accTs > 0); - filterCoeffs(params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA); - filterCoeffs(params.restFilterTau, accTs, restAccLpB, restAccLpA); + filterCoeffs(params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA); + filterCoeffs(params.restFilterTau, accTs, restAccLpB, restAccLpA); - resetState(); + resetState(); #endif - } - + } private: - RestDetectionParams params; - bool restDetected; - sensor_real_t restTime; - sensor_real_t gyrLastSquaredDeviation = 0; - sensor_real_t accLastSquaredDeviation = 0; - - sensor_real_t gyrTs; - sensor_real_t accTs; + RestDetectionParams params; + bool restDetected; + sensor_real_t restTime; + sensor_real_t gyrLastSquaredDeviation = 0; + sensor_real_t accLastSquaredDeviation = 0; + + sensor_real_t gyrTs; + sensor_real_t accTs; #ifndef REST_DETECTION_DISABLE_LPF - sensor_real_t restLastGyrLp[3]; - double restGyrLpState[3*2]; - double restGyrLpB[3]; - double restGyrLpA[2]; - sensor_real_t restLastAccLp[3]; - double restAccLpState[3*2]; - double restAccLpB[3]; - double restAccLpA[2]; + sensor_real_t restLastGyrLp[3]; + double restGyrLpState[3 * 2]; + double restGyrLpB[3]; + double restGyrLpA[2]; + sensor_real_t restLastAccLp[3]; + double restAccLpState[3 * 2]; + double restAccLpB[3]; + double restAccLpA[2]; #else - struct { - float gyr[3]; - float acc[3]; - } lastSample; + struct { + float gyr[3]; + float acc[3]; + } lastSample; #endif }; - #endif \ No newline at end of file diff --git a/src/motionprocessing/types.h b/src/motionprocessing/types.h index e3953a14e..dbcd9141f 100644 --- a/src/motionprocessing/types.h +++ b/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; - #define VQF_SINGLE_PRECISION +typedef float sensor_real_t; +#define VQF_SINGLE_PRECISION #endif #endif diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 8796be775..36dec41bc 100644 --- a/src/network/connection.cpp +++ b/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) { - return write(&byte, 1); -} +size_t Connection::write(uint8_t byte) { 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> & sensors) { +void Connection::updateSensorState(std::vector>& 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 (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) { - m_Logger.debug("Server supports packet bundling"); - } - #endif +#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED + if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) { + m_Logger.debug("Server supports packet bundling"); + } +#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(); - if(sensorId < sensors.size()) { + auto& sensors = sensorManager.getSensors(); + 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; } } diff --git a/src/network/connection.h b/src/network/connection.h index c20231691..1b4240024 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -26,11 +26,11 @@ #include #include +#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 @@ class Connection { ); #endif - const ServerFeatures& getServerFeatureFlags() { - return m_ServerFeatures; - } + const ServerFeatures& getServerFeatureFlags() { return m_ServerFeatures; } bool beginBundle(); bool endBundle(); private: - void updateSensorState(std::vector> & sensors); + void updateSensorState(std::vector>& 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); diff --git a/src/network/featureflags.h b/src/network/featureflags.h index f9e1dfa1f..bc917ba2e 100644 --- a/src/network/featureflags.h +++ b/src/network/featureflags.h @@ -24,17 +24,17 @@ #ifndef SLIMEVR_FEATURE_FLAGS_H_ #define SLIMEVR_FEATURE_FLAGS_H_ -#include #include +#include /** * 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 @@ struct ServerFeatures { /** * Whether the server supports the "feature flags" feature, * set to true when we've received flags packet from the server. - */ - bool isAvailable() { - return m_Available; - } + */ + bool isAvailable() { return m_Available; } static ServerFeatures from(uint8_t* received, uint32_t length) { ServerFeatures res; res.m_Available = true; - memcpy(res.m_Flags, received, std::min(static_cast(sizeof(res.m_Flags)), length)); + memcpy( + res.m_Flags, + received, + std::min(static_cast(sizeof(res.m_Flags)), length) + ); return res; } @@ -71,7 +73,7 @@ struct ServerFeatures { class FirmwareFeatures { public: - enum EFirmwareFeatureFlags: uint32_t { + enum EFirmwareFeatureFlags : uint32_t { // EXAMPLE_FEATURE, B64_WIFI_SCANNING = 1, SENSOR_CONFIG = 2, @@ -88,7 +90,7 @@ class FirmwareFeatures { // Add enabled flags here }; - static constexpr auto flags = []{ + static constexpr auto flags = [] { constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1; std::array packed{}; diff --git a/src/network/packets.h b/src/network/packets.h index 47978042d..f6c3d9b53 100644 --- a/src/network/packets.h +++ b/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 diff --git a/src/network/wifihandler.cpp b/src/network/wifihandler.cpp index 4119c93c9..2bf2984ea 100644 --- a/src/network/wifihandler.cpp +++ b/src/network/wifihandler.cpp @@ -1,28 +1,28 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ +#include "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()) { - lastWifiReportTime = millis(); - Serial.print("."); - } + if (lastWifiReportTime + 1000 < millis()) { + lastWifiReportTime = millis(); + Serial.print("."); + } } void setStaticIPIfDefined() { - #ifdef WIFI_USE_STATICIP - const IPAddress ip(WIFI_STATIC_IP); - const IPAddress gateway(WIFI_STATIC_GATEWAY); - const IPAddress subnet(WIFI_STATIC_SUBNET); - WiFi.config(ip, gateway, subnet); - #endif +#ifdef WIFI_USE_STATICIP + const IPAddress ip(WIFI_STATIC_IP); + const IPAddress gateway(WIFI_STATIC_GATEWAY); + const IPAddress subnet(WIFI_STATIC_SUBNET); + WiFi.config(ip, gateway, subnet); +#endif } -bool WiFiNetwork::isConnected() { - return isWifiConnected; -} +bool WiFiNetwork::isConnected() { 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) { + 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(); -} +IPAddress WiFiNetwork::getAddress() { return WiFi.localIP(); } void WiFiNetwork::setUp() { - wifiHandlerLogger.info("Setting up WiFi"); - WiFi.persistent(true); - WiFi.mode(WIFI_STA); - #if ESP8266 - #if USE_ATTENUATION - WiFi.setOutputPower(20.0 - ATTENUATION_N); - #endif - WiFi.setPhyMode(WIFI_PHY_MODE_11N); - #endif - WiFi.hostname("SlimeVR FBT Tracker"); - wifiHandlerLogger.info("Loaded credentials for SSID %s and pass length %d", WiFi.SSID().c_str(), WiFi.psk().length()); - 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(); + wifiHandlerLogger.info("Setting up WiFi"); + WiFi.persistent(true); + WiFi.mode(WIFI_STA); +#if ESP8266 +#if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_N); +#endif + WiFi.setPhyMode(WIFI_PHY_MODE_11N); +#endif + WiFi.hostname("SlimeVR FBT Tracker"); + wifiHandlerLogger.info( + "Loaded credentials for SSID %s and pass length %d", + WiFi.SSID().c_str(), + WiFi.psk().length() + ); + 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); -#elif POWERSAVING_MODE == POWER_SAVING_MODERATE || POWERSAVING_MODE == POWER_SAVING_MAXIMUM - wifi_config_t conf; - if (esp_wifi_get_config(WIFI_IF_STA, &conf) == ESP_OK) - { - conf.sta.listen_interval = 10; - esp_wifi_set_config(WIFI_IF_STA, &conf); - WiFi.setSleep(WIFI_PS_MAX_MODEM); - } - else - { - wifiHandlerLogger.error("Unable to get WiFi config, power saving not enabled!"); - } + WiFi.setSleep(WIFI_PS_MIN_MODEM); +#elif POWERSAVING_MODE == POWER_SAVING_MODERATE \ + || POWERSAVING_MODE == POWER_SAVING_MAXIMUM + wifi_config_t conf; + if (esp_wifi_get_config(WIFI_IF_STA, &conf) == ESP_OK) { + conf.sta.listen_interval = 10; + esp_wifi_set_config(WIFI_IF_STA, &conf); + WiFi.setSleep(WIFI_PS_MAX_MODEM); + } else { + wifiHandlerLogger.error("Unable to get WiFi config, power saving not enabled!"); + } #endif #endif } void onConnected() { - WiFiNetwork::stopProvisioning(); - statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false); - isWifiConnected = true; - hadWifi = true; - wifiHandlerLogger.info("Connected successfully to SSID '%s', ip address %s", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str()); + WiFiNetwork::stopProvisioning(); + statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false); + isWifiConnected = true; + hadWifi = true; + wifiHandlerLogger.info( + "Connected successfully to SSID '%s', ip address %s", + WiFi.SSID().c_str(), + WiFi.localIP().toString().c_str() + ); } -uint8_t WiFiNetwork::getWiFiState() { - return wifiState; -} +uint8_t WiFiNetwork::getWiFiState() { return wifiState; } void WiFiNetwork::upkeep() { - upkeepProvisioning(); - if(WiFi.status() != WL_CONNECTED) { - if(isWifiConnected) { - wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting..."); - isWifiConnected = false; - } - statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true); - reportWifiError(); - if(wifiConnectionTimeout + 11000 < millis()) { - switch(wifiState) { - case SLIME_WIFI_NOT_SETUP: // Wasn't set up - return; - case SLIME_WIFI_SAVED_ATTEMPT: // Couldn't connect with first set of credentials - #if ESP8266 - // Try again but with 11G - // But only if there are credentials, otherwise we just waste time before - // switching to hardcoded credentials. - if (WiFi.SSID().length() > 0) { - #if USE_ATTENUATION - WiFi.setOutputPower(20.0 - ATTENUATION_G); - #endif - WiFi.setPhyMode(WIFI_PHY_MODE_11G); - setStaticIPIfDefined(); - WiFi.begin(); - wifiConnectionTimeout = millis(); - wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status()); - wifiHandlerLogger.debug("Trying saved credentials with PHY Mode G..."); - } else { - wifiHandlerLogger.debug("Skipping PHY Mode G attempt on 0-length SSID..."); - } - #endif - wifiState = SLIME_WIFI_SAVED_G_ATTEMPT; - return; - case SLIME_WIFI_SAVED_G_ATTEMPT: // Couldn't connect with first set of credentials with PHY Mode G - #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) - // Try hardcoded credentials now - #if ESP8266 - #if USE_ATTENUATION - WiFi.setOutputPower(20.0 - ATTENUATION_N); - #endif - WiFi.setPhyMode(WIFI_PHY_MODE_11N); - #endif - setStaticIPIfDefined(); - WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD); - wifiConnectionTimeout = millis(); - wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status()); - wifiHandlerLogger.debug("Trying hardcoded credentials..."); - #endif - wifiState = SLIME_WIFI_HARDCODE_ATTEMPT; - return; - case SLIME_WIFI_HARDCODE_ATTEMPT: // Couldn't connect with second set of credentials - #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266 - // Try hardcoded credentials again, but with PHY Mode G - #if USE_ATTENUATION - WiFi.setOutputPower(20.0 - ATTENUATION_G); - #endif - WiFi.setPhyMode(WIFI_PHY_MODE_11G); - setStaticIPIfDefined(); - WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD); - wifiConnectionTimeout = millis(); - wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status()); - wifiHandlerLogger.debug("Trying hardcoded credentials with WiFi PHY Mode G..."); - #endif - wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT; - return; - case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with server-sent credentials. - #if ESP8266 - // Try again silently but with 11G - #if USE_ATTENUATION - WiFi.setOutputPower(20.0 - ATTENUATION_G); - #endif - WiFi.setPhyMode(WIFI_PHY_MODE_11G); - setStaticIPIfDefined(); - WiFi.begin(); - wifiConnectionTimeout = millis(); - wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT; - #endif - return; - case SLIME_WIFI_HARDCODE_G_ATTEMPT: // Couldn't connect with second set of credentials with PHY Mode G. - case SLIME_WIFI_SERVER_CRED_G_ATTEMPT: // Or if couldn't connect with server-sent credentials - // Return to the default PHY Mode N. - #if ESP8266 - #if USE_ATTENUATION - WiFi.setOutputPower(20.0 - ATTENUATION_N); - #endif - WiFi.setPhyMode(WIFI_PHY_MODE_11N); - #endif - // Start smart config - if(!hadWifi && !WiFi.smartConfigDone() && wifiConnectionTimeout + 11000 < millis()) { - if(WiFi.status() != WL_IDLE_STATUS) { - wifiHandlerLogger.error("Can't connect from any credentials, status: %d.", WiFi.status()); - wifiConnectionTimeout = millis(); - } - startProvisioning(); - } - return; - } - } - return; - } - if(!isWifiConnected) { - onConnected(); - return; - } else { - if(millis() - last_rssi_sample >= 2000) { - last_rssi_sample = millis(); - uint8_t signalStrength = WiFi.RSSI(); - networkConnection.sendSignalStrength(signalStrength); - } - } - return; + upkeepProvisioning(); + if (WiFi.status() != WL_CONNECTED) { + if (isWifiConnected) { + wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting..."); + isWifiConnected = false; + } + statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true); + reportWifiError(); + if (wifiConnectionTimeout + 11000 < millis()) { + switch (wifiState) { + case SLIME_WIFI_NOT_SETUP: // Wasn't set up + return; + case SLIME_WIFI_SAVED_ATTEMPT: // Couldn't connect with first set of + // credentials +#if ESP8266 + // Try again but with 11G but only if there are credentials, + // otherwise we just waste time before switching to hardcoded + // credentials. + if (WiFi.SSID().length() > 0) { +#if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_G); +#endif + WiFi.setPhyMode(WIFI_PHY_MODE_11G); + setStaticIPIfDefined(); + WiFi.begin(); + wifiConnectionTimeout = millis(); + wifiHandlerLogger.error( + "Can't connect from saved credentials, status: %d.", + WiFi.status() + ); + wifiHandlerLogger.debug( + "Trying saved credentials with PHY Mode G..." + ); + } else { + wifiHandlerLogger.debug( + "Skipping PHY Mode G attempt on 0-length SSID..." + ); + } +#endif + wifiState = SLIME_WIFI_SAVED_G_ATTEMPT; + return; + case SLIME_WIFI_SAVED_G_ATTEMPT: // Couldn't connect with first set of + // credentials with PHY Mode G +#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) + // Try hardcoded credentials now +#if ESP8266 +#if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_N); +#endif + WiFi.setPhyMode(WIFI_PHY_MODE_11N); +#endif + setStaticIPIfDefined(); + WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD); + wifiConnectionTimeout = millis(); + wifiHandlerLogger.error( + "Can't connect from saved credentials, status: %d.", + WiFi.status() + ); + wifiHandlerLogger.debug("Trying hardcoded credentials..."); +#endif + wifiState = SLIME_WIFI_HARDCODE_ATTEMPT; + return; + case SLIME_WIFI_HARDCODE_ATTEMPT: // Couldn't connect with second set + // of credentials +#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266 + // Try hardcoded credentials again, + // but with PHY Mode G +#if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_G); +#endif + WiFi.setPhyMode(WIFI_PHY_MODE_11G); + setStaticIPIfDefined(); + WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD); + wifiConnectionTimeout = millis(); + wifiHandlerLogger.error( + "Can't connect from saved credentials, status: %d.", + WiFi.status() + ); + wifiHandlerLogger.debug( + "Trying hardcoded credentials with WiFi PHY Mode G..." + ); +#endif + wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT; + return; + case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with + // server-sent credentials. +#if ESP8266 + // Try again silently but with 11G +#if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_G); +#endif + WiFi.setPhyMode(WIFI_PHY_MODE_11G); + setStaticIPIfDefined(); + WiFi.begin(); + wifiConnectionTimeout = millis(); + wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT; +#endif + return; + case SLIME_WIFI_HARDCODE_G_ATTEMPT: // Couldn't connect with second set + // of credentials with PHY Mode G. + case SLIME_WIFI_SERVER_CRED_G_ATTEMPT: // Or if couldn't connect with + // server-sent credentials +// Return to the default PHY Mode N. +#if ESP8266 +#if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_N); +#endif + WiFi.setPhyMode(WIFI_PHY_MODE_11N); +#endif + // Start smart config + if (!hadWifi && !WiFi.smartConfigDone() + && wifiConnectionTimeout + 11000 < millis()) { + if (WiFi.status() != WL_IDLE_STATUS) { + wifiHandlerLogger.error( + "Can't connect from any credentials, status: %d.", + WiFi.status() + ); + wifiConnectionTimeout = millis(); + } + startProvisioning(); + } + return; + } + } + return; + } + if (!isWifiConnected) { + onConnected(); + return; + } else { + if (millis() - last_rssi_sample >= 2000) { + last_rssi_sample = millis(); + uint8_t signalStrength = WiFi.RSSI(); + networkConnection.sendSignalStrength(signalStrength); + } + } + return; } diff --git a/src/network/wifihandler.h b/src/network/wifihandler.h index c1d5244b7..eeb3b3dbf 100644 --- a/src/network/wifihandler.h +++ b/src/network/wifihandler.h @@ -1,52 +1,52 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_WIFI_H_ #define SLIMEVR_WIFI_H_ #ifdef ESP8266 - #include +#include #else - #include +#include #endif namespace WiFiNetwork { - bool isConnected(); - void setUp(); - void upkeep(); - void setWiFiCredentials(const char * SSID, const char * pass); - IPAddress getAddress(); - uint8_t getWiFiState(); -} +bool isConnected(); +void setUp(); +void upkeep(); +void setWiFiCredentials(const char* SSID, const char* pass); +IPAddress getAddress(); +uint8_t getWiFiState(); +} // namespace WiFiNetwork /** Wifi Reconnection Statuses **/ typedef enum { - SLIME_WIFI_NOT_SETUP = 0, - SLIME_WIFI_SAVED_ATTEMPT, - SLIME_WIFI_SAVED_G_ATTEMPT, - SLIME_WIFI_HARDCODE_ATTEMPT, - SLIME_WIFI_HARDCODE_G_ATTEMPT, - SLIME_WIFI_SERVER_CRED_ATTEMPT, - SLIME_WIFI_SERVER_CRED_G_ATTEMPT + SLIME_WIFI_NOT_SETUP = 0, + SLIME_WIFI_SAVED_ATTEMPT, + SLIME_WIFI_SAVED_G_ATTEMPT, + SLIME_WIFI_HARDCODE_ATTEMPT, + SLIME_WIFI_HARDCODE_G_ATTEMPT, + SLIME_WIFI_SERVER_CRED_ATTEMPT, + SLIME_WIFI_SERVER_CRED_G_ATTEMPT } wifi_reconnection_statuses; -#endif // SLIMEVR_WIFI_H_ +#endif // SLIMEVR_WIFI_H_ diff --git a/src/network/wifiprovisioning.cpp b/src/network/wifiprovisioning.cpp index bacf64da6..ad3f954b1 100644 --- a/src/network/wifiprovisioning.cpp +++ b/src/network/wifiprovisioning.cpp @@ -1,28 +1,29 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "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()) { - provisioning = true; - wifiProvisioningLogger.info("SmartConfig started"); - } + if (WiFi.beginSmartConfig()) { + provisioning = true; + wifiProvisioningLogger.info("SmartConfig started"); + } } void WiFiNetwork::stopProvisioning() { - WiFi.stopSmartConfig(); - provisioning = false; + WiFi.stopSmartConfig(); + 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() { - return provisioning && !WiFi.smartConfigDone(); -} +bool WiFiNetwork::isProvisioning() { return provisioning && !WiFi.smartConfigDone(); } diff --git a/src/network/wifiprovisioning.h b/src/network/wifiprovisioning.h index 9fa86cdf1..46ce53cc7 100644 --- a/src/network/wifiprovisioning.h +++ b/src/network/wifiprovisioning.h @@ -1,34 +1,34 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_WIFIPROVISIONING_H_ #define SLIMEVR_WIFIPROVISIONING_H_ namespace WiFiNetwork { - void upkeepProvisioning(); - void startProvisioning(); - void stopProvisioning(); - bool isProvisioning(); - void provideNeighbours(); -} +void upkeepProvisioning(); +void startProvisioning(); +void stopProvisioning(); +bool isProvisioning(); +void provideNeighbours(); +} // namespace WiFiNetwork -#endif // SLIMEVR_WIFIPROVISIONING_H_ +#endif // SLIMEVR_WIFIPROVISIONING_H_ diff --git a/src/sensors/EmptySensor.h b/src/sensors/EmptySensor.h index c5e785f51..5da697990 100644 --- a/src/sensors/EmptySensor.h +++ b/src/sensors/EmptySensor.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_EMPTYSENSOR_H @@ -26,26 +26,23 @@ #include "sensor.h" -namespace SlimeVR -{ - namespace Sensors - { - class EmptySensor : public Sensor - { - public: - EmptySensor(uint8_t id) : Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){}; - ~EmptySensor(){}; - - void motionSetup() override final{}; - void motionLoop() override final{}; - void sendData() override final{}; - void startCalibration(int calibrationType) override final{}; - SensorStatus getSensorState() override final - { - return SensorStatus::SENSOR_OFFLINE; - }; - }; - } -} +namespace SlimeVR { +namespace Sensors { +class EmptySensor : public Sensor { +public: + EmptySensor(uint8_t id) + : Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){}; + ~EmptySensor(){}; + + void motionSetup() override final{}; + void motionLoop() override final{}; + void sendData() override final{}; + void startCalibration(int calibrationType) override final{}; + SensorStatus getSensorState() override final { + return SensorStatus::SENSOR_OFFLINE; + }; +}; +} // namespace Sensors +} // namespace SlimeVR #endif diff --git a/src/sensors/ErroneousSensor.cpp b/src/sensors/ErroneousSensor.cpp index 58bb3ab59..24524f02f 100644 --- a/src/sensors/ErroneousSensor.cpp +++ b/src/sensors/ErroneousSensor.cpp @@ -1,41 +1,39 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "ErroneousSensor.h" -#include "GlobalVars.h" -namespace SlimeVR -{ - namespace Sensors - { - void ErroneousSensor::motionSetup() - { - m_Logger.error("IMU of type %s failed to initialize", getIMUNameByType(m_ExpectedType)); - } +#include "GlobalVars.h" - SensorStatus ErroneousSensor::getSensorState() - { - return SensorStatus::SENSOR_ERROR; - }; - } +namespace SlimeVR { +namespace Sensors { +void ErroneousSensor::motionSetup() { + m_Logger.error( + "IMU of type %s failed to initialize", + getIMUNameByType(m_ExpectedType) + ); } + +SensorStatus ErroneousSensor::getSensorState() { return SensorStatus::SENSOR_ERROR; }; +} // namespace Sensors +} // namespace SlimeVR diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h index 7cd0fc413..e20d0bd64 100644 --- a/src/sensors/ErroneousSensor.h +++ b/src/sensors/ErroneousSensor.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_ERRONEOUSSENSOR_H @@ -26,26 +26,25 @@ #include "sensor.h" -namespace SlimeVR -{ - namespace Sensors - { - class ErroneousSensor : public Sensor - { - public: - ErroneousSensor(uint8_t id, ImuID type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){}; - ~ErroneousSensor(){}; - - void motionSetup() override; - void motionLoop() override final{}; - void sendData() override{}; - void startCalibration(int calibrationType) override final{}; - SensorStatus getSensorState() override final; - - private: - ImuID m_ExpectedType; - }; - } -} +namespace SlimeVR { +namespace Sensors { +class ErroneousSensor : public Sensor { +public: + ErroneousSensor(uint8_t id, ImuID type) + : Sensor("ErroneousSensor", type, id, 0, 0.0) + , m_ExpectedType(type){}; + ~ErroneousSensor(){}; + + void motionSetup() override; + void motionLoop() override final{}; + void sendData() override{}; + void startCalibration(int calibrationType) override final{}; + SensorStatus getSensorState() override final; + +private: + ImuID m_ExpectedType; +}; +} // namespace Sensors +} // namespace SlimeVR #endif diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index 478b9abbf..c608d488f 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -1,181 +1,215 @@ #include "SensorFusion.h" -namespace SlimeVR -{ - 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) - { - 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 SlimeVR { +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 +) { + 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 diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index cc0116205..c60b02f2d 100644 --- a/src/sensors/SensorFusion.h +++ b/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 +#include #include "../motionprocessing/types.h" - -#include "mahony.h" #include "madgwick.h" -#include -#include +#include "mahony.h" + +namespace SlimeVR { +namespace Sensors { +#if SENSOR_USE_VQF +struct SensorVQFParams : VQFParams { + SensorVQFParams() + : VQFParams() { +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + motionBiasEstEnabled = true; +#endif + tauAcc = 2.0f; + restMinT = 2.0f; + restThGyr = 0.6f; // 400 norm + restThAcc = 0.06f; // 100 norm + } +}; +#endif + +class SensorFusion { +public: + SensorFusion( + sensor_real_t gyrTs, + sensor_real_t accTs = -1.0, + sensor_real_t magTs = -1.0 + ) + : gyrTs(gyrTs) + , accTs((accTs < 0) ? gyrTs : accTs) + , magTs((magTs < 0) ? gyrTs : magTs) +#if SENSOR_USE_MAHONY +#elif SENSOR_USE_MADGWICK +#elif SENSOR_USE_BASICVQF + , basicvqf(gyrTs, ((accTs < 0) ? gyrTs : accTs), ((magTs < 0) ? gyrTs : magTs)) +#elif SENSOR_USE_VQF + , vqf(vqfParams, + 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 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(); + void clearUpdated(); + sensor_real_t const* getQuaternion(); + Quat getQuaternionQuat(); + sensor_real_t const* getGravityVec(); + sensor_real_t const* getLinearAcc(); + void getLinearAcc(sensor_real_t outLinAccel[3]); + Vector3 getLinearAccVec(); + + 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] + ); + +protected: + sensor_real_t gyrTs; + sensor_real_t accTs; + sensor_real_t magTs; + +#if SENSOR_USE_MAHONY + Mahony mahony; +#elif SENSOR_USE_MADGWICK + Madgwick madgwick; +#elif SENSOR_USE_BASICVQF + BasicVQF basicvqf; +#elif SENSOR_USE_VQF + SensorVQFParams vqfParams{}; + VQF vqf; +#endif + + // A also used for linear acceleration extraction + sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f}; + +#if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK + // Buffer M here to keep the behavior of BMI160 + sensor_real_t bMxyz[3]{0.0f, 0.0f, 0.0f}; + bool accelUpdated = false; +#endif + + bool magExist = false; + sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f}; + bool updated = false; + + 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 -namespace SlimeVR -{ - namespace Sensors - { - #if SENSOR_USE_VQF - struct SensorVQFParams: VQFParams { - SensorVQFParams() : VQFParams() { - #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - motionBiasEstEnabled = true; - #endif - tauAcc = 2.0f; - restMinT = 2.0f; - restThGyr = 0.6f; // 400 norm - restThAcc = 0.06f; // 100 norm - } - }; - #endif - - class SensorFusion - { - public: - SensorFusion(sensor_real_t gyrTs, sensor_real_t accTs=-1.0, sensor_real_t magTs=-1.0) - : gyrTs(gyrTs), - accTs( (accTs<0) ? gyrTs : accTs ), - magTs( (magTs<0) ? gyrTs : magTs ) - #if SENSOR_USE_MAHONY - #elif SENSOR_USE_MADGWICK - #elif SENSOR_USE_BASICVQF - , basicvqf(gyrTs, ((accTs<0) ? gyrTs : accTs), - ((magTs<0) ? gyrTs : magTs)) - #elif SENSOR_USE_VQF - , vqf(vqfParams, 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 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(); - void clearUpdated(); - sensor_real_t const * getQuaternion(); - Quat getQuaternionQuat(); - sensor_real_t const * getGravityVec(); - sensor_real_t const * getLinearAcc(); - void getLinearAcc(sensor_real_t outLinAccel[3]); - Vector3 getLinearAccVec(); - - 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]); - - protected: - sensor_real_t gyrTs; - sensor_real_t accTs; - sensor_real_t magTs; - - #if SENSOR_USE_MAHONY - Mahony mahony; - #elif SENSOR_USE_MADGWICK - Madgwick madgwick; - #elif SENSOR_USE_BASICVQF - BasicVQF basicvqf; - #elif SENSOR_USE_VQF - SensorVQFParams vqfParams {}; - VQF vqf; - #endif - - // A also used for linear acceleration extraction - sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f}; - - #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK - // Buffer M here to keep the behavior of BMI160 - sensor_real_t bMxyz[3]{0.0f, 0.0f, 0.0f}; - bool accelUpdated = false; - #endif - - bool magExist = false; - sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f}; - bool updated = false; - - 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 - }; - } -} - -#endif // SLIMEVR_SENSORFUSION_H +#endif // SLIMEVR_SENSORFUSION_H diff --git a/src/sensors/SensorFusionDMP.cpp b/src/sensors/SensorFusionDMP.cpp index 831195d71..4208a536e 100644 --- a/src/sensors/SensorFusionDMP.cpp +++ b/src/sensors/SensorFusionDMP.cpp @@ -1,113 +1,100 @@ #include "SensorFusionDMP.h" -namespace SlimeVR -{ - namespace Sensors - { - 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]); - } - } +namespace SlimeVR { +namespace Sensors { +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]); +} +} // namespace Sensors +} // namespace SlimeVR diff --git a/src/sensors/SensorFusionDMP.h b/src/sensors/SensorFusionDMP.h index ce7e745bd..579c382a8 100644 --- a/src/sensors/SensorFusionDMP.h +++ b/src/sensors/SensorFusionDMP.h @@ -4,47 +4,44 @@ #include "SensorFusion.h" #include "dmpmag.h" -namespace SlimeVR -{ - namespace Sensors - { - class SensorFusionDMP - { - public: - void updateQuaternion(sensor_real_t nqwxyz[4]); - void updateQuaternion(Quaternion const & nq); - void updateQuaternion(Quat const & nq); - void updateAcc(sensor_real_t Axyz[3]); - void updateMag(sensor_real_t Mxyz[3]); - - bool isUpdated(); - void clearUpdated(); - sensor_real_t const * getQuaternion(); - Quat getQuaternionQuat(); - sensor_real_t const * getGravityVec(); - sensor_real_t const * getLinearAcc(); - void getLinearAcc(sensor_real_t outLinAccel[3]); - Vector3 getLinearAccVec(); - - protected: - DMPMag dmpmag; - - 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; - 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 - }; - } -} - -#endif // SLIMEVR_SENSORFUSIONDMP_H +namespace SlimeVR { +namespace Sensors { +class SensorFusionDMP { +public: + void updateQuaternion(sensor_real_t nqwxyz[4]); + void updateQuaternion(Quaternion const& nq); + void updateQuaternion(Quat const& nq); + void updateAcc(sensor_real_t Axyz[3]); + void updateMag(sensor_real_t Mxyz[3]); + + bool isUpdated(); + void clearUpdated(); + sensor_real_t const* getQuaternion(); + Quat getQuaternionQuat(); + sensor_real_t const* getGravityVec(); + sensor_real_t const* getLinearAcc(); + void getLinearAcc(sensor_real_t outLinAccel[3]); + Vector3 getLinearAccVec(); + +protected: + DMPMag dmpmag; + + 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; + 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 diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index a742c883a..9d04d03a6 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/src/sensors/SensorFusionRestDetect.cpp @@ -1,32 +1,37 @@ #include "SensorFusionRestDetect.h" -namespace SlimeVR -{ - namespace Sensors - { - #if !SENSOR_FUSION_WITH_RESTDETECT - void SensorFusionRestDetect::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat) - { - if (deltat < 0) deltat = accTs; - restDetection.updateAcc(deltat, Axyz); - SensorFusion::updateAcc(Axyz, deltat); - } +namespace SlimeVR { +namespace Sensors { +#if !SENSOR_FUSION_WITH_RESTDETECT +void SensorFusionRestDetect::updateAcc( + const sensor_real_t Axyz[3], + sensor_real_t deltat +) { + if (deltat < 0) { + deltat = accTs; + } + restDetection.updateAcc(deltat, Axyz); + SensorFusion::updateAcc(Axyz, deltat); +} - void SensorFusionRestDetect::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat) - { - if (deltat < 0) deltat = gyrTs; - restDetection.updateGyr(Gxyz); - SensorFusion::updateGyro(Gxyz, deltat); - } - #endif +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 - } - } +bool SensorFusionRestDetect::getRestDetected() { +#if !SENSOR_FUSION_WITH_RESTDETECT + return restDetection.getRestDetected(); +#elif SENSOR_USE_VQF + return vqf.getRestDetected(); +#endif } +} // namespace Sensors +} // namespace SlimeVR diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index 03e7354bc..95c396372 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/src/sensors/SensorFusionRestDetect.h @@ -1,55 +1,51 @@ #ifndef SLIMEVR_SENSORFUSIONRESTDETECT_H_ #define SLIMEVR_SENSORFUSIONRESTDETECT_H_ -#include "SensorFusion.h" - #include "../motionprocessing/RestDetection.h" +#include "SensorFusion.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 Sensors - { - #if !SENSOR_FUSION_WITH_RESTDETECT - struct SensorRestDetectionParams: RestDetectionParams { - SensorRestDetectionParams() : RestDetectionParams() { - restMinTime = 2.0f; - restThGyr = 0.6f; // 400 norm - restThAcc = 0.06f; // 100 norm - } - }; - #endif +namespace SlimeVR { +namespace Sensors { +#if !SENSOR_FUSION_WITH_RESTDETECT +struct SensorRestDetectionParams : RestDetectionParams { + SensorRestDetectionParams() + : RestDetectionParams() { + restMinTime = 2.0f; + restThGyr = 0.6f; // 400 norm + restThAcc = 0.06f; // 100 norm + } +}; +#endif - class SensorFusionRestDetect : public SensorFusion - { - public: - SensorFusionRestDetect(float gyrTs, float accTs=-1.0, float magTs=-1.0) - : SensorFusion(gyrTs, accTs, magTs) - #if !SENSOR_FUSION_WITH_RESTDETECT - , restDetection(restDetectionParams, gyrTs, - (accTs<0) ? gyrTs : accTs) - #endif - {} +class SensorFusionRestDetect : public SensorFusion { +public: + SensorFusionRestDetect(float gyrTs, float accTs = -1.0, float magTs = -1.0) + : SensorFusion(gyrTs, accTs, magTs) +#if !SENSOR_FUSION_WITH_RESTDETECT + , restDetection(restDetectionParams, gyrTs, (accTs < 0) ? gyrTs : accTs) +#endif + { + } - bool getRestDetected(); + bool getRestDetected(); - #if !SENSOR_FUSION_WITH_RESTDETECT - void updateAcc(const sensor_real_t Axyz[3], const sensor_real_t deltat); - void updateGyro(const sensor_real_t Gxyz[3], const sensor_real_t deltat); - #endif - protected: - #if !SENSOR_FUSION_WITH_RESTDETECT - SensorRestDetectionParams restDetectionParams {}; - RestDetection restDetection; - #endif - - }; - } -} +#if !SENSOR_FUSION_WITH_RESTDETECT + void updateAcc(const sensor_real_t Axyz[3], const sensor_real_t deltat); + void updateGyro(const sensor_real_t Gxyz[3], const sensor_real_t deltat); +#endif +protected: +#if !SENSOR_FUSION_WITH_RESTDETECT + SensorRestDetectionParams restDetectionParams{}; + RestDetection restDetection; +#endif +}; +} // namespace Sensors +} // namespace SlimeVR -#endif // SLIMEVR_SENSORFUSIONRESTDETECT_H_ +#endif // SLIMEVR_SENSORFUSIONRESTDETECT_H_ diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 1c1dc9cc7..09262d035 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -1,192 +1,199 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "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" +#endif +namespace SlimeVR { +namespace Sensors { +using SoftFusionLSM6DS3TRC + = SoftFusionSensor; +using SoftFusionICM42688 + = SoftFusionSensor; +using SoftFusionBMI270 + = SoftFusionSensor; +using SoftFusionLSM6DSV + = SoftFusionSensor; +using SoftFusionLSM6DSO + = SoftFusionSensor; +using SoftFusionLSM6DSR + = SoftFusionSensor; +using SoftFusionMPU6050 + = SoftFusionSensor; + +// TODO Make it more generic in the future and move another place (abstract sensor +// interface) +void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) { + if (sclPin != activeSCL || sdaPin != activeSDA || !running) { + Wire.flush(); #if ESP32 - #include "driver/i2c.h" + if (running) { + } else { + // Reset HWI2C to avoid being affected by I2CBUS reset + Wire.end(); + } + // Disconnect pins from HWI2C + gpio_set_direction((gpio_num_t)activeSCL, GPIO_MODE_INPUT); + gpio_set_direction((gpio_num_t)activeSDA, GPIO_MODE_INPUT); + + if (running) { + i2c_set_pin(I2C_NUM_0, sdaPin, sclPin, false, false, I2C_MODE_MASTER); + } else { + Wire.begin(static_cast(sdaPin), static_cast(sclPin), I2C_SPEED); + Wire.setTimeOut(150); + } +#else + Wire.begin(static_cast(sdaPin), static_cast(sclPin)); #endif -namespace SlimeVR -{ - namespace Sensors - { - using SoftFusionLSM6DS3TRC = SoftFusionSensor; - using SoftFusionICM42688 = SoftFusionSensor; - using SoftFusionBMI270 = SoftFusionSensor; - using SoftFusionLSM6DSV = SoftFusionSensor; - using SoftFusionLSM6DSO = SoftFusionSensor; - using SoftFusionLSM6DSR = SoftFusionSensor; - using SoftFusionMPU6050 = SoftFusionSensor; - - // TODO Make it more generic in the future and move another place (abstract sensor interface) - void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) - { - if (sclPin != activeSCL || sdaPin != activeSDA || !running) { - Wire.flush(); - #if ESP32 - if (running) {} - else { - // Reset HWI2C to avoid being affected by I2CBUS reset - Wire.end(); - } - // Disconnect pins from HWI2C - gpio_set_direction((gpio_num_t)activeSCL, GPIO_MODE_INPUT); - gpio_set_direction((gpio_num_t)activeSDA, GPIO_MODE_INPUT); - - if (running) { - i2c_set_pin(I2C_NUM_0, sdaPin, sclPin, false, false, I2C_MODE_MASTER); - } else { - Wire.begin(static_cast(sdaPin), static_cast(sclPin), I2C_SPEED); - Wire.setTimeOut(150); - } - #else - Wire.begin(static_cast(sdaPin), static_cast(sclPin)); - #endif - - activeSCL = sclPin; - activeSDA = sdaPin; - } - } - - void SensorManager::setup() - { - running = false; - activeSCL = PIN_IMU_SCL; - activeSDA = PIN_IMU_SDA; - - uint8_t sensorID = 0; - uint8_t activeSensorCount = 0; -#define IMU_DESC_ENTRY(ImuType, ...) \ - { \ - auto sensor = buildSensor(sensorID, __VA_ARGS__); \ - if (sensor->isWorking()) { \ - m_Logger.info("Sensor %d configured", sensorID+1);\ - activeSensorCount++; \ - } \ - m_Sensors.push_back(std::move(sensor)); \ - sensorID++; \ - } - // Apply descriptor list and expand to entrys - IMU_DESC_LIST; + activeSCL = sclPin; + activeSDA = sdaPin; + } +} + +void SensorManager::setup() { + running = false; + activeSCL = PIN_IMU_SCL; + activeSDA = PIN_IMU_SDA; + + uint8_t sensorID = 0; + uint8_t activeSensorCount = 0; +#define IMU_DESC_ENTRY(ImuType, ...) \ + { \ + auto sensor = buildSensor(sensorID, __VA_ARGS__); \ + if (sensor->isWorking()) { \ + m_Logger.info("Sensor %d configured", sensorID + 1); \ + activeSensorCount++; \ + } \ + m_Sensors.push_back(std::move(sensor)); \ + sensorID++; \ + } + // Apply descriptor list and expand to entrys + IMU_DESC_LIST; #undef IMU_DESC_ENTRY - m_Logger.info("%d sensor(s) configured", activeSensorCount); - // Check and scan i2c if no sensors active - if (activeSensorCount == 0) { - m_Logger.error("Can't find I2C device on provided addresses, scanning for all I2C devices and returning"); - 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 - } - - } + m_Logger.info("%d sensor(s) configured", activeSensorCount); + // Check and scan i2c if no sensors active + if (activeSensorCount == 0) { + m_Logger.error( + "Can't find I2C device on provided addresses, scanning for all I2C devices " + "and returning" + ); + 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 } + +} // namespace Sensors +} // namespace SlimeVR diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index d1bc6318d..9a8980672 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -1,112 +1,136 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_SENSORMANAGER #define SLIMEVR_SENSORMANAGER -#include "globals.h" -#include "sensor.h" -#include "EmptySensor.h" -#include "ErroneousSensor.h" -#include "logging/Logger.h" - #include #include +#include "EmptySensor.h" +#include "ErroneousSensor.h" +#include "globals.h" +#include "logging/Logger.h" +#include "sensor.h" -namespace SlimeVR -{ - namespace Sensors - { - class SensorManager - { - public: - SensorManager() - : m_Logger(SlimeVR::Logging::Logger("SensorManager")) { } - void setup(); - void postSetup(); - - void update(); - - std::vector> & getSensors() { return m_Sensors; }; - ImuID getSensorType(size_t id) { - if(id < m_Sensors.size()) { - return m_Sensors[id]->getSensorType(); - } - return ImuID::Unknown; - } - - private: - SlimeVR::Logging::Logger m_Logger; - - std::vector> m_Sensors; - - template - std::unique_ptr buildSensor(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\ +namespace SlimeVR { +namespace Sensors { +class SensorManager { +public: + SensorManager() + : m_Logger(SlimeVR::Logging::Logger("SensorManager")) {} + void setup(); + void postSetup(); + + void update(); + + std::vector>& getSensors() { return m_Sensors; }; + ImuID getSensorType(size_t id) { + if (id < m_Sensors.size()) { + return m_Sensors[id]->getSensorType(); + } + return ImuID::Unknown; + } + +private: + SlimeVR::Logging::Logger m_Logger; + + std::vector> m_Sensors; + + template + std::unique_ptr buildSensor( + 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, - sclPin, sdaPin, extraParam, optional); - - // Now start detecting and building the IMU - std::unique_ptr sensor; - - // Clear and reset I2C bus for each sensor upon startup - I2CSCAN::clearBus(sdaPin, sclPin); - swapI2C(sclPin, sdaPin); - - if (I2CSCAN::hasDevOnBus(address)) { - m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address); - } else { - if (!optional) { - m_Logger.error("Mandatory sensor %d not found at address 0x%02X", sensorID + 1, address); - sensor = std::make_unique(sensorID, ImuType::TypeID); - } - else { - m_Logger.debug("Optional sensor %d not found at address 0x%02X", sensorID + 1, address); - sensor = std::make_unique(sensorID); - } - return sensor; - } - - uint8_t intPin = extraParam; - sensor = std::make_unique(sensorID, addrSuppl, rotation, sclPin, sdaPin, intPin); - - sensor->motionSetup(); - return sensor; - } - uint8_t activeSCL = 0; - uint8_t activeSDA = 0; - bool running = false; - void swapI2C(uint8_t scl, uint8_t sda); - - uint32_t m_LastBundleSentAtMicros = micros(); - }; - } -} - -#endif // SLIMEVR_SENSORFACTORY_H_ + sensorID, + address, + rotation, + sclPin, + sdaPin, + extraParam, + optional + ); + + // Now start detecting and building the IMU + std::unique_ptr sensor; + + // Clear and reset I2C bus for each sensor upon startup + I2CSCAN::clearBus(sdaPin, sclPin); + swapI2C(sclPin, sdaPin); + + if (I2CSCAN::hasDevOnBus(address)) { + m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address); + } else { + if (!optional) { + m_Logger.error( + "Mandatory sensor %d not found at address 0x%02X", + sensorID + 1, + address + ); + sensor = std::make_unique(sensorID, ImuType::TypeID); + } else { + m_Logger.debug( + "Optional sensor %d not found at address 0x%02X", + sensorID + 1, + address + ); + sensor = std::make_unique(sensorID); + } + return sensor; + } + + uint8_t intPin = extraParam; + sensor = std::make_unique( + sensorID, + addrSuppl, + rotation, + sclPin, + sdaPin, + intPin + ); + + sensor->motionSetup(); + return sensor; + } + uint8_t activeSCL = 0; + uint8_t activeSDA = 0; + bool running = false; + void swapI2C(uint8_t scl, uint8_t sda); + + uint32_t m_LastBundleSentAtMicros = micros(); +}; +} // namespace Sensors +} // namespace SlimeVR + +#endif // SLIMEVR_SENSORFACTORY_H_ diff --git a/src/sensors/axisremap.h b/src/sensors/axisremap.h index 2db57ff5b..c0cf86ce9 100644 --- a/src/sensors/axisremap.h +++ b/src/sensors/axisremap.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2023 SlimeVR Contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 SlimeVR Contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef 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_Y 1 -#define AXIS_REMAP_USE_Z 2 +#define AXIS_REMAP_USE_X 0 +#define AXIS_REMAP_USE_Y 1 +#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) \ - (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)) +#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_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, \ - AXIS_REMAP_USE_X, AXIS_REMAP_USE_Y, AXIS_REMAP_USE_Z) +#define AXIS_REMAP_DEFAULT \ + 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 +template T inline remapOneAxis(int axisdesc, T x, T y, T z) { - T result; - switch (axisdesc & 0x3) { - case AXIS_REMAP_USE_X: result = x; break; - case AXIS_REMAP_USE_Y: result = y; break; - case AXIS_REMAP_USE_Z: result = z; break; - default: result = 0; - } - return (axisdesc & AXIS_REMAP_INVERT) ? -result : result; + T result; + switch (axisdesc & 0x3) { + case AXIS_REMAP_USE_X: + result = x; + break; + case AXIS_REMAP_USE_Y: + result = y; + break; + case AXIS_REMAP_USE_Z: + result = z; + break; + default: + result = 0; + } + return (axisdesc & AXIS_REMAP_INVERT) ? -result : result; } -template -void inline remapAllAxis(int axisdesc, T *x, T *y, T *z) { - T bx = *x, by = *y, bz = *z; - *x = remapOneAxis(AXIS_REMAP_GET_X(axisdesc), bx, by, bz); - *y = remapOneAxis(AXIS_REMAP_GET_Y(axisdesc), bx, by, bz); - *z = remapOneAxis(AXIS_REMAP_GET_Z(axisdesc), bx, by, bz); +template +void inline remapAllAxis(int axisdesc, T* x, T* y, T* z) { + T bx = *x, by = *y, bz = *z; + *x = remapOneAxis(AXIS_REMAP_GET_X(axisdesc), bx, by, bz); + *y = remapOneAxis(AXIS_REMAP_GET_Y(axisdesc), bx, by, bz); + *z = remapOneAxis(AXIS_REMAP_GET_Z(axisdesc), bx, by, bz); } -#endif // AXISREMAP_H +#endif // AXISREMAP_H diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index a6a65dd32..7da6cbc76 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -1,1008 +1,1183 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 S.J. Remington & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 S.J. Remington & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "bmi160sensor.h" -#include "GlobalVars.h" + #include #include + #include +#include "GlobalVars.h" + void BMI160Sensor::initHMC(BMI160MagRate magRate) { - /* Configure MAG interface and setup mode */ - /* Set MAG interface normal power mode */ - imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); - delay(60); - - /* Enable MAG interface */ - imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); - delay(1); - - imu.setMagDeviceAddress(HMC_DEVADDR); - delay(3); - imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_SETUP_MODE); - delay(3); - - /* Configure HMC5883L Sensor */ - imu.setMagRegister(HMC_RA_CFGA, HMC_CFGA_DATA_RATE_75 | HMC_CFGA_AVG_SAMPLES_8 | HMC_CFGA_BIAS_NORMAL); - imu.setMagRegister(HMC_RA_CFGB, HMC_CFGB_GAIN_1_30); - imu.setMagRegister(HMC_RA_MODE, HMC_MODE_HIGHSPEED | HMC_MODE_READ_CONTINUOUS); - - imu.setRegister(BMI160_RA_MAG_IF_2_READ_RA, HMC_RA_DATA); - imu.setRegister(BMI160_RA_MAG_CONF, magRate); - delay(3); - imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_DATA_MODE_6); + /* Configure MAG interface and setup mode */ + /* Set MAG interface normal power mode */ + imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); + delay(60); + + /* Enable MAG interface */ + imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); + delay(1); + + imu.setMagDeviceAddress(HMC_DEVADDR); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_SETUP_MODE); + delay(3); + + /* Configure HMC5883L Sensor */ + imu.setMagRegister( + HMC_RA_CFGA, + HMC_CFGA_DATA_RATE_75 | HMC_CFGA_AVG_SAMPLES_8 | HMC_CFGA_BIAS_NORMAL + ); + imu.setMagRegister(HMC_RA_CFGB, HMC_CFGB_GAIN_1_30); + imu.setMagRegister(HMC_RA_MODE, HMC_MODE_HIGHSPEED | HMC_MODE_READ_CONTINUOUS); + + imu.setRegister(BMI160_RA_MAG_IF_2_READ_RA, HMC_RA_DATA); + imu.setRegister(BMI160_RA_MAG_CONF, magRate); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_DATA_MODE_6); } void BMI160Sensor::initQMC(BMI160MagRate magRate) { - /* Configure MAG interface and setup mode */ - /* Set MAG interface normal power mode */ - imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); - delay(60); - - /* Enable MAG interface */ - imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); - delay(1); - - imu.setMagDeviceAddress(QMC_DEVADDR); - delay(3); - imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_SETUP_MODE); - delay(3); - - /* Configure QMC5883L Sensor */ - imu.setMagRegister(QMC_RA_RESET, 1); - delay(3); - imu.setMagRegister(QMC_RA_CONTROL, QMC_CFG_MODE_CONTINUOUS | QMC_CFG_ODR_200HZ | QMC_CFG_RNG_8G | QMC_CFG_OSR_512); - - imu.setRegister(BMI160_RA_MAG_IF_2_READ_RA, QMC_RA_DATA); - imu.setRegister(BMI160_RA_MAG_CONF, magRate); - delay(3); - imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_DATA_MODE_6); + /* Configure MAG interface and setup mode */ + /* Set MAG interface normal power mode */ + imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); + delay(60); + + /* Enable MAG interface */ + imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); + delay(1); + + imu.setMagDeviceAddress(QMC_DEVADDR); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_SETUP_MODE); + delay(3); + + /* Configure QMC5883L Sensor */ + imu.setMagRegister(QMC_RA_RESET, 1); + delay(3); + imu.setMagRegister( + QMC_RA_CONTROL, + QMC_CFG_MODE_CONTINUOUS | QMC_CFG_ODR_200HZ | QMC_CFG_RNG_8G | QMC_CFG_OSR_512 + ); + + imu.setRegister(BMI160_RA_MAG_IF_2_READ_RA, QMC_RA_DATA); + imu.setRegister(BMI160_RA_MAG_CONF, magRate); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_DATA_MODE_6); } void BMI160Sensor::motionSetup() { - // initialize device - imu.initialize( - addr, - BMI160_GYRO_RATE, - BMI160_GYRO_RANGE, - BMI160_GYRO_FILTER_MODE, - BMI160_ACCEL_RATE, - BMI160_ACCEL_RANGE, - BMI160_ACCEL_FILTER_MODE - ); - #if !USE_6_AXIS - #if BMI160_MAG_TYPE == BMI160_MAG_TYPE_HMC - initHMC(BMI160_MAG_RATE); - #elif BMI160_MAG_TYPE == BMI160_MAG_TYPE_QMC - initQMC(BMI160_MAG_RATE); - #else - static_assert(false, "Mag is enabled but BMI160_MAG_TYPE not set in defines"); - #endif - #endif - - if (!imu.testConnection()) { - m_Logger.fatal("Can't connect to BMI160 (reported device ID 0x%02x) at address 0x%02x", imu.getDeviceID(), addr); - ledManager.pattern(50, 50, 200); - return; - } - - m_Logger.info("Connected to BMI160 (reported device ID 0x%02x) at address 0x%02x", imu.getDeviceID(), addr); - - // Initialize the configuration - { - SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorConfig.type) { - case SlimeVR::Configuration::SensorConfigType::BMI160: - m_Config = sensorConfig.data.bmi160; - break; - - case SlimeVR::Configuration::SensorConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } - } - - int16_t ax, ay, az; - getRemappedAcceleration(&ax, &ay, &az); - float g_az = (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; - if (g_az < -0.75f) { - ledManager.on(); - - m_Logger.info("Flip front to confirm start calibration"); - delay(5000); - getRemappedAcceleration(&ax, &ay, &az); - g_az = (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; - if (g_az > 0.75f) { - m_Logger.debug("Starting calibration..."); - startCalibration(0); - } - - ledManager.off(); - } - - { - #define IS_INT16_CLIPPED(value) (value == INT16_MIN || value == INT16_MAX) - const bool anyClipped = IS_INT16_CLIPPED(ax) || IS_INT16_CLIPPED(ay) || IS_INT16_CLIPPED(az); - const bool anyZero = ax == 0 || ay == 0 || az == 0; - if (anyClipped || anyZero) { - m_Logger.warn("---------------- WARNING -----------------"); - m_Logger.warn("One or more accelerometer axes may be dead"); - m_Logger.warn("Acceleration: %i %i %i (Z = %f G)", - ax, ay, az, (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB); - m_Logger.warn("---------------- WARNING -----------------"); - } - } - - // allocate temperature memory after calibration because OOM - gyroTempCalibrator = new GyroTemperatureCalibrator( - SlimeVR::Configuration::SensorConfigType::BMI160, - sensorId, - BMI160_GYRO_TYPICAL_SENSITIVITY_LSB, - BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP - ); - - #if BMI160_USE_TEMPCAL - gyroTempCalibrator->loadConfig(BMI160_GYRO_TYPICAL_SENSITIVITY_LSB); - if (gyroTempCalibrator->config.hasCoeffs) { - float GOxyzAtTemp[3]; - gyroTempCalibrator->approximateOffset(m_Config.temperature, GOxyzAtTemp); - for (uint32_t i = 0; i < 3; i++) { - GOxyzStaticTempCompensated[i] = m_Config.G_off[i] - GOxyzAtTemp[i]; - } - } - #endif - - #if BMI160_USE_SENSCAL - { - String localDevice = WiFi.macAddress(); - for (auto const& offsets : sensitivityOffsets) { - if (!localDevice.equals(offsets.mac)) continue; - if (offsets.sensorId != sensorId) continue; - - #define BMI160_CALCULATE_SENSITIVTY_MUL(degrees) (1.0 / (1.0 - ((degrees)/(360.0 * offsets.spins)))) - - gscaleX = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.x); - gscaleY = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.y); - gscaleZ = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.z); - m_Logger.debug("Custom sensitivity offset enabled: %s %s", - offsets.mac, - offsets.sensorId == SENSORID_PRIMARY ? "primary" : "aux" - ); - } - } - #endif - - isGyroCalibrated = hasGyroCalibration(); - isAccelCalibrated = hasAccelCalibration(); - #if !USE_6_AXIS - isMagCalibrated = hasMagCalibration(); - #endif - m_Logger.info("Calibration data for gyro: %s", isGyroCalibrated ? "found" : "not found"); - m_Logger.info("Calibration data for accel: %s", isAccelCalibrated ? "found" : "not found"); - #if !USE_6_AXIS - m_Logger.info("Calibration data for mag: %s", isMagCalibrated ? "found" : "not found"); - #endif - - imu.setFIFOHeaderModeEnabled(true); - imu.setGyroFIFOEnabled(true); - imu.setAccelFIFOEnabled(true); - #if !USE_6_AXIS - imu.setMagFIFOEnabled(true); - #endif - delay(4); - imu.resetFIFO(); - delay(2); - - uint8_t err; - if (imu.getErrReg(&err)) { - if (err & BMI160_ERR_MASK_CHIP_NOT_OPERABLE) { - m_Logger.fatal("Fatal error: chip not operable"); - return; - } else if (err & BMI160_ERR_MASK_ERROR_CODE) { - m_Logger.error("Error code 0x%02x", err); - } else { - m_Logger.info("Initialized"); - } - } else { - m_Logger.error("Failed to get error register value"); - } - - working = true; + // initialize device + imu.initialize( + addr, + BMI160_GYRO_RATE, + BMI160_GYRO_RANGE, + BMI160_GYRO_FILTER_MODE, + BMI160_ACCEL_RATE, + BMI160_ACCEL_RANGE, + BMI160_ACCEL_FILTER_MODE + ); +#if !USE_6_AXIS +#if BMI160_MAG_TYPE == BMI160_MAG_TYPE_HMC + initHMC(BMI160_MAG_RATE); +#elif BMI160_MAG_TYPE == BMI160_MAG_TYPE_QMC + initQMC(BMI160_MAG_RATE); +#else + static_assert(false, "Mag is enabled but BMI160_MAG_TYPE not set in defines"); +#endif +#endif + + if (!imu.testConnection()) { + m_Logger.fatal( + "Can't connect to BMI160 (reported device ID 0x%02x) at address 0x%02x", + imu.getDeviceID(), + addr + ); + ledManager.pattern(50, 50, 200); + return; + } + + m_Logger.info( + "Connected to BMI160 (reported device ID 0x%02x) at address 0x%02x", + imu.getDeviceID(), + addr + ); + + // Initialize the configuration + { + SlimeVR::Configuration::SensorConfig sensorConfig + = configuration.getSensor(sensorId); + // If no compatible calibration data is found, the calibration data will just be + // zero-ed out + switch (sensorConfig.type) { + case SlimeVR::Configuration::SensorConfigType::BMI160: + m_Config = sensorConfig.data.bmi160; + break; + + case SlimeVR::Configuration::SensorConfigType::NONE: + m_Logger.warn( + "No calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn( + "Incompatible calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + } + } + + int16_t ax, ay, az; + getRemappedAcceleration(&ax, &ay, &az); + float g_az = (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; + if (g_az < -0.75f) { + ledManager.on(); + + m_Logger.info("Flip front to confirm start calibration"); + delay(5000); + getRemappedAcceleration(&ax, &ay, &az); + g_az = (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; + if (g_az > 0.75f) { + m_Logger.debug("Starting calibration..."); + startCalibration(0); + } + + ledManager.off(); + } + + { +#define IS_INT16_CLIPPED(value) (value == INT16_MIN || value == INT16_MAX) + const bool anyClipped + = IS_INT16_CLIPPED(ax) || IS_INT16_CLIPPED(ay) || IS_INT16_CLIPPED(az); + const bool anyZero = ax == 0 || ay == 0 || az == 0; + if (anyClipped || anyZero) { + m_Logger.warn("---------------- WARNING -----------------"); + m_Logger.warn("One or more accelerometer axes may be dead"); + m_Logger.warn( + "Acceleration: %i %i %i (Z = %f G)", + ax, + ay, + az, + (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB + ); + m_Logger.warn("---------------- WARNING -----------------"); + } + } + + // allocate temperature memory after calibration because OOM + gyroTempCalibrator = new GyroTemperatureCalibrator( + SlimeVR::Configuration::SensorConfigType::BMI160, + sensorId, + BMI160_GYRO_TYPICAL_SENSITIVITY_LSB, + BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP + ); + +#if BMI160_USE_TEMPCAL + gyroTempCalibrator->loadConfig(BMI160_GYRO_TYPICAL_SENSITIVITY_LSB); + if (gyroTempCalibrator->config.hasCoeffs) { + float GOxyzAtTemp[3]; + gyroTempCalibrator->approximateOffset(m_Config.temperature, GOxyzAtTemp); + for (uint32_t i = 0; i < 3; i++) { + GOxyzStaticTempCompensated[i] = m_Config.G_off[i] - GOxyzAtTemp[i]; + } + } +#endif + +#if BMI160_USE_SENSCAL + { + String localDevice = WiFi.macAddress(); + for (auto const& offsets : sensitivityOffsets) { + if (!localDevice.equals(offsets.mac)) { + continue; + } + if (offsets.sensorId != sensorId) { + continue; + } + +#define BMI160_CALCULATE_SENSITIVTY_MUL(degrees) \ + (1.0 / (1.0 - ((degrees) / (360.0 * offsets.spins)))) + + gscaleX = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.x); + gscaleY = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.y); + gscaleZ = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.z); + m_Logger.debug( + "Custom sensitivity offset enabled: %s %s", + offsets.mac, + offsets.sensorId == SENSORID_PRIMARY ? "primary" : "aux" + ); + } + } +#endif + + isGyroCalibrated = hasGyroCalibration(); + isAccelCalibrated = hasAccelCalibration(); +#if !USE_6_AXIS + isMagCalibrated = hasMagCalibration(); +#endif + m_Logger.info( + "Calibration data for gyro: %s", + isGyroCalibrated ? "found" : "not found" + ); + m_Logger.info( + "Calibration data for accel: %s", + isAccelCalibrated ? "found" : "not found" + ); +#if !USE_6_AXIS + m_Logger.info( + "Calibration data for mag: %s", + isMagCalibrated ? "found" : "not found" + ); +#endif + + imu.setFIFOHeaderModeEnabled(true); + imu.setGyroFIFOEnabled(true); + imu.setAccelFIFOEnabled(true); +#if !USE_6_AXIS + imu.setMagFIFOEnabled(true); +#endif + delay(4); + imu.resetFIFO(); + delay(2); + + uint8_t err; + if (imu.getErrReg(&err)) { + if (err & BMI160_ERR_MASK_CHIP_NOT_OPERABLE) { + m_Logger.fatal("Fatal error: chip not operable"); + return; + } else if (err & BMI160_ERR_MASK_ERROR_CODE) { + m_Logger.error("Error code 0x%02x", err); + } else { + m_Logger.info("Initialized"); + } + } else { + m_Logger.error("Failed to get error register value"); + } + + working = true; } void BMI160Sensor::motionLoop() { - #if ENABLE_INSPECTION - { - int16_t rX, rY, rZ, aX, aY, aZ; - getRemappedRotation(&rX, &rY, &rZ); - getRemappedAcceleration(&aX, &aY, &aZ); - - networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); - } - #endif - - { - uint32_t now = micros(); - constexpr uint32_t BMI160_TARGET_SYNC_INTERVAL_MICROS = 25000; - uint32_t elapsed = now - lastClockPollTime; - if (elapsed >= BMI160_TARGET_SYNC_INTERVAL_MICROS) { - lastClockPollTime = now - (elapsed - BMI160_TARGET_SYNC_INTERVAL_MICROS); - - const uint32_t nextLocalTime1 = micros(); - uint32_t rawSensorTime; - if (imu.getSensorTime(&rawSensorTime)) { - localTime0 = localTime1; - localTime1 = nextLocalTime1; - syncLatencyMicros = (micros() - localTime1) * 0.3; - sensorTime0 = sensorTime1; - sensorTime1 = rawSensorTime; - if ((sensorTime0 > 0 || localTime0 > 0) && (sensorTime1 > 0 || sensorTime1 > 0)) { - // handle 24 bit overflow - double remoteDt = - sensorTime1 >= sensorTime0 ? - sensorTime1 - sensorTime0 : - (sensorTime1 + 0xFFFFFF) - sensorTime0; - double localDt = localTime1 - localTime0; - const double nextSensorTimeRatio = localDt / (remoteDt * BMI160_TIMESTAMP_RESOLUTION_MICROS); - - // handle sdk lags and time travel - if (round(nextSensorTimeRatio) == 1.0) { - sensorTimeRatio = nextSensorTimeRatio; - - if (round(sensorTimeRatioEma) != 1.0) { - sensorTimeRatioEma = sensorTimeRatio; - } - - constexpr double EMA_APPROX_SECONDS = 1.0; - constexpr uint32_t EMA_SAMPLES = (EMA_APPROX_SECONDS / 3 * 1e6) / BMI160_TARGET_SYNC_INTERVAL_MICROS; - sensorTimeRatioEma -= sensorTimeRatioEma / EMA_SAMPLES; - sensorTimeRatioEma += sensorTimeRatio / EMA_SAMPLES; - - sampleDtMicros = BMI160_ODR_GYR_MICROS * sensorTimeRatioEma; - samplesSinceClockSync = 0; - } - } - } - - getTemperature(&temperature); - optimistic_yield(100); - } - } - - { - uint32_t now = micros(); - constexpr uint32_t BMI160_TARGET_POLL_INTERVAL_MICROS = 6000; - uint32_t elapsed = now - lastPollTime; - if (elapsed >= BMI160_TARGET_POLL_INTERVAL_MICROS) { - lastPollTime = now - (elapsed - BMI160_TARGET_POLL_INTERVAL_MICROS); - - #if BMI160_DEBUG - uint32_t start = micros(); - readFIFO(); - uint32_t end = micros(); - cpuUsageMicros += end - start; - if (!lastCpuUsagePrinted) lastCpuUsagePrinted = end; - if (end - lastCpuUsagePrinted > 1e6) { - bool restDetected = sfusion.getRestDetected(); - - m_Logger.debug("readFIFO took %0.4f ms, read gyr %i acc %i mag %i rest %i resets %i readerrs %i type " SENSOR_FUSION_TYPE_STRING, - ((float)cpuUsageMicros / 1e3f), - gyrReads, - accReads, - magReads, - restDetected, - numFIFODropped, - numFIFOFailedReads - ); - - cpuUsageMicros = 0; - lastCpuUsagePrinted = end; - gyrReads = 0; - accReads = 0; - magReads = 0; - } - #else - readFIFO(); - #endif - optimistic_yield(100); - if (!sfusion.isUpdated()) return; - hadData = true; - sfusion.clearUpdated(); - } - } - - { - uint32_t now = micros(); - constexpr float maxSendRateHz = 2.0f; - constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; - uint32_t elapsed = now - lastTemperaturePacketSent; - if (elapsed >= sendInterval) { - lastTemperaturePacketSent = now - (elapsed - sendInterval); - #if BMI160_TEMPCAL_DEBUG - uint32_t isCalibrating = gyroTempCalibrator->isCalibrating() ? 10000 : 0; - networkConnection.sendTemperature(sensorId, isCalibrating + 10000 + (gyroTempCalibrator->config.samplesTotal * 100) + temperature); - #else - networkConnection.sendTemperature(sensorId, temperature); - #endif - optimistic_yield(100); - } - } - - { - uint32_t now = micros(); - constexpr float maxSendRateHz = 120.0f; - constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; - uint32_t elapsed = now - lastRotationPacketSent; - if (elapsed >= sendInterval) { - lastRotationPacketSent = now - (elapsed - sendInterval); - - setFusedRotation(sfusion.getQuaternionQuat()); - setAcceleration(sfusion.getLinearAccVec()); - - optimistic_yield(100); - } - } +#if ENABLE_INSPECTION + { + int16_t rX, rY, rZ, aX, aY, aZ; + getRemappedRotation(&rX, &rY, &rZ); + getRemappedAcceleration(&aX, &aY, &aZ); + + networkConnection.sendInspectionRawIMUData( + sensorId, + rX, + rY, + rZ, + 255, + aX, + aY, + aZ, + 255, + 0, + 0, + 0, + 255 + ); + } +#endif + + { + uint32_t now = micros(); + constexpr uint32_t BMI160_TARGET_SYNC_INTERVAL_MICROS = 25000; + uint32_t elapsed = now - lastClockPollTime; + if (elapsed >= BMI160_TARGET_SYNC_INTERVAL_MICROS) { + lastClockPollTime = now - (elapsed - BMI160_TARGET_SYNC_INTERVAL_MICROS); + + const uint32_t nextLocalTime1 = micros(); + uint32_t rawSensorTime; + if (imu.getSensorTime(&rawSensorTime)) { + localTime0 = localTime1; + localTime1 = nextLocalTime1; + syncLatencyMicros = (micros() - localTime1) * 0.3; + sensorTime0 = sensorTime1; + sensorTime1 = rawSensorTime; + if ((sensorTime0 > 0 || localTime0 > 0) + && (sensorTime1 > 0 || sensorTime1 > 0)) { + // handle 24 bit overflow + double remoteDt = sensorTime1 >= sensorTime0 + ? sensorTime1 - sensorTime0 + : (sensorTime1 + 0xFFFFFF) - sensorTime0; + double localDt = localTime1 - localTime0; + const double nextSensorTimeRatio + = localDt / (remoteDt * BMI160_TIMESTAMP_RESOLUTION_MICROS); + + // handle sdk lags and time travel + if (round(nextSensorTimeRatio) == 1.0) { + sensorTimeRatio = nextSensorTimeRatio; + + if (round(sensorTimeRatioEma) != 1.0) { + sensorTimeRatioEma = sensorTimeRatio; + } + + constexpr double EMA_APPROX_SECONDS = 1.0; + constexpr uint32_t EMA_SAMPLES + = (EMA_APPROX_SECONDS / 3 * 1e6) + / BMI160_TARGET_SYNC_INTERVAL_MICROS; + sensorTimeRatioEma -= sensorTimeRatioEma / EMA_SAMPLES; + sensorTimeRatioEma += sensorTimeRatio / EMA_SAMPLES; + + sampleDtMicros = BMI160_ODR_GYR_MICROS * sensorTimeRatioEma; + samplesSinceClockSync = 0; + } + } + } + + getTemperature(&temperature); + optimistic_yield(100); + } + } + + { + uint32_t now = micros(); + constexpr uint32_t BMI160_TARGET_POLL_INTERVAL_MICROS = 6000; + uint32_t elapsed = now - lastPollTime; + if (elapsed >= BMI160_TARGET_POLL_INTERVAL_MICROS) { + lastPollTime = now - (elapsed - BMI160_TARGET_POLL_INTERVAL_MICROS); + +#if BMI160_DEBUG + uint32_t start = micros(); + readFIFO(); + uint32_t end = micros(); + cpuUsageMicros += end - start; + if (!lastCpuUsagePrinted) { + lastCpuUsagePrinted = end; + } + if (end - lastCpuUsagePrinted > 1e6) { + bool restDetected = sfusion.getRestDetected(); + + m_Logger.debug( + "readFIFO took %0.4f ms, read gyr %i acc %i mag %i rest %i resets " + "%i readerrs %i type " SENSOR_FUSION_TYPE_STRING, + ((float)cpuUsageMicros / 1e3f), + gyrReads, + accReads, + magReads, + restDetected, + numFIFODropped, + numFIFOFailedReads + ); + + cpuUsageMicros = 0; + lastCpuUsagePrinted = end; + gyrReads = 0; + accReads = 0; + magReads = 0; + } +#else + readFIFO(); +#endif + optimistic_yield(100); + if (!sfusion.isUpdated()) { + return; + } + hadData = true; + sfusion.clearUpdated(); + } + } + + { + uint32_t now = micros(); + constexpr float maxSendRateHz = 2.0f; + constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6; + uint32_t elapsed = now - lastTemperaturePacketSent; + if (elapsed >= sendInterval) { + lastTemperaturePacketSent = now - (elapsed - sendInterval); +#if BMI160_TEMPCAL_DEBUG + uint32_t isCalibrating = gyroTempCalibrator->isCalibrating() ? 10000 : 0; + networkConnection.sendTemperature( + sensorId, + isCalibrating + 10000 + (gyroTempCalibrator->config.samplesTotal * 100) + + temperature + ); +#else + networkConnection.sendTemperature(sensorId, temperature); +#endif + optimistic_yield(100); + } + } + + { + uint32_t now = micros(); + constexpr float maxSendRateHz = 120.0f; + constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6; + uint32_t elapsed = now - lastRotationPacketSent; + if (elapsed >= sendInterval) { + lastRotationPacketSent = now - (elapsed - sendInterval); + + setFusedRotation(sfusion.getQuaternionQuat()); + setAcceleration(sfusion.getLinearAccVec()); + + optimistic_yield(100); + } + } } void BMI160Sensor::readFIFO() { - if (!imu.getFIFOCount(&fifo.length)) { - #if BMI160_DEBUG - numFIFOFailedReads++; - #endif - return; - } - - if (fifo.length <= 1) return; - if (fifo.length > sizeof(fifo.data)) { - #if BMI160_DEBUG - numFIFODropped++; - #endif - imu.resetFIFO(); - return; - } - std::fill(fifo.data, fifo.data + fifo.length, 0); - if (!imu.getFIFOBytes(fifo.data, fifo.length)) { - #if BMI160_DEBUG - numFIFOFailedReads++; - #endif - return; - } - - optimistic_yield(100); - - int16_t gx, gy, gz; - int16_t ax, ay, az; - bool gnew, anew; - #if !USE_6_AXIS - int16_t mx, my, mz; - bool mnew; - #endif - - uint8_t header; - for (uint32_t i = 0; i < fifo.length;) { - #define BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(len) { if (i + len > fifo.length) break; } - BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(1); - - // ignore interrupt tags in header - header = fifo.data[i] & 0b11111100; - i++; - - if (header == BMI160_FIFO_HEADER_CTL_SKIP_FRAME) { - BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_SKIP_FRAME_LEN); - break; - } else if (header == BMI160_FIFO_HEADER_CTL_SENSOR_TIME) { - BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_SENSOR_TIME_LEN); - i += BMI160_FIFO_SENSOR_TIME_LEN; - } else if (header == BMI160_FIFO_HEADER_CTL_INPUT_CONFIG) { - BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_INPUT_CONFIG_LEN); - i += BMI160_FIFO_INPUT_CONFIG_LEN; - } else if (header & BMI160_FIFO_HEADER_DATA_FRAME_BASE) { - if (!(header & BMI160_FIFO_HEADER_DATA_FRAME_MASK_HAS_DATA)) { - break; - } - gnew = false; - anew = false; - #if !USE_6_AXIS - mnew = false; - #endif - - // mag - if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_M) { - BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_M_LEN); - #if !USE_6_AXIS - getMagnetometerXYZFromBuffer(&fifo.data[i], &mx, &my, &mz); - mnew = true; - #endif - i += BMI160_FIFO_M_LEN; - } - - // bmi160 -> 0 lsb 1 msb - // gyro - if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G) { - BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_G_LEN); - gx = ((int16_t)fifo.data[i + 1] << 8) | fifo.data[i + 0]; - gy = ((int16_t)fifo.data[i + 3] << 8) | fifo.data[i + 2]; - gz = ((int16_t)fifo.data[i + 5] << 8) | fifo.data[i + 4]; - gnew = true; - i += BMI160_FIFO_G_LEN; - } - - // bmi160 -> 0 lsb 1 msb - // accel - if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_A) { - BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_A_LEN); - ax = ((int16_t)fifo.data[i + 1] << 8) | fifo.data[i + 0]; - ay = ((int16_t)fifo.data[i + 3] << 8) | fifo.data[i + 2]; - az = ((int16_t)fifo.data[i + 5] << 8) | fifo.data[i + 4]; - anew = true; - i += BMI160_FIFO_A_LEN; - } - - // gyro callback updates fusion and must be last - #if !USE_6_AXIS - if (mnew) onMagRawSample(BMI160_ODR_MAG_MICROS, mx, my, mz); - #endif - if (anew) onAccelRawSample(BMI160_ODR_ACC_MICROS, ax, ay, az); - if (gnew) { - constexpr uint32_t alignmentBitmask = ~(0xFFFFFFFF << (16 - BMI160_GYRO_RATE)); - uint32_t alignmentOffset = - (sensorTime1 & alignmentBitmask) * BMI160_TIMESTAMP_RESOLUTION_MICROS * sensorTimeRatioEma; - - timestamp0 = timestamp1; - timestamp1 = (localTime1 - alignmentOffset - syncLatencyMicros) + - (++samplesSinceClockSync) * sampleDtMicros; - int32_t dtMicros = timestamp1 - timestamp0; - - constexpr float invPeriod = 1.0f / BMI160_ODR_GYR_MICROS; - int32_t sampleOffset = round((float)dtMicros * invPeriod) - 1; - if (abs(sampleOffset) > 3) { - dtMicros = sampleDtMicros; - } else if (sampleOffset != 0) { - dtMicros -= sampleOffset * sampleDtMicros; - } - - onGyroRawSample(dtMicros, gx, gy, gz); - } - } else { - break; - } - } + if (!imu.getFIFOCount(&fifo.length)) { +#if BMI160_DEBUG + numFIFOFailedReads++; +#endif + return; + } + + if (fifo.length <= 1) { + return; + } + if (fifo.length > sizeof(fifo.data)) { +#if BMI160_DEBUG + numFIFODropped++; +#endif + imu.resetFIFO(); + return; + } + std::fill(fifo.data, fifo.data + fifo.length, 0); + if (!imu.getFIFOBytes(fifo.data, fifo.length)) { +#if BMI160_DEBUG + numFIFOFailedReads++; +#endif + return; + } + + optimistic_yield(100); + + int16_t gx, gy, gz; + int16_t ax, ay, az; + bool gnew, anew; +#if !USE_6_AXIS + int16_t mx, my, mz; + bool mnew; +#endif + + uint8_t header; + for (uint32_t i = 0; i < fifo.length;) { +#define BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(len) \ + { \ + if (i + len > fifo.length) \ + break; \ + } + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(1); + + // ignore interrupt tags in header + header = fifo.data[i] & 0b11111100; + i++; + + if (header == BMI160_FIFO_HEADER_CTL_SKIP_FRAME) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_SKIP_FRAME_LEN); + break; + } else if (header == BMI160_FIFO_HEADER_CTL_SENSOR_TIME) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_SENSOR_TIME_LEN); + i += BMI160_FIFO_SENSOR_TIME_LEN; + } else if (header == BMI160_FIFO_HEADER_CTL_INPUT_CONFIG) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_INPUT_CONFIG_LEN); + i += BMI160_FIFO_INPUT_CONFIG_LEN; + } else if (header & BMI160_FIFO_HEADER_DATA_FRAME_BASE) { + if (!(header & BMI160_FIFO_HEADER_DATA_FRAME_MASK_HAS_DATA)) { + break; + } + gnew = false; + anew = false; +#if !USE_6_AXIS + mnew = false; +#endif + + // mag + if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_M) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_M_LEN); +#if !USE_6_AXIS + getMagnetometerXYZFromBuffer(&fifo.data[i], &mx, &my, &mz); + mnew = true; +#endif + i += BMI160_FIFO_M_LEN; + } + + // bmi160 -> 0 lsb 1 msb + // gyro + if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_G_LEN); + gx = ((int16_t)fifo.data[i + 1] << 8) | fifo.data[i + 0]; + gy = ((int16_t)fifo.data[i + 3] << 8) | fifo.data[i + 2]; + gz = ((int16_t)fifo.data[i + 5] << 8) | fifo.data[i + 4]; + gnew = true; + i += BMI160_FIFO_G_LEN; + } + + // bmi160 -> 0 lsb 1 msb + // accel + if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_A) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_A_LEN); + ax = ((int16_t)fifo.data[i + 1] << 8) | fifo.data[i + 0]; + ay = ((int16_t)fifo.data[i + 3] << 8) | fifo.data[i + 2]; + az = ((int16_t)fifo.data[i + 5] << 8) | fifo.data[i + 4]; + anew = true; + i += BMI160_FIFO_A_LEN; + } + +// gyro callback updates fusion and must be last +#if !USE_6_AXIS + if (mnew) { + onMagRawSample(BMI160_ODR_MAG_MICROS, mx, my, mz); + } +#endif + if (anew) { + onAccelRawSample(BMI160_ODR_ACC_MICROS, ax, ay, az); + } + if (gnew) { + constexpr uint32_t alignmentBitmask + = ~(0xFFFFFFFF << (16 - BMI160_GYRO_RATE)); + uint32_t alignmentOffset = (sensorTime1 & alignmentBitmask) + * BMI160_TIMESTAMP_RESOLUTION_MICROS + * sensorTimeRatioEma; + + timestamp0 = timestamp1; + timestamp1 = (localTime1 - alignmentOffset - syncLatencyMicros) + + (++samplesSinceClockSync) * sampleDtMicros; + int32_t dtMicros = timestamp1 - timestamp0; + + constexpr float invPeriod = 1.0f / BMI160_ODR_GYR_MICROS; + int32_t sampleOffset = round((float)dtMicros * invPeriod) - 1; + if (abs(sampleOffset) > 3) { + dtMicros = sampleDtMicros; + } else if (sampleOffset != 0) { + dtMicros -= sampleOffset * sampleDtMicros; + } + + onGyroRawSample(dtMicros, gx, gy, gz); + } + } else { + break; + } + } } void BMI160Sensor::onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) { - #if BMI160_DEBUG - gyrReads++; - #endif - - #if BMI160_USE_TEMPCAL - bool restDetected = sfusion.getRestDetected(); - gyroTempCalibrator->updateGyroTemperatureCalibration(temperature, restDetected, x, y, z); - #endif - - sensor_real_t gyroCalibratedStatic[3]; - gyroCalibratedStatic[0] = (sensor_real_t)((((double)x - m_Config.G_off[0]) * gscaleX)); - gyroCalibratedStatic[1] = (sensor_real_t)((((double)y - m_Config.G_off[1]) * gscaleY)); - gyroCalibratedStatic[2] = (sensor_real_t)((((double)z - m_Config.G_off[2]) * gscaleZ)); - - #if BMI160_USE_TEMPCAL - float GOxyz[3]; - if (gyroTempCalibrator->approximateOffset(temperature, GOxyz)) { - Gxyz[0] = (sensor_real_t)((((double)x - GOxyz[0] - GOxyzStaticTempCompensated[0]) * gscaleX)); - Gxyz[1] = (sensor_real_t)((((double)y - GOxyz[1] - GOxyzStaticTempCompensated[1]) * gscaleY)); - Gxyz[2] = (sensor_real_t)((((double)z - GOxyz[2] - GOxyzStaticTempCompensated[2]) * gscaleZ)); - } - else - #endif - { - Gxyz[0] = gyroCalibratedStatic[0]; - Gxyz[1] = gyroCalibratedStatic[1]; - Gxyz[2] = gyroCalibratedStatic[2]; - } - remapGyroAccel(&Gxyz[0], &Gxyz[1], &Gxyz[2]); +#if BMI160_DEBUG + gyrReads++; +#endif + +#if BMI160_USE_TEMPCAL + bool restDetected = sfusion.getRestDetected(); + gyroTempCalibrator + ->updateGyroTemperatureCalibration(temperature, restDetected, x, y, z); +#endif + + sensor_real_t gyroCalibratedStatic[3]; + gyroCalibratedStatic[0] + = (sensor_real_t)((((double)x - m_Config.G_off[0]) * gscaleX)); + gyroCalibratedStatic[1] + = (sensor_real_t)((((double)y - m_Config.G_off[1]) * gscaleY)); + gyroCalibratedStatic[2] + = (sensor_real_t)((((double)z - m_Config.G_off[2]) * gscaleZ)); + +#if BMI160_USE_TEMPCAL + float GOxyz[3]; + if (gyroTempCalibrator->approximateOffset(temperature, GOxyz)) { + Gxyz[0] = (sensor_real_t)(( + ((double)x - GOxyz[0] - GOxyzStaticTempCompensated[0]) * gscaleX + )); + Gxyz[1] = (sensor_real_t)(( + ((double)y - GOxyz[1] - GOxyzStaticTempCompensated[1]) * gscaleY + )); + Gxyz[2] = (sensor_real_t)(( + ((double)z - GOxyz[2] - GOxyzStaticTempCompensated[2]) * gscaleZ + )); + } else +#endif + { + Gxyz[0] = gyroCalibratedStatic[0]; + Gxyz[1] = gyroCalibratedStatic[1]; + Gxyz[2] = gyroCalibratedStatic[2]; + } + remapGyroAccel(&Gxyz[0], &Gxyz[1], &Gxyz[2]); sfusion.updateGyro(Gxyz, (sensor_real_t)dtMicros * 1.0e-6); optimistic_yield(100); } -void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) { - #if BMI160_DEBUG - accReads++; - #endif - - Axyz[0] = (sensor_real_t)x; - Axyz[1] = (sensor_real_t)y; - Axyz[2] = (sensor_real_t)z; - applyAccelCalibrationAndScale(Axyz); - remapGyroAccel(&Axyz[0], &Axyz[1], &Axyz[2]); - lastAxyz[0] = Axyz[0]; - lastAxyz[1] = Axyz[1]; - lastAxyz[2] = Axyz[2]; +void BMI160Sensor::onAccelRawSample( + uint32_t dtMicros, + int16_t x, + int16_t y, + int16_t z +) { +#if BMI160_DEBUG + accReads++; +#endif + + Axyz[0] = (sensor_real_t)x; + Axyz[1] = (sensor_real_t)y; + Axyz[2] = (sensor_real_t)z; + applyAccelCalibrationAndScale(Axyz); + remapGyroAccel(&Axyz[0], &Axyz[1], &Axyz[2]); + lastAxyz[0] = Axyz[0]; + lastAxyz[1] = Axyz[1]; + lastAxyz[2] = Axyz[2]; sfusion.updateAcc(Axyz, (sensor_real_t)dtMicros * 1.0e-6); optimistic_yield(100); } void BMI160Sensor::onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) { - #if BMI160_DEBUG - magReads++; - #endif - - #if !USE_6_AXIS - Mxyz[0] = (sensor_real_t)x; - Mxyz[1] = (sensor_real_t)y; - Mxyz[2] = (sensor_real_t)z; - applyMagCalibrationAndScale(Mxyz); - remapMagnetometer(&Mxyz[0], &Mxyz[1], &Mxyz[2]); - sfusion.updateMag(Mxyz); - #endif +#if BMI160_DEBUG + magReads++; +#endif + +#if !USE_6_AXIS + Mxyz[0] = (sensor_real_t)x; + Mxyz[1] = (sensor_real_t)y; + Mxyz[2] = (sensor_real_t)z; + applyMagCalibrationAndScale(Mxyz); + remapMagnetometer(&Mxyz[0], &Mxyz[1], &Mxyz[2]); + sfusion.updateMag(Mxyz); +#endif } void BMI160Sensor::printTemperatureCalibrationState() { - const auto degCtoF = [](float degC) { return (degC * 9.0f/5.0f) + 32.0f; }; - - m_Logger.info("Sensor %i temperature calibration state:", sensorId); - m_Logger.info(" current temp: %0.4f C (%0.4f F)", temperature, degCtoF(temperature)); - auto printTemperatureRange = [&](const char* label, float min, float max) { - m_Logger.info(" %s: min %0.4f C max %0.4f C (min %0.4f F max %0.4f F)", - label, min, max, degCtoF(min), degCtoF(max) - ); - }; - printTemperatureRange("total range", - TEMP_CALIBRATION_MIN, - TEMP_CALIBRATION_MAX - ); - printTemperatureRange("calibrated range", - gyroTempCalibrator->config.minTemperatureRange, - gyroTempCalibrator->config.maxTemperatureRange - ); - m_Logger.info(" done: %0.1f%", gyroTempCalibrator->config.getCalibrationDonePercent()); + const auto degCtoF = [](float degC) { return (degC * 9.0f / 5.0f) + 32.0f; }; + + m_Logger.info("Sensor %i temperature calibration state:", sensorId); + m_Logger + .info(" current temp: %0.4f C (%0.4f F)", temperature, degCtoF(temperature)); + auto printTemperatureRange = [&](const char* label, float min, float max) { + m_Logger.info( + " %s: min %0.4f C max %0.4f C (min %0.4f F max %0.4f F)", + label, + min, + max, + degCtoF(min), + degCtoF(max) + ); + }; + printTemperatureRange("total range", TEMP_CALIBRATION_MIN, TEMP_CALIBRATION_MAX); + printTemperatureRange( + "calibrated range", + gyroTempCalibrator->config.minTemperatureRange, + gyroTempCalibrator->config.maxTemperatureRange + ); + m_Logger.info( + " done: %0.1f%", + gyroTempCalibrator->config.getCalibrationDonePercent() + ); } void BMI160Sensor::printDebugTemperatureCalibrationState() { - m_Logger.info("Sensor %i gyro odr %f hz, sensitivity %f lsb", - sensorId, - BMI160_ODR_GYR_HZ, - BMI160_GYRO_TYPICAL_SENSITIVITY_LSB - ); - m_Logger.info("Sensor %i temperature calibration matrix (tempC x y z):", sensorId); - m_Logger.info("BUF %i %i", sensorId, TEMP_CALIBRATION_BUFFER_SIZE); - m_Logger.info("SENS %i %f", sensorId, BMI160_GYRO_TYPICAL_SENSITIVITY_LSB); - m_Logger.info("DATA %i", sensorId); - for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { - m_Logger.info("%f %f %f %f", - gyroTempCalibrator->config.samples[i].t, - gyroTempCalibrator->config.samples[i].x, - gyroTempCalibrator->config.samples[i].y, - gyroTempCalibrator->config.samples[i].z - ); - } - m_Logger.info("END %i", sensorId); - m_Logger.info("y = %f + (%fx) + (%fxx) + (%fxxx)", UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cx), gyroTempCalibrator->config.cx[3]); - m_Logger.info("y = %f + (%fx) + (%fxx) + (%fxxx)", UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cy), gyroTempCalibrator->config.cy[3]); - m_Logger.info("y = %f + (%fx) + (%fxx) + (%fxxx)", UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cz), gyroTempCalibrator->config.cz[3]); -} -void BMI160Sensor::saveTemperatureCalibration() { - gyroTempCalibrator->saveConfig(); + m_Logger.info( + "Sensor %i gyro odr %f hz, sensitivity %f lsb", + sensorId, + BMI160_ODR_GYR_HZ, + BMI160_GYRO_TYPICAL_SENSITIVITY_LSB + ); + m_Logger.info("Sensor %i temperature calibration matrix (tempC x y z):", sensorId); + m_Logger.info("BUF %i %i", sensorId, TEMP_CALIBRATION_BUFFER_SIZE); + m_Logger.info("SENS %i %f", sensorId, BMI160_GYRO_TYPICAL_SENSITIVITY_LSB); + m_Logger.info("DATA %i", sensorId); + for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { + m_Logger.info( + "%f %f %f %f", + gyroTempCalibrator->config.samples[i].t, + gyroTempCalibrator->config.samples[i].x, + gyroTempCalibrator->config.samples[i].y, + gyroTempCalibrator->config.samples[i].z + ); + } + m_Logger.info("END %i", sensorId); + m_Logger.info( + "y = %f + (%fx) + (%fxx) + (%fxxx)", + UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cx), + gyroTempCalibrator->config.cx[3] + ); + m_Logger.info( + "y = %f + (%fx) + (%fxx) + (%fxxx)", + UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cy), + gyroTempCalibrator->config.cy[3] + ); + m_Logger.info( + "y = %f + (%fx) + (%fxx) + (%fxxx)", + UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cz), + gyroTempCalibrator->config.cz[3] + ); } +void BMI160Sensor::saveTemperatureCalibration() { gyroTempCalibrator->saveConfig(); } bool BMI160Sensor::getTemperature(float* out) { - // Middle value is 23 degrees C (0x0000) - #define BMI160_ZERO_TEMP_OFFSET 23 - // Temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 degrees C (0x7FFF) - constexpr float TEMP_STEP = 128. / 65535; - int16_t temp; - if (imu.getTemperature(&temp)) { - *out = (temp * TEMP_STEP) + BMI160_ZERO_TEMP_OFFSET; - return true; - } - return false; +// Middle value is 23 degrees C (0x0000) +#define BMI160_ZERO_TEMP_OFFSET 23 + // Temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 degrees C + // (0x7FFF) + constexpr float TEMP_STEP = 128. / 65535; + int16_t temp; + if (imu.getTemperature(&temp)) { + *out = (temp * TEMP_STEP) + BMI160_ZERO_TEMP_OFFSET; + return true; + } + return false; } void BMI160Sensor::applyAccelCalibrationAndScale(sensor_real_t Axyz[3]) { - //apply offsets (bias) and scale factors from Magneto - if (isAccelCalibrated) { - #if useFullCalibrationMatrix == true - float tmp[3]; - for (uint8_t i = 0; i < 3; i++) - tmp[i] = (Axyz[i] - m_Config.A_B[i]); - Axyz[0] = m_Config.A_Ainv[0][0] * tmp[0] + m_Config.A_Ainv[0][1] * tmp[1] + m_Config.A_Ainv[0][2] * tmp[2]; - Axyz[1] = m_Config.A_Ainv[1][0] * tmp[0] + m_Config.A_Ainv[1][1] * tmp[1] + m_Config.A_Ainv[1][2] * tmp[2]; - Axyz[2] = m_Config.A_Ainv[2][0] * tmp[0] + m_Config.A_Ainv[2][1] * tmp[1] + m_Config.A_Ainv[2][2] * tmp[2]; - #else - for (uint8_t i = 0; i < 3; i++) - Axyz[i] = (Axyz[i] - calibration->A_B[i]); - #endif - } - Axyz[0] *= BMI160_ASCALE; - Axyz[1] *= BMI160_ASCALE; - Axyz[2] *= BMI160_ASCALE; + // apply offsets (bias) and scale factors from Magneto + if (isAccelCalibrated) { +#if useFullCalibrationMatrix == true + float tmp[3]; + for (uint8_t i = 0; i < 3; i++) { + tmp[i] = (Axyz[i] - m_Config.A_B[i]); + } + Axyz[0] = m_Config.A_Ainv[0][0] * tmp[0] + m_Config.A_Ainv[0][1] * tmp[1] + + m_Config.A_Ainv[0][2] * tmp[2]; + Axyz[1] = m_Config.A_Ainv[1][0] * tmp[0] + m_Config.A_Ainv[1][1] * tmp[1] + + m_Config.A_Ainv[1][2] * tmp[2]; + Axyz[2] = m_Config.A_Ainv[2][0] * tmp[0] + m_Config.A_Ainv[2][1] * tmp[1] + + m_Config.A_Ainv[2][2] * tmp[2]; +#else + for (uint8_t i = 0; i < 3; i++) { + Axyz[i] = (Axyz[i] - calibration->A_B[i]); + } +#endif + } + Axyz[0] *= BMI160_ASCALE; + Axyz[1] *= BMI160_ASCALE; + Axyz[2] *= BMI160_ASCALE; } void BMI160Sensor::applyMagCalibrationAndScale(sensor_real_t Mxyz[3]) { - #if !USE_6_AXIS - //apply offsets and scale factors from Magneto - #if useFullCalibrationMatrix == true - float temp[3]; - for (uint8_t i = 0; i < 3; i++) - temp[i] = (Mxyz[i] - m_Config.M_B[i]); - Mxyz[0] = m_Config.M_Ainv[0][0] * temp[0] + m_Config.M_Ainv[0][1] * temp[1] + m_Config.M_Ainv[0][2] * temp[2]; - Mxyz[1] = m_Config.M_Ainv[1][0] * temp[0] + m_Config.M_Ainv[1][1] * temp[1] + m_Config.M_Ainv[1][2] * temp[2]; - Mxyz[2] = m_Config.M_Ainv[2][0] * temp[0] + m_Config.M_Ainv[2][1] * temp[1] + m_Config.M_Ainv[2][2] * temp[2]; - #else - for (i = 0; i < 3; i++) - Mxyz[i] = (Mxyz[i] - m_Config.M_B[i]); - #endif - #endif +#if !USE_6_AXIS +// apply offsets and scale factors from Magneto +#if useFullCalibrationMatrix == true + float temp[3]; + for (uint8_t i = 0; i < 3; i++) { + temp[i] = (Mxyz[i] - m_Config.M_B[i]); + } + Mxyz[0] = m_Config.M_Ainv[0][0] * temp[0] + m_Config.M_Ainv[0][1] * temp[1] + + m_Config.M_Ainv[0][2] * temp[2]; + Mxyz[1] = m_Config.M_Ainv[1][0] * temp[0] + m_Config.M_Ainv[1][1] * temp[1] + + m_Config.M_Ainv[1][2] * temp[2]; + Mxyz[2] = m_Config.M_Ainv[2][0] * temp[0] + m_Config.M_Ainv[2][1] * temp[1] + + m_Config.M_Ainv[2][2] * temp[2]; +#else + for (i = 0; i < 3; i++) { + Mxyz[i] = (Mxyz[i] - m_Config.M_B[i]); + } +#endif +#endif } bool BMI160Sensor::hasGyroCalibration() { - for (int i = 0; i < 3; i++) { - if (m_Config.G_off[i] != 0.0) - return true; - } - return false; + for (int i = 0; i < 3; i++) { + if (m_Config.G_off[i] != 0.0) { + return true; + } + } + return false; } bool BMI160Sensor::hasAccelCalibration() { - for (int i = 0; i < 3; i++) { - if (m_Config.A_B[i] != 0.0 || - m_Config.A_Ainv[0][i] != 0.0 || - m_Config.A_Ainv[1][i] != 0.0 || - m_Config.A_Ainv[2][i] != 0.0) - return true; - } - return false; + for (int i = 0; i < 3; i++) { + if (m_Config.A_B[i] != 0.0 || m_Config.A_Ainv[0][i] != 0.0 + || m_Config.A_Ainv[1][i] != 0.0 || m_Config.A_Ainv[2][i] != 0.0) { + return true; + } + } + return false; } bool BMI160Sensor::hasMagCalibration() { - for (int i = 0; i < 3; i++) { - if (m_Config.M_B[i] != 0.0 || - m_Config.M_Ainv[0][i] != 0.0 || - m_Config.M_Ainv[1][i] != 0.0 || - m_Config.M_Ainv[2][i] != 0.0) - return true; - } - return false; + for (int i = 0; i < 3; i++) { + if (m_Config.M_B[i] != 0.0 || m_Config.M_Ainv[0][i] != 0.0 + || m_Config.M_Ainv[1][i] != 0.0 || m_Config.M_Ainv[2][i] != 0.0) { + return true; + } + } + return false; } void BMI160Sensor::startCalibration(int calibrationType) { - ledManager.on(); + ledManager.on(); - maybeCalibrateGyro(); - maybeCalibrateAccel(); - maybeCalibrateMag(); + maybeCalibrateGyro(); + maybeCalibrateAccel(); + maybeCalibrateMag(); - m_Logger.debug("Saving the calibration data"); + m_Logger.debug("Saving the calibration data"); - SlimeVR::Configuration::SensorConfig config; - config.type = SlimeVR::Configuration::SensorConfigType::BMI160; - config.data.bmi160 = m_Config; - configuration.setSensor(sensorId, config); - configuration.save(); + SlimeVR::Configuration::SensorConfig config; + config.type = SlimeVR::Configuration::SensorConfigType::BMI160; + config.data.bmi160 = m_Config; + configuration.setSensor(sensorId, config); + configuration.save(); - m_Logger.debug("Saved the calibration data"); + m_Logger.debug("Saved the calibration data"); - m_Logger.info("Calibration data gathered, exiting calibration mode in..."); - constexpr uint8_t POST_CALIBRATION_DELAY_SEC = 3; - ledManager.on(); - for (uint8_t i = POST_CALIBRATION_DELAY_SEC; i > 0; i--) { - m_Logger.info("%i...", i); - delay(1000); - } + m_Logger.info("Calibration data gathered, exiting calibration mode in..."); + constexpr uint8_t POST_CALIBRATION_DELAY_SEC = 3; + ledManager.on(); + for (uint8_t i = POST_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } } void BMI160Sensor::maybeCalibrateGyro() { - #ifndef BMI160_CALIBRATION_GYRO_SECONDS - static_assert(false, "BMI160_CALIBRATION_GYRO_SECONDS not set in defines"); - #endif - - #if BMI160_CALIBRATION_GYRO_SECONDS == 0 - m_Logger.debug("Skipping gyro calibration"); - return; - #endif - - // Wait for sensor to calm down before calibration - constexpr uint8_t GYRO_CALIBRATION_DELAY_SEC = 3; - constexpr float GYRO_CALIBRATION_DURATION_SEC = BMI160_CALIBRATION_GYRO_SECONDS; - m_Logger.info("Put down the device and wait for baseline gyro reading calibration (%.1f seconds)", GYRO_CALIBRATION_DURATION_SEC); - ledManager.on(); - for (uint8_t i = GYRO_CALIBRATION_DELAY_SEC; i > 0; i--) { - m_Logger.info("%i...", i); - delay(1000); - } - ledManager.off(); - - if (!getTemperature(&temperature)) { - m_Logger.error("Error: can't read temperature"); - } - m_Config.temperature = temperature; - - #ifdef DEBUG_SENSOR - m_Logger.trace("Calibration temperature: %f", temperature); - #endif - - if (!imu.getGyroDrdy()) { - m_Logger.error("Fatal error: gyroscope drdy = 0 (dead?)"); - return; - } - - ledManager.pattern(100, 100, 3); - ledManager.on(); - m_Logger.info("Gyro calibration started..."); - - constexpr uint16_t gyroCalibrationSamples = - GYRO_CALIBRATION_DURATION_SEC / (BMI160_ODR_GYR_MICROS / 1e6); - int32_t rawGxyz[3] = {0}; - for (int i = 0; i < gyroCalibrationSamples; i++) { - imu.waitForGyroDrdy(); - - int16_t gx, gy, gz; - imu.getRotation(&gx, &gy, &gz); - rawGxyz[0] += gx; - rawGxyz[1] += gy; - rawGxyz[2] += gz; - } - ledManager.off(); - m_Config.G_off[0] = ((double)rawGxyz[0]) / gyroCalibrationSamples; - m_Config.G_off[1] = ((double)rawGxyz[1]) / gyroCalibrationSamples; - m_Config.G_off[2] = ((double)rawGxyz[2]) / gyroCalibrationSamples; - - #ifdef DEBUG_SENSOR - m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Config.G_off)); - #endif +#ifndef BMI160_CALIBRATION_GYRO_SECONDS + static_assert(false, "BMI160_CALIBRATION_GYRO_SECONDS not set in defines"); +#endif + +#if BMI160_CALIBRATION_GYRO_SECONDS == 0 + m_Logger.debug("Skipping gyro calibration"); + return; +#endif + + // Wait for sensor to calm down before calibration + constexpr uint8_t GYRO_CALIBRATION_DELAY_SEC = 3; + constexpr float GYRO_CALIBRATION_DURATION_SEC = BMI160_CALIBRATION_GYRO_SECONDS; + m_Logger.info( + "Put down the device and wait for baseline gyro reading calibration (%.1f " + "seconds)", + GYRO_CALIBRATION_DURATION_SEC + ); + ledManager.on(); + for (uint8_t i = GYRO_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } + ledManager.off(); + + if (!getTemperature(&temperature)) { + m_Logger.error("Error: can't read temperature"); + } + m_Config.temperature = temperature; + +#ifdef DEBUG_SENSOR + m_Logger.trace("Calibration temperature: %f", temperature); +#endif + + if (!imu.getGyroDrdy()) { + m_Logger.error("Fatal error: gyroscope drdy = 0 (dead?)"); + return; + } + + ledManager.pattern(100, 100, 3); + ledManager.on(); + m_Logger.info("Gyro calibration started..."); + + constexpr uint16_t gyroCalibrationSamples + = GYRO_CALIBRATION_DURATION_SEC / (BMI160_ODR_GYR_MICROS / 1e6); + int32_t rawGxyz[3] = {0}; + for (int i = 0; i < gyroCalibrationSamples; i++) { + imu.waitForGyroDrdy(); + + int16_t gx, gy, gz; + imu.getRotation(&gx, &gy, &gz); + rawGxyz[0] += gx; + rawGxyz[1] += gy; + rawGxyz[2] += gz; + } + ledManager.off(); + m_Config.G_off[0] = ((double)rawGxyz[0]) / gyroCalibrationSamples; + m_Config.G_off[1] = ((double)rawGxyz[1]) / gyroCalibrationSamples; + m_Config.G_off[2] = ((double)rawGxyz[2]) / gyroCalibrationSamples; + +#ifdef DEBUG_SENSOR + m_Logger.trace( + "Gyro calibration results: %f %f %f", + UNPACK_VECTOR_ARRAY(m_Config.G_off) + ); +#endif } void BMI160Sensor::maybeCalibrateAccel() { - #ifndef BMI160_ACCEL_CALIBRATION_METHOD - static_assert(false, "BMI160_ACCEL_CALIBRATION_METHOD not set in defines"); - #endif - - #if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_SKIP - m_Logger.debug("Skipping accelerometer calibration"); - return; - #endif - - MagnetoCalibration* magneto = new MagnetoCalibration(); - - // Blink calibrating led before user should rotate the sensor - #if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_ROTATION - m_Logger.info("After 3 seconds, Gently rotate the device while it's gathering data"); - #elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT - m_Logger.info("Put the device into 6 unique orientations (all sides), leave it still and do not hold/touch for 3 seconds each"); - #endif - constexpr uint8_t ACCEL_CALIBRATION_DELAY_SEC = 3; - ledManager.on(); - for (uint8_t i = ACCEL_CALIBRATION_DELAY_SEC; i > 0; i--) { - m_Logger.info("%i...", i); - delay(1000); - } - ledManager.off(); - - #if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_ROTATION - uint16_t accelCalibrationSamples = 200; - ledManager.pattern(100, 100, 6); - delay(100); - ledManager.on(); - m_Logger.debug("Gathering accelerometer data..."); - for (int i = 0; i < accelCalibrationSamples; i++) - { - int16_t ax, ay, az; - imu.getAcceleration(&ax, &ay, &az); - magneto->sample(ax, ay, az); - - delay(100); - } - ledManager.off(); - m_Logger.debug("Calculating accelerometer calibration data..."); - #elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT - RestDetectionParams calibrationRestDetectionParams; - calibrationRestDetectionParams.restMinTime = 3; - calibrationRestDetectionParams.restThAcc = 0.25f; - RestDetection calibrationRestDetection( - calibrationRestDetectionParams, - BMI160_ODR_GYR_MICROS * 1.0e-6, - BMI160_ODR_ACC_MICROS * 1.0e-6 - ); - - constexpr uint16_t expectedPositions = 6; - constexpr uint16_t numSamplesPerPosition = 96; - - uint16_t numPositionsRecorded = 0; - uint16_t numCurrentPositionSamples = 0; - bool waitForMotion = true; - - float* accelCalibrationChunk = new float[numSamplesPerPosition * 3]; - ledManager.pattern(100, 100, 6); - ledManager.on(); - m_Logger.info("Gathering accelerometer data..."); - m_Logger.info("Waiting for position %i, you can leave the device as is...", numPositionsRecorded + 1); - while (true) { - int16_t ax, ay, az; - imu.getAcceleration(&ax, &ay, &az); - sensor_real_t scaled[3]; - scaled[0] = ax * BMI160_ASCALE; - scaled[1] = ay * BMI160_ASCALE; - scaled[2] = az * BMI160_ASCALE; - - calibrationRestDetection.updateAcc(BMI160_ODR_ACC_MICROS * 1.0e-6, scaled); - - if (waitForMotion) { - if (!calibrationRestDetection.getRestDetected()) { - waitForMotion = false; - } - delayMicroseconds(BMI160_ODR_ACC_MICROS); - continue; - } - - if (calibrationRestDetection.getRestDetected()) { - const uint16_t i = numCurrentPositionSamples * 3; - accelCalibrationChunk[i + 0] = ax; - accelCalibrationChunk[i + 1] = ay; - accelCalibrationChunk[i + 2] = az; - numCurrentPositionSamples++; - - if (numCurrentPositionSamples >= numSamplesPerPosition) { - for (int i = 0; i < numSamplesPerPosition; i++) { - magneto->sample( - accelCalibrationChunk[i * 3 + 0], - accelCalibrationChunk[i * 3 + 1], - accelCalibrationChunk[i * 3 + 2] - ); - } - numPositionsRecorded++; - numCurrentPositionSamples = 0; - if (numPositionsRecorded < expectedPositions) { - ledManager.pattern(50, 50, 2); - ledManager.on(); - m_Logger.info("Recorded, waiting for position %i...", numPositionsRecorded + 1); - waitForMotion = true; - } - } - } else { - numCurrentPositionSamples = 0; - } - - if (numPositionsRecorded >= expectedPositions) break; - - delayMicroseconds(BMI160_ODR_ACC_MICROS); - } - ledManager.off(); - m_Logger.debug("Calculating accelerometer calibration data..."); - delete[] accelCalibrationChunk; - #endif - - float A_BAinv[4][3]; - magneto->current_calibration(A_BAinv); - delete magneto; - - m_Logger.debug("Finished calculating accelerometer calibration"); - m_Logger.debug("Accelerometer calibration matrix:"); - m_Logger.debug("{"); - for (int i = 0; i < 3; i++) { - m_Config.A_B[i] = A_BAinv[0][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[2][i] = A_BAinv[3][i]; - m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); - } - m_Logger.debug("}"); +#ifndef BMI160_ACCEL_CALIBRATION_METHOD + static_assert(false, "BMI160_ACCEL_CALIBRATION_METHOD not set in defines"); +#endif + +#if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_SKIP + m_Logger.debug("Skipping accelerometer calibration"); + return; +#endif + + MagnetoCalibration* magneto = new MagnetoCalibration(); + +// Blink calibrating led before user should rotate the sensor +#if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_ROTATION + m_Logger.info("After 3 seconds, Gently rotate the device while it's gathering data" + ); +#elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT + m_Logger.info( + "Put the device into 6 unique orientations (all sides), leave it still and do " + "not hold/touch for 3 seconds each" + ); +#endif + constexpr uint8_t ACCEL_CALIBRATION_DELAY_SEC = 3; + ledManager.on(); + for (uint8_t i = ACCEL_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } + ledManager.off(); + +#if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_ROTATION + uint16_t accelCalibrationSamples = 200; + ledManager.pattern(100, 100, 6); + delay(100); + ledManager.on(); + m_Logger.debug("Gathering accelerometer data..."); + for (int i = 0; i < accelCalibrationSamples; i++) { + int16_t ax, ay, az; + imu.getAcceleration(&ax, &ay, &az); + magneto->sample(ax, ay, az); + + delay(100); + } + ledManager.off(); + m_Logger.debug("Calculating accelerometer calibration data..."); +#elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT + RestDetectionParams calibrationRestDetectionParams; + calibrationRestDetectionParams.restMinTime = 3; + calibrationRestDetectionParams.restThAcc = 0.25f; + RestDetection calibrationRestDetection( + calibrationRestDetectionParams, + BMI160_ODR_GYR_MICROS * 1.0e-6, + BMI160_ODR_ACC_MICROS * 1.0e-6 + ); + + constexpr uint16_t expectedPositions = 6; + constexpr uint16_t numSamplesPerPosition = 96; + + uint16_t numPositionsRecorded = 0; + uint16_t numCurrentPositionSamples = 0; + bool waitForMotion = true; + + float* accelCalibrationChunk = new float[numSamplesPerPosition * 3]; + ledManager.pattern(100, 100, 6); + ledManager.on(); + m_Logger.info("Gathering accelerometer data..."); + m_Logger.info( + "Waiting for position %i, you can leave the device as is...", + numPositionsRecorded + 1 + ); + while (true) { + int16_t ax, ay, az; + imu.getAcceleration(&ax, &ay, &az); + sensor_real_t scaled[3]; + scaled[0] = ax * BMI160_ASCALE; + scaled[1] = ay * BMI160_ASCALE; + scaled[2] = az * BMI160_ASCALE; + + calibrationRestDetection.updateAcc(BMI160_ODR_ACC_MICROS * 1.0e-6, scaled); + + if (waitForMotion) { + if (!calibrationRestDetection.getRestDetected()) { + waitForMotion = false; + } + delayMicroseconds(BMI160_ODR_ACC_MICROS); + continue; + } + + if (calibrationRestDetection.getRestDetected()) { + const uint16_t i = numCurrentPositionSamples * 3; + accelCalibrationChunk[i + 0] = ax; + accelCalibrationChunk[i + 1] = ay; + accelCalibrationChunk[i + 2] = az; + numCurrentPositionSamples++; + + if (numCurrentPositionSamples >= numSamplesPerPosition) { + for (int i = 0; i < numSamplesPerPosition; i++) { + magneto->sample( + accelCalibrationChunk[i * 3 + 0], + accelCalibrationChunk[i * 3 + 1], + accelCalibrationChunk[i * 3 + 2] + ); + } + numPositionsRecorded++; + numCurrentPositionSamples = 0; + if (numPositionsRecorded < expectedPositions) { + ledManager.pattern(50, 50, 2); + ledManager.on(); + m_Logger.info( + "Recorded, waiting for position %i...", + numPositionsRecorded + 1 + ); + waitForMotion = true; + } + } + } else { + numCurrentPositionSamples = 0; + } + + if (numPositionsRecorded >= expectedPositions) { + break; + } + + delayMicroseconds(BMI160_ODR_ACC_MICROS); + } + ledManager.off(); + m_Logger.debug("Calculating accelerometer calibration data..."); + delete[] accelCalibrationChunk; +#endif + + float A_BAinv[4][3]; + magneto->current_calibration(A_BAinv); + delete magneto; + + m_Logger.debug("Finished calculating accelerometer calibration"); + m_Logger.debug("Accelerometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + m_Config.A_B[i] = A_BAinv[0][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[2][i] = A_BAinv[3][i]; + m_Logger.debug( + " %f, %f, %f, %f", + A_BAinv[0][i], + A_BAinv[1][i], + A_BAinv[2][i], + A_BAinv[3][i] + ); + } + m_Logger.debug("}"); } void BMI160Sensor::maybeCalibrateMag() { #if !USE_6_AXIS - #ifndef BMI160_CALIBRATION_MAG_SECONDS - static_assert(false, "BMI160_CALIBRATION_MAG_SECONDS not set in defines"); - #endif - - #if BMI160_CALIBRATION_MAG_SECONDS == 0 - m_Logger.debug("Skipping magnetometer calibration"); - return; - #endif - - MagnetoCalibration* magneto = new MagnetoCalibration(); - - constexpr uint8_t MAG_CALIBRATION_DELAY_SEC = 3; - constexpr float MAG_CALIBRATION_DURATION_SEC = BMI160_CALIBRATION_MAG_SECONDS; - m_Logger.info("After 3 seconds, rotate the device in figure 8 pattern while it's gathering data (%.1f seconds)", MAG_CALIBRATION_DURATION_SEC); - for (uint8_t i = MAG_CALIBRATION_DELAY_SEC; i > 0; i--) { - m_Logger.info("%i...", i); - delay(1000); - } - ledManager.pattern(100, 100, 9); - delay(100); - ledManager.on(); - m_Logger.debug("Gathering magnetometer data..."); - - constexpr float SAMPLE_DELAY_MS = 100.0f; - constexpr uint16_t magCalibrationSamples = - MAG_CALIBRATION_DURATION_SEC / (SAMPLE_DELAY_MS / 1e3f); - - uint8_t magdata[6]; - for (int i = 0; i < magCalibrationSamples; i++) { - ledManager.on(); - - int16_t mx, my, mz; - imu.getMagnetometerXYZBuffer(magdata); - getMagnetometerXYZFromBuffer(magdata, &mx, &my, &mz); - magneto->sample(mx, my, mz); - - ledManager.off(); - delay(SAMPLE_DELAY_MS); - } - ledManager.off(); - m_Logger.debug("Calculating magnetometer calibration data..."); - - float M_BAinv[4][3]; - magneto->current_calibration(M_BAinv); - delete magneto; - - m_Logger.debug("[INFO] Magnetometer calibration matrix:"); - m_Logger.debug("{"); - 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_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("}"); +#ifndef BMI160_CALIBRATION_MAG_SECONDS + static_assert(false, "BMI160_CALIBRATION_MAG_SECONDS not set in defines"); +#endif + +#if BMI160_CALIBRATION_MAG_SECONDS == 0 + m_Logger.debug("Skipping magnetometer calibration"); + return; +#endif + + MagnetoCalibration* magneto = new MagnetoCalibration(); + + constexpr uint8_t MAG_CALIBRATION_DELAY_SEC = 3; + constexpr float MAG_CALIBRATION_DURATION_SEC = BMI160_CALIBRATION_MAG_SECONDS; + m_Logger.info( + "After 3 seconds, rotate the device in figure 8 pattern while it's gathering " + "data (%.1f seconds)", + MAG_CALIBRATION_DURATION_SEC + ); + for (uint8_t i = MAG_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } + ledManager.pattern(100, 100, 9); + delay(100); + ledManager.on(); + m_Logger.debug("Gathering magnetometer data..."); + + constexpr float SAMPLE_DELAY_MS = 100.0f; + constexpr uint16_t magCalibrationSamples + = MAG_CALIBRATION_DURATION_SEC / (SAMPLE_DELAY_MS / 1e3f); + + uint8_t magdata[6]; + for (int i = 0; i < magCalibrationSamples; i++) { + ledManager.on(); + + int16_t mx, my, mz; + imu.getMagnetometerXYZBuffer(magdata); + getMagnetometerXYZFromBuffer(magdata, &mx, &my, &mz); + magneto->sample(mx, my, mz); + + ledManager.off(); + delay(SAMPLE_DELAY_MS); + } + ledManager.off(); + m_Logger.debug("Calculating magnetometer calibration data..."); + + float M_BAinv[4][3]; + magneto->current_calibration(M_BAinv); + delete magneto; + + m_Logger.debug("[INFO] Magnetometer calibration matrix:"); + m_Logger.debug("{"); + 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_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 } -void BMI160Sensor::remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z) { - remapAllAxis(AXIS_REMAP_GET_ALL_IMU(axisRemap), x, y, z); +void BMI160Sensor::remapGyroAccel( + sensor_real_t* x, + sensor_real_t* y, + sensor_real_t* z +) { + remapAllAxis(AXIS_REMAP_GET_ALL_IMU(axisRemap), x, y, z); } -void BMI160Sensor::remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z) { - remapAllAxis(AXIS_REMAP_GET_ALL_MAG(axisRemap), x, y, z); +void BMI160Sensor::remapMagnetometer( + sensor_real_t* x, + sensor_real_t* y, + sensor_real_t* z +) { + remapAllAxis(AXIS_REMAP_GET_ALL_MAG(axisRemap), x, y, z); } void BMI160Sensor::getRemappedRotation(int16_t* x, int16_t* y, int16_t* z) { - imu.getRotation(x, y, z); - remapAllAxis(AXIS_REMAP_GET_ALL_IMU(axisRemap), x, y, z); + imu.getRotation(x, y, z); + remapAllAxis(AXIS_REMAP_GET_ALL_IMU(axisRemap), x, y, z); } void BMI160Sensor::getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z) { - imu.getAcceleration(x, y, z); - remapAllAxis(AXIS_REMAP_GET_ALL_IMU(axisRemap), x, y, z); + imu.getAcceleration(x, y, z); + remapAllAxis(AXIS_REMAP_GET_ALL_IMU(axisRemap), x, y, z); } -void BMI160Sensor::getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z) { - #if BMI160_MAG_TYPE == BMI160_MAG_TYPE_HMC - // hmc5883l -> 0 msb 1 lsb - // XZY order - *x = ((int16_t)data[0] << 8) | data[1]; - *z = ((int16_t)data[2] << 8) | data[3]; - *y = ((int16_t)data[4] << 8) | data[5]; - #elif BMI160_MAG_TYPE == BMI160_MAG_TYPE_QMC - // qmc5883l -> 0 lsb 1 msb - // XYZ order - *x = ((int16_t)data[1] << 8) | data[0]; - *y = ((int16_t)data[3] << 8) | data[2]; - *z = ((int16_t)data[5] << 8) | data[4]; - #endif +void BMI160Sensor::getMagnetometerXYZFromBuffer( + uint8_t* data, + int16_t* x, + int16_t* y, + int16_t* z +) { +#if BMI160_MAG_TYPE == BMI160_MAG_TYPE_HMC + // hmc5883l -> 0 msb 1 lsb + // XZY order + *x = ((int16_t)data[0] << 8) | data[1]; + *z = ((int16_t)data[2] << 8) | data[3]; + *y = ((int16_t)data[4] << 8) | data[5]; +#elif BMI160_MAG_TYPE == BMI160_MAG_TYPE_QMC + // qmc5883l -> 0 lsb 1 msb + // XYZ order + *x = ((int16_t)data[1] << 8) | data[0]; + *y = ((int16_t)data[3] << 8) | data[2]; + *z = ((int16_t)data[5] << 8) | data[4]; +#endif } diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 55a8c895b..1b1cf11e9 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -1,52 +1,51 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_BMI160SENSOR_H #define SENSORS_BMI160SENSOR_H -#include "sensor.h" -#include "sensors/axisremap.h" -#include "magneto1.4.h" - #include -#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 - #define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ - #else - #define BMI160_GYRO_RATE BMI160_GYRO_RATE_200HZ - #endif +#if USE_6_AXIS +#define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ #else - #if USE_6_AXIS - #define BMI160_GYRO_RATE BMI160_GYRO_RATE_800HZ - #else - #define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ - #endif +#define BMI160_GYRO_RATE BMI160_GYRO_RATE_200HZ +#endif +#else +#if USE_6_AXIS +#define BMI160_GYRO_RATE BMI160_GYRO_RATE_800HZ +#else +#define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ +#endif #endif #define BMI160_GYRO_RANGE BMI160_GYRO_RANGE_1000 #define BMI160_GYRO_FILTER_MODE BMI160_DLPF_MODE_NORM @@ -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_ACC_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_ACC_HZ * 1e6f); +constexpr float BMI160_ODR_GYR_MICROS + = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_GYR_HZ * 1e6f); +constexpr float BMI160_ODR_ACC_MICROS + = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_ACC_HZ * 1e6f); #if !USE_6_AXIS -// note: this value only sets polling and fusion update rate - HMC is internally sampled at 75hz, QMC at 200hz +// note: this value only sets polling and fusion update rate - HMC is internally sampled +// at 75hz, QMC at 200hz #define BMI160_MAG_RATE BMI160_MAG_RATE_50HZ -constexpr float BMI160_ODR_MAG_HZ = (25.0f/32.0f) * (1 << (BMI160_MAG_RATE - 1)); -constexpr float BMI160_ODR_MAG_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_MAG_HZ * 1e6f); +constexpr float BMI160_ODR_MAG_HZ = (25.0f / 32.0f) * (1 << (BMI160_MAG_RATE - 1)); +constexpr float BMI160_ODR_MAG_MICROS + = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_MAG_HZ * 1e6f); #else constexpr float BMI160_ODR_MAG_HZ = 0; constexpr float BMI160_ODR_MAG_MICROS = 0; #endif -constexpr uint16_t BMI160_SETTINGS_MAX_ODR_HZ = max(max(BMI160_ODR_GYR_HZ, BMI160_ODR_ACC_HZ), BMI160_ODR_MAG_HZ); -constexpr uint16_t BMI160_SETTINGS_MAX_ODR_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_SETTINGS_MAX_ODR_HZ * 1e6f); +constexpr uint16_t BMI160_SETTINGS_MAX_ODR_HZ + = max(max(BMI160_ODR_GYR_HZ, BMI160_ODR_ACC_HZ), BMI160_ODR_MAG_HZ); +constexpr uint16_t BMI160_SETTINGS_MAX_ODR_MICROS + = BMI160_MAP_ODR_MICROS(1.0f / BMI160_SETTINGS_MAX_ODR_HZ * 1e6f); -constexpr float BMI160_FIFO_AVG_DATA_FRAME_LENGTH = ( - BMI160_SETTINGS_MAX_ODR_HZ * 1 + - BMI160_ODR_GYR_HZ * BMI160_FIFO_G_LEN + - BMI160_ODR_ACC_HZ * BMI160_FIFO_A_LEN + - BMI160_ODR_MAG_HZ * BMI160_FIFO_M_LEN -) / BMI160_SETTINGS_MAX_ODR_HZ; +constexpr float BMI160_FIFO_AVG_DATA_FRAME_LENGTH + = (BMI160_SETTINGS_MAX_ODR_HZ * 1 + BMI160_ODR_GYR_HZ * BMI160_FIFO_G_LEN + + BMI160_ODR_ACC_HZ * BMI160_FIFO_A_LEN + BMI160_ODR_MAG_HZ * BMI160_FIFO_M_LEN) + / BMI160_SETTINGS_MAX_ODR_HZ; constexpr float BMI160_FIFO_READ_BUFFER_SIZE_MICROS = 30000; -constexpr float BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES = - BMI160_SETTINGS_MAX_ODR_HZ * BMI160_FIFO_READ_BUFFER_SIZE_MICROS / 1e6f; +constexpr float BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES + = BMI160_SETTINGS_MAX_ODR_HZ * BMI160_FIFO_READ_BUFFER_SIZE_MICROS / 1e6f; constexpr uint16_t BMI160_FIFO_MAX_LENGTH = 1024; constexpr uint16_t BMI160_FIFO_READ_BUFFER_SIZE_BYTES = min( - (float)BMI160_FIFO_MAX_LENGTH - 64, - BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES * BMI160_FIFO_AVG_DATA_FRAME_LENGTH * 1.25f + (float)BMI160_FIFO_MAX_LENGTH - 64, + 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 std::pair BMI160_ACCEL_SENSITIVITY_LSB_MAP[] = { - {BMI160_ACCEL_RANGE_2G, 16384.0f}, - {BMI160_ACCEL_RANGE_4G, 8192.0f}, - {BMI160_ACCEL_RANGE_8G, 4096.0f}, - {BMI160_ACCEL_RANGE_16G, 2048.0f} -}; -constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB = BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second; -constexpr double BMI160_ASCALE = CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; +constexpr double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB + = (16.4f * (1 << BMI160_GYRO_RANGE)); + +constexpr std::pair BMI160_ACCEL_SENSITIVITY_LSB_MAP[] + = {{BMI160_ACCEL_RANGE_2G, 16384.0f}, + {BMI160_ACCEL_RANGE_4G, 8192.0f}, + {BMI160_ACCEL_RANGE_8G, 4096.0f}, + {BMI160_ACCEL_RANGE_16G, 2048.0f}}; +constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB + = BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second; +constexpr double BMI160_ASCALE + = CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; // Scale conversion steps: LSB/°/s -> °/s -> step/°/s -> step/rad/s -constexpr double BMI160_GSCALE = ((32768. / BMI160_GYRO_TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0); +constexpr double BMI160_GSCALE + = ((32768. / BMI160_GYRO_TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0); constexpr float targetSampleRateMs = 10.0f; constexpr uint32_t targetSampleRateMicros = (uint32_t)targetSampleRateMs * 1e3; -constexpr uint32_t BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP = - TEMP_CALIBRATION_SECONDS_PER_STEP / (BMI160_ODR_GYR_MICROS / 1e6); -static_assert(0x7FFF * BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP < 0x7FFFFFFF, "Temperature calibration sum overflow"); +constexpr uint32_t BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP + = TEMP_CALIBRATION_SECONDS_PER_STEP / (BMI160_ODR_GYR_MICROS / 1e6); +static_assert( + 0x7FFF * BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP < 0x7FFFFFFF, + "Temperature calibration sum overflow" +); class BMI160Sensor : public Sensor { - public: - static constexpr uint8_t Address = 0x68; - static constexpr auto TypeID = ImuID::BMI160; - - BMI160Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, int axisRemapParam) : - Sensor("BMI160Sensor", ImuID::BMI160, id, Address+addrSuppl, 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 motionLoop() override final; - void startCalibration(int calibrationType) override final; - void maybeCalibrateGyro(); - void maybeCalibrateAccel(); - void maybeCalibrateMag(); - - void printTemperatureCalibrationState() override final; - void printDebugTemperatureCalibrationState() override final; - void resetTemperatureCalibrationState() override final { - gyroTempCalibrator->reset(); - m_Logger.info("Temperature calibration state has been reset for sensorId:%i", sensorId); - }; - void saveTemperatureCalibration() override final; - - void applyAccelCalibrationAndScale(sensor_real_t Axyz[3]); - void applyMagCalibrationAndScale(sensor_real_t Mxyz[3]); - - bool hasGyroCalibration(); - bool hasAccelCalibration(); - bool hasMagCalibration(); - - void onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); - void onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); - void onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); - void readFIFO(); - - void getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z); - - void remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z); - void remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z); - void getRemappedRotation(int16_t* x, int16_t* y, int16_t* z); - void getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z); - - bool getTemperature(float* out); - - private: - BMI160 imu {}; - int axisRemap; - - SlimeVR::Sensors::SensorFusionRestDetect sfusion; - - // clock sync and sample timestamping - uint32_t sensorTime0 = 0; - uint32_t sensorTime1 = 0; - uint32_t localTime0 = 0; - uint32_t localTime1 = 0; - double sensorTimeRatio = 1; - double sensorTimeRatioEma = 1; - double sampleDtMicros = BMI160_ODR_GYR_MICROS; - uint32_t syncLatencyMicros = 0; - uint32_t samplesSinceClockSync = 0; - uint32_t timestamp0 = 0; - uint32_t timestamp1 = 0; - - // scheduling - uint32_t lastPollTime = micros(); - uint32_t lastClockPollTime = micros(); - #if BMI160_DEBUG - uint32_t cpuUsageMicros = 0; - uint32_t lastCpuUsagePrinted = 0; - uint32_t gyrReads = 0; - uint32_t accReads = 0; - uint32_t magReads = 0; - uint16_t numFIFODropped = 0; - uint16_t numFIFOFailedReads = 0; - #endif - - uint32_t lastRotationPacketSent = 0; - uint32_t lastTemperaturePacketSent = 0; - - struct BMI160FIFO { - uint8_t data[BMI160_FIFO_READ_BUFFER_SIZE_BYTES]; - uint16_t length; - } fifo {}; - float temperature = 0; - GyroTemperatureCalibrator* gyroTempCalibrator = nullptr; - sensor_real_t Gxyz[3] = {0}; - sensor_real_t Axyz[3] = {0}; - sensor_real_t Mxyz[3] = {0}; - sensor_real_t lastAxyz[3] = {0}; - - double gscaleX = BMI160_GSCALE; - double gscaleY = BMI160_GSCALE; - double gscaleZ = BMI160_GSCALE; - - double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0}; - - bool isGyroCalibrated = false; - bool isAccelCalibrated = false; - bool isMagCalibrated = false; - - SlimeVR::Configuration::BMI160SensorConfig m_Config = {}; +public: + static constexpr uint8_t Address = 0x68; + static constexpr auto TypeID = ImuID::BMI160; + + BMI160Sensor( + uint8_t id, + uint8_t addrSuppl, + float rotation, + uint8_t sclPin, + uint8_t sdaPin, + int axisRemapParam + ) + : Sensor( + "BMI160Sensor", + ImuID::BMI160, + id, + Address + addrSuppl, + 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 motionLoop() override final; + void startCalibration(int calibrationType) override final; + void maybeCalibrateGyro(); + void maybeCalibrateAccel(); + void maybeCalibrateMag(); + + void printTemperatureCalibrationState() override final; + void printDebugTemperatureCalibrationState() override final; + void resetTemperatureCalibrationState() override final { + gyroTempCalibrator->reset(); + m_Logger.info( + "Temperature calibration state has been reset for sensorId:%i", + sensorId + ); + }; + void saveTemperatureCalibration() override final; + + void applyAccelCalibrationAndScale(sensor_real_t Axyz[3]); + void applyMagCalibrationAndScale(sensor_real_t Mxyz[3]); + + bool hasGyroCalibration(); + bool hasAccelCalibration(); + bool hasMagCalibration(); + + void onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); + void onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); + void onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); + void readFIFO(); + + void + getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z); + + void remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z); + void remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z); + void getRemappedRotation(int16_t* x, int16_t* y, int16_t* z); + void getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z); + + bool getTemperature(float* out); + +private: + BMI160 imu{}; + int axisRemap; + + SlimeVR::Sensors::SensorFusionRestDetect sfusion; + + // clock sync and sample timestamping + uint32_t sensorTime0 = 0; + uint32_t sensorTime1 = 0; + uint32_t localTime0 = 0; + uint32_t localTime1 = 0; + double sensorTimeRatio = 1; + double sensorTimeRatioEma = 1; + double sampleDtMicros = BMI160_ODR_GYR_MICROS; + uint32_t syncLatencyMicros = 0; + uint32_t samplesSinceClockSync = 0; + uint32_t timestamp0 = 0; + uint32_t timestamp1 = 0; + + // scheduling + uint32_t lastPollTime = micros(); + uint32_t lastClockPollTime = micros(); +#if BMI160_DEBUG + uint32_t cpuUsageMicros = 0; + uint32_t lastCpuUsagePrinted = 0; + uint32_t gyrReads = 0; + uint32_t accReads = 0; + uint32_t magReads = 0; + uint16_t numFIFODropped = 0; + uint16_t numFIFOFailedReads = 0; +#endif + + uint32_t lastRotationPacketSent = 0; + uint32_t lastTemperaturePacketSent = 0; + + struct BMI160FIFO { + uint8_t data[BMI160_FIFO_READ_BUFFER_SIZE_BYTES]; + uint16_t length; + } fifo{}; + float temperature = 0; + GyroTemperatureCalibrator* gyroTempCalibrator = nullptr; + sensor_real_t Gxyz[3] = {0}; + sensor_real_t Axyz[3] = {0}; + sensor_real_t Mxyz[3] = {0}; + sensor_real_t lastAxyz[3] = {0}; + + double gscaleX = BMI160_GSCALE; + double gscaleY = BMI160_GSCALE; + double gscaleZ = BMI160_GSCALE; + + double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0}; + + bool isGyroCalibrated = false; + bool isAccelCalibrated = false; + bool isMagCalibrated = false; + + SlimeVR::Configuration::BMI160SensorConfig m_Config = {}; }; #endif diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index def918c48..086759f84 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -1,72 +1,80 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "bno055sensor.h" -#include "globals.h" + #include "GlobalVars.h" +#include "globals.h" void BNO055Sensor::motionSetup() { - imu = Adafruit_BNO055(sensorId, addr); - delay(3000); + imu = Adafruit_BNO055(sensorId, addr); + 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); - ledManager.pattern(50, 50, 200); - return; - } + { + m_Logger.fatal("Can't connect to BNO055 at address 0x%02x", addr); + ledManager.pattern(50, 50, 200); + return; + } - delay(1000); - imu.setExtCrystalUse(true); //Adafruit BNO055's use external crystal. Enable it, otherwise it does not work. - imu.setAxisRemap(Adafruit_BNO055::REMAP_CONFIG_P0); - imu.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P0); - m_Logger.info("Connected to BNO055 at address 0x%02x", addr); + delay(1000); + imu.setExtCrystalUse(true); // Adafruit BNO055's use external crystal. Enable it, + // otherwise it does not work. + imu.setAxisRemap(Adafruit_BNO055::REMAP_CONFIG_P0); + 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 accel = imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - Vector3 mag = imu.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); + { + Vector3 gyro = imu.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); + Vector3 accel = imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + 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 - setFusedRotation(imu.getQuat()); - hadData = true; + // TODO Optimize a bit with setting rawQuat directly + setFusedRotation(imu.getQuat()); + 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) {} diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 19054388b..1e348bfd7 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -1,49 +1,63 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_BNO055SENSOR_H #define SENSORS_BNO055SENSOR_H -#include "sensor.h" - #include -class BNO055Sensor : public Sensor -{ -public: - static constexpr auto TypeID = ImuID::BNO055; - static constexpr uint8_t Address = 0x28; +#include "sensor.h" - 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; +class BNO055Sensor : public Sensor { +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 + ) + : 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; - SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {}; + Adafruit_BNO055 imu; + SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {}; }; #endif diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 7699c390b..5fa03281e 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -1,231 +1,286 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "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)) { - m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr); - ledManager.pattern(50, 50, 200); - return; - } - - m_Logger.info("Connected to %s on 0x%02x. " - "Info: SW Version Major: 0x%02x " - "SW Version Minor: 0x%02x " - "SW Part Number: 0x%02x " - "SW Build Number: 0x%02x " - "SW Version Patch: 0x%02x", - getIMUNameByType(sensorType), - addr, - imu.swMajor, - imu.swMinor, - imu.swPartNumber, - imu.swBuildNumber, - imu.swVersionPatch - ); - - this->imu.enableLinearAccelerometer(10); - - SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorConfig.type) { - case SlimeVR::Configuration::SensorConfigType::BNO0XX: - m_Config = sensorConfig.data.bno0XX; - magStatus = m_Config.magEnabled ? MagnetometerStatus::MAG_ENABLED - : MagnetometerStatus::MAG_DISABLED; - break; - default: - // Ignore lack of config for BNO, by default use from FW build - magStatus = USE_6_AXIS ? MagnetometerStatus::MAG_DISABLED - : MagnetometerStatus::MAG_ENABLED; - break; - } - - if(!isMagEnabled()) { - if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { - imu.enableARVRStabilizedGameRotationVector(10); - } else { - imu.enableGameRotationVector(10); - } - } else { - if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { - imu.enableARVRStabilizedRotationVector(10); - } else { - imu.enableRotationVector(10); - } - } + if (!imu.begin(addr, Wire, m_IntPin)) { + m_Logger.fatal( + "Can't connect to %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + ledManager.pattern(50, 50, 200); + return; + } + + m_Logger.info( + "Connected to %s on 0x%02x. " + "Info: SW Version Major: 0x%02x " + "SW Version Minor: 0x%02x " + "SW Part Number: 0x%02x " + "SW Build Number: 0x%02x " + "SW Version Patch: 0x%02x", + getIMUNameByType(sensorType), + addr, + imu.swMajor, + imu.swMinor, + imu.swPartNumber, + imu.swBuildNumber, + imu.swVersionPatch + ); + + this->imu.enableLinearAccelerometer(10); + + SlimeVR::Configuration::SensorConfig sensorConfig + = configuration.getSensor(sensorId); + // If no compatible calibration data is found, the calibration data will just be + // zero-ed out + switch (sensorConfig.type) { + case SlimeVR::Configuration::SensorConfigType::BNO0XX: + m_Config = sensorConfig.data.bno0XX; + magStatus = m_Config.magEnabled ? MagnetometerStatus::MAG_ENABLED + : MagnetometerStatus::MAG_DISABLED; + break; + default: + // Ignore lack of config for BNO, by default use from FW build + magStatus = USE_6_AXIS ? MagnetometerStatus::MAG_DISABLED + : MagnetometerStatus::MAG_ENABLED; + break; + } + + if (!isMagEnabled()) { + if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) + && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedGameRotationVector(10); + } else { + imu.enableGameRotationVector(10); + } + } else { + if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) + && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedRotationVector(10); + } else { + imu.enableRotationVector(10); + } + } #if ENABLE_INSPECTION - imu.enableRawGyro(10); - imu.enableRawAccelerometer(10); - imu.enableRawMagnetometer(10); + imu.enableRawGyro(10); + imu.enableRawAccelerometer(10); + imu.enableRawMagnetometer(10); #endif - lastReset = 0; - lastData = millis(); - working = true; - configured = true; + lastReset = 0; + lastData = millis(); + working = true; + configured = true; } -void BNO080Sensor::motionLoop() -{ - //Look for reports from the IMU - while (imu.dataAvailable()) - { - hadData = true; +void BNO080Sensor::motionLoop() { + // Look for reports from the IMU + while (imu.dataAvailable()) { + hadData = true; #if ENABLE_INSPECTION - { - int16_t rX = imu.getRawGyroX(); - int16_t rY = imu.getRawGyroY(); - int16_t rZ = imu.getRawGyroZ(); - uint8_t rA = imu.getGyroAccuracy(); - - int16_t aX = imu.getRawAccelX(); - int16_t aY = imu.getRawAccelY(); - int16_t aZ = imu.getRawAccelZ(); - uint8_t aA = imu.getAccelAccuracy(); - - int16_t mX = imu.getRawMagX(); - int16_t mY = imu.getRawMagY(); - int16_t mZ = imu.getRawMagZ(); - uint8_t mA = imu.getMagAccuracy(); - - networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA); - } + { + int16_t rX = imu.getRawGyroX(); + int16_t rY = imu.getRawGyroY(); + int16_t rZ = imu.getRawGyroZ(); + uint8_t rA = imu.getGyroAccuracy(); + + int16_t aX = imu.getRawAccelX(); + int16_t aY = imu.getRawAccelY(); + int16_t aZ = imu.getRawAccelZ(); + uint8_t aA = imu.getAccelAccuracy(); + + int16_t mX = imu.getRawMagX(); + int16_t mY = imu.getRawMagY(); + int16_t mZ = imu.getRawMagZ(); + uint8_t mA = imu.getMagAccuracy(); + + networkConnection.sendInspectionRawIMUData( + sensorId, + rX, + rY, + rZ, + rA, + aX, + aY, + aZ, + aA, + mX, + mY, + mZ, + mA + ); + } #endif - lastReset = 0; - lastData = millis(); - - if(!isMagEnabled()) { - if (imu.hasNewGameQuat()) // New quaternion if context - { - Quat nRotation; - imu.getGameQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, calibrationAccuracy); - - setFusedRotation(nRotation); - // Leave new quaternion if context open, it's closed later - } - } else { - - if (imu.hasNewQuat()) // New quaternion if context - { - 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 - } - - // Continuation of the new quaternion if context, used for both 6 and 9 axis + lastReset = 0; + lastData = millis(); + + if (!isMagEnabled()) { + if (imu.hasNewGameQuat()) // New quaternion if context + { + Quat nRotation; + imu.getGameQuat( + nRotation.x, + nRotation.y, + nRotation.z, + nRotation.w, + calibrationAccuracy + ); + + setFusedRotation(nRotation); + // Leave new quaternion if context open, it's closed later + } + } else { + if (imu.hasNewQuat()) // New quaternion if context + { + 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 + } + + // Continuation of the new quaternion if context, used for both 6 and 9 axis #if SEND_ACCELERATION - { - uint8_t acc; - Vector3 nAccel; - imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc); - setAcceleration(nAccel); - } -#endif // SEND_ACCELERATION - - if (imu.getTapDetected()) - { - tap = imu.getTapDetector(); - } - if (m_IntPin == 255 || imu.I2CTimedOut()) - break; - } - if (lastData + 1000 < millis() && configured) - { - while(true) { - BNO080Error error = imu.readError(); - if(error.error_source == 255) - break; - lastError = error; - m_Logger.error("BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d", - error.severity, error.error_sequence_number, error.error_source, error.error, error.error_module, error.error_code); - } - 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("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); - } + { + uint8_t acc; + Vector3 nAccel; + imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc); + setAcceleration(nAccel); + } +#endif // SEND_ACCELERATION + + if (imu.getTapDetected()) { + tap = imu.getTapDetector(); + } + if (m_IntPin == 255 || imu.I2CTimedOut()) { + break; + } + } + if (lastData + 1000 < millis() && configured) { + while (true) { + BNO080Error error = imu.readError(); + if (error.error_source == 255) { + break; + } + lastError = error; + m_Logger.error( + "BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: " + "%d", + error.severity, + error.error_sequence_number, + error.error_source, + error.error, + error.error_module, + error.error_code + ); + } + 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( + "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() -{ - return lastReset > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; +SensorStatus BNO080Sensor::getSensorState() { + return lastReset > 0 ? SensorStatus::SENSOR_ERROR + : isWorking() ? SensorStatus::SENSOR_OK + : SensorStatus::SENSOR_OFFLINE; } -void BNO080Sensor::sendData() -{ - if (newFusedRotation) - { - newFusedRotation = false; - networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); +void BNO080Sensor::sendData() { + if (newFusedRotation) { + newFusedRotation = false; + 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) - { - newAcceleration = false; - networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration); - } + if (newAcceleration) { + newAcceleration = false; + networkConnection.sendSensorAcceleration( + this->sensorId, + this->acceleration + ); + } #endif - } + } - if (tap != 0) - { - networkConnection.sendSensorTap(sensorId, tap); - tap = 0; - } + if (tap != 0) { + networkConnection.sendSensorTap(sensorId, tap); + tap = 0; + } } -void BNO080Sensor::setFlag(uint16_t flagId, bool state) -{ - if(flagId == FLAG_SENSOR_BNO0XX_MAG_ENABLED) { - m_Config.magEnabled = state; - magStatus = state ? MagnetometerStatus::MAG_ENABLED +void BNO080Sensor::setFlag(uint16_t flagId, bool state) { + if (flagId == FLAG_SENSOR_BNO0XX_MAG_ENABLED) { + m_Config.magEnabled = state; + 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) -{ - // BNO does automatic calibration, - // it's always enabled except accelerometer - // that is disabled 30 seconds after startup +void BNO080Sensor::startCalibration(int calibrationType) { + // BNO does automatic calibration, + // it's always enabled except accelerometer + // that is disabled 30 seconds after startup } diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 3f04a7507..57f797b79 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -1,91 +1,146 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_BNO080SENSOR_H #define SENSORS_BNO080SENSOR_H -#include "sensor.h" #include +#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 uint8_t Address = 0x4a; - - BNO080Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, 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 sendData() override final; - void startCalibration(int calibrationType) override final; - SensorStatus getSensorState() override final; - void setFlag(uint16_t flagId, bool state) override final; + static constexpr auto TypeID = ImuID::BNO080; + static constexpr uint8_t Address = 0x4a; + + BNO080Sensor( + uint8_t id, + uint8_t addrSuppl, + float rotation, + uint8_t sclPin, + 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 sendData() override final; + void startCalibration(int calibrationType) override final; + SensorStatus getSensorState() override final; + void setFlag(uint16_t flagId, bool state) override final; protected: - // 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) - : Sensor(sensorName, imuId, id, Address+addrSuppl, rotation, sclPin, sdaPin), m_IntPin(intPin) {}; + // 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 + ) + : Sensor(sensorName, imuId, id, Address + addrSuppl, rotation, sclPin, sdaPin) + , m_IntPin(intPin){}; + private: - BNO080 imu{}; - - uint8_t m_IntPin; - - uint8_t tap; - unsigned long lastData = 0; - uint8_t lastReset = 0; - BNO080Error lastError{}; - SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {}; - - // Magnetometer specific members - Quat magQuaternion{}; - uint8_t magCalibrationAccuracy = 0; - float magneticAccuracyEstimate = 999; - bool newMagData = false; - bool configured = false; + BNO080 imu{}; + + uint8_t m_IntPin; + + uint8_t tap; + unsigned long lastData = 0; + uint8_t lastReset = 0; + BNO080Error lastError{}; + SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {}; + + // Magnetometer specific members + Quat magQuaternion{}; + uint8_t magCalibrationAccuracy = 0; + float magneticAccuracyEstimate = 999; + bool newMagData = false; + bool configured = false; }; -class BNO085Sensor : public BNO080Sensor -{ +class BNO085Sensor : public BNO080Sensor { public: - static constexpr auto TypeID = ImuID::BNO085; - BNO085Sensor(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) {}; + static constexpr auto TypeID = ImuID::BNO085; + BNO085Sensor( + 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; - BNO086Sensor(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) {}; + static constexpr auto TypeID = ImuID::BNO086; + BNO086Sensor( + 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 diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index c3159a268..e33476836 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -1,760 +1,989 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 S.J. Remington & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 S.J. Remington & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "icm20948sensor.h" -#include "calibration.h" + #include + #include "GlobalVars.h" +#include "calibration.h" -// seconds after previous save (from start) when calibration (DMP Bias) data will be saved to NVS. Increments through the list then stops; to prevent unwelcome eeprom wear. -int bias_save_periods[] = { 120, 180, 300, 600, 600 }; // 2min + 3min + 5min + 10min + 10min (no more saves after 30min) +// seconds after previous save (from start) when calibration (DMP Bias) data will be +// saved to NVS. Increments through the list then stops; to prevent unwelcome eeprom +// wear. +int bias_save_periods[] + = {120, 180, 300, 600, 600 +}; // 2min + 3min + 5min + 10min + 10min (no more saves after 30min) #define ACCEL_SENSITIVITY_4G 8192.0f // Accel scale conversion steps: LSB/G -> G -> m/s^2 -constexpr float ASCALE_4G = ((32768. / ACCEL_SENSITIVITY_4G) / 32768.) * CONST_EARTH_GRAVITY; - -void ICM20948Sensor::motionSetup() -{ - connectSensor(); - startDMP(); - loadCalibration(); - startMotionLoop(); - startCalibrationAutoSave(); +constexpr float ASCALE_4G + = ((32768. / ACCEL_SENSITIVITY_4G) / 32768.) * CONST_EARTH_GRAVITY; + +void ICM20948Sensor::motionSetup() { + connectSensor(); + startDMP(); + loadCalibration(); + startMotionLoop(); + startCalibrationAutoSave(); } -void ICM20948Sensor::motionLoop() -{ +void ICM20948Sensor::motionLoop() { #if ENABLE_INSPECTION - { - (void)imu.getAGMT(); - - float rX = imu.gyrX(); - float rY = imu.gyrY(); - float rZ = imu.gyrZ(); + { + (void)imu.getAGMT(); + + float rX = imu.gyrX(); + float rY = imu.gyrY(); + float rZ = imu.gyrZ(); + + float aX = imu.accX(); + float aY = imu.accY(); + float aZ = imu.accZ(); + + float mX = imu.magX(); + float mY = imu.magY(); + float mZ = imu.magZ(); + + networkConnection.sendInspectionRawIMUData( + sensorId, + rX, + rY, + rZ, + 255, + aX, + aY, + aZ, + 255, + mX, + mY, + mZ, + 255 + ); + } +#endif - float aX = imu.accX(); - float aY = imu.accY(); - float aZ = imu.accZ(); + hasdata = false; + readFIFOToEnd(); + readRotation(); + checkSensorTimeout(); + // Performance Test + /* + if (hasdata) cntrounds ++; + + if ((lastData2 + 2000) <= millis()) + { + lastData2 = millis(); + printf("Data worked/2sec: %d, Dataframes: %d\n", cntrounds,cntbuf); + cntbuf = 0; + cntrounds = 0; + } + */ +} - float mX = imu.magX(); - float mY = imu.magY(); - float mZ = imu.magZ(); +void ICM20948Sensor::readFIFOToEnd() { + ICM_20948_Status_e readStatus = imu.readDMPdataFromFIFO(&dmpDataTemp); - networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); - } +#ifdef DEBUG_SENSOR + { m_Logger.trace("e0x%02x", readStatus); } #endif - hasdata = false; - readFIFOToEnd(); - readRotation(); - checkSensorTimeout(); -// Performance Test -/* - if (hasdata) cntrounds ++; - - if ((lastData2 + 2000) <= millis()) - { - lastData2 = millis(); - printf("Data worked/2sec: %d, Dataframes: %d\n", cntrounds,cntbuf); - cntbuf = 0; - cntrounds = 0; - } -*/ + if (readStatus == ICM_20948_Stat_Ok) { + dmpData = dmpDataTemp; + // Performance Test + // cntbuf ++; + hasdata = true; + hadData = true; + readFIFOToEnd(); + } } -void ICM20948Sensor::readFIFOToEnd() -{ - ICM_20948_Status_e readStatus = imu.readDMPdataFromFIFO(&dmpDataTemp); - - #ifdef DEBUG_SENSOR - { - m_Logger.trace("e0x%02x", readStatus); - } - #endif - - if(readStatus == ICM_20948_Stat_Ok) - { - dmpData = dmpDataTemp; -// Performance Test -// cntbuf ++; - hasdata = true; - hadData = true; - readFIFOToEnd(); - } -} +void ICM20948Sensor::sendData() { + if (newFusedRotation) { + newFusedRotation = false; -void ICM20948Sensor::sendData() -{ - if(newFusedRotation) - { - newFusedRotation = false; - - #if(USE_6_AXIS) - { - networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, 0); - } - #else - { - networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy); - } - #endif +#if (USE_6_AXIS) + { + networkConnection + .sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, 0); + } +#else + { + networkConnection.sendRotationData( + sensorId, + &fusedRotation, + DATA_TYPE_NORMAL, + dmpData.Quat9.Data.Accuracy + ); + } +#endif #if SEND_ACCELERATION - if (newAcceleration) { - newAcceleration = false; - networkConnection.sendSensorAcceleration(sensorId, acceleration); - } + if (newAcceleration) { + newAcceleration = false; + networkConnection.sendSensorAcceleration(sensorId, acceleration); + } #endif - } + } } -void ICM20948Sensor::startCalibration(int calibrationType) -{ - // 20948 does continuous calibration - saveCalibration(false); +void ICM20948Sensor::startCalibration(int calibrationType) { + // 20948 does continuous calibration + saveCalibration(false); } -void ICM20948Sensor::startCalibrationAutoSave() -{ - #if SAVE_BIAS - globalTimer.in(bias_save_periods[0] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->saveCalibration(true); return false; }, this); - #endif +void ICM20948Sensor::startCalibrationAutoSave() { +#if SAVE_BIAS + globalTimer.in( + bias_save_periods[0] * 1000, + [](void* arg) -> bool { + ((ICM20948Sensor*)arg)->saveCalibration(true); + return false; + }, + this + ); +#endif } -void ICM20948Sensor::startDMP() -{ +void ICM20948Sensor::startDMP() { #ifdef ESP32 - #if ESP32C3 - #define ICM20948_ODRGYR 1 - #define ICM20948_ODRAXL 1 - #else - #define ICM20948_ODRGYR 1 - #define ICM20948_ODRAXL 1 - #endif +#if ESP32C3 +#define ICM20948_ODRGYR 1 +#define ICM20948_ODRAXL 1 +#else +#define ICM20948_ODRGYR 1 +#define ICM20948_ODRAXL 1 +#endif +#else +#define ICM20948_ODRGYR 1 +#define ICM20948_ODRAXL 1 +#endif + + if (imu.initializeDMP() == ICM_20948_Stat_Ok) { + m_Logger.debug("DMP initialized"); + } else { + m_Logger.fatal("Failed to initialize DMP"); + return; + } + +#if (USE_6_AXIS) + { + m_Logger.debug("Using 6 axis configuration"); + if (imu.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) + == ICM_20948_Stat_Ok) { + m_Logger.debug("Enabled DMP sensor for game rotation vector"); + } else { + m_Logger.fatal("Failed to enable DMP sensor for game rotation vector"); + return; + } + } +#else + { + m_Logger.debug("Using 9 axis configuration"); + if (imu.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok) { + m_Logger.debug("Enabled DMP sensor for sensor orientation"); + } else { + m_Logger.fatal("Failed to enable DMP sensor orientation"); + return; + } + } +#endif + +#if (SEND_ACCELERATION) + if (imu.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) + == ICM_20948_Stat_Ok) { + m_Logger.debug("Enabled DMP sensor for accelerometer"); + } else { + m_Logger.fatal("Failed to enable DMP sensor for accelerometer"); + return; + } +#endif + + // Might need to set up other DMP functions later, just Quad6/Quad9/Accel for now + +#if (USE_6_AXIS) + { + if (imu.setDMPODRrate(DMP_ODR_Reg_Quat6, ICM20948_ODRGYR) + == ICM_20948_Stat_Ok) { + m_Logger.debug("Set Quat6 to 100Hz frequency"); + } else { + m_Logger.fatal("Failed to set Quat6 to 100Hz frequency"); + return; + } + } #else - #define ICM20948_ODRGYR 1 - #define ICM20948_ODRAXL 1 + { + if (imu.setDMPODRrate(DMP_ODR_Reg_Quat9, ICM20948_ODRGYR) + == ICM_20948_Stat_Ok) { + m_Logger.debug("Set Quat9 to 100Hz frequency"); + } else { + m_Logger.fatal("Failed to set Quat9 to 100Hz frequency"); + return; + } + } +#endif + +#if (SEND_ACCELERATION) + if (this->imu.setDMPODRrate(DMP_ODR_Reg_Accel, ICM20948_ODRAXL) + == ICM_20948_Stat_Ok) { + this->m_Logger.debug("Set Accel to 100Hz frequency"); + } else { + this->m_Logger.fatal("Failed to set Accel to 100Hz frequency"); + return; + } #endif - if(imu.initializeDMP() == ICM_20948_Stat_Ok) - { - m_Logger.debug("DMP initialized"); - } - else - { - m_Logger.fatal("Failed to initialize DMP"); - return; - } - - #if(USE_6_AXIS) - { - m_Logger.debug("Using 6 axis configuration"); - if(imu.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok) - { - m_Logger.debug("Enabled DMP sensor for game rotation vector"); - } - else - { - m_Logger.fatal("Failed to enable DMP sensor for game rotation vector"); - return; - } - } - #else - { - m_Logger.debug("Using 9 axis configuration"); - if(imu.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok) - { - m_Logger.debug("Enabled DMP sensor for sensor orientation"); - } - else - { - m_Logger.fatal("Failed to enable DMP sensor orientation"); - return; - } - } - #endif - - #if(SEND_ACCELERATION) - if (imu.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok) - { - m_Logger.debug("Enabled DMP sensor for accelerometer"); - } - else - { - m_Logger.fatal("Failed to enable DMP sensor for accelerometer"); - return; - } - #endif - - // Might need to set up other DMP functions later, just Quad6/Quad9/Accel for now - - #if(USE_6_AXIS) - { - if(imu.setDMPODRrate(DMP_ODR_Reg_Quat6, ICM20948_ODRGYR) == ICM_20948_Stat_Ok) - { - m_Logger.debug("Set Quat6 to 100Hz frequency"); - } - else - { - m_Logger.fatal("Failed to set Quat6 to 100Hz frequency"); - return; - } - } - #else - { - if(imu.setDMPODRrate(DMP_ODR_Reg_Quat9, ICM20948_ODRGYR) == ICM_20948_Stat_Ok) - { - m_Logger.debug("Set Quat9 to 100Hz frequency"); - } - else - { - m_Logger.fatal("Failed to set Quat9 to 100Hz frequency"); - return; - } - } - #endif - - #if(SEND_ACCELERATION) - if (this->imu.setDMPODRrate(DMP_ODR_Reg_Accel, ICM20948_ODRAXL) == ICM_20948_Stat_Ok) - { - this->m_Logger.debug("Set Accel to 100Hz frequency"); - } - else - { - this->m_Logger.fatal("Failed to set Accel to 100Hz frequency"); - return; - } - #endif - - // Enable the FIFO - if(imu.enableFIFO() == ICM_20948_Stat_Ok) - { - m_Logger.debug("FIFO Enabled"); - } - else - { - m_Logger.fatal("Failed to enable FIFO"); - return; - } - - // Enable the DMP - if(imu.enableDMP() == ICM_20948_Stat_Ok) - { - m_Logger.debug("DMP Enabled"); - } - else - { - m_Logger.fatal("Failed to enable DMP"); - return; - } - - // Reset DMP - if(imu.resetDMP() == ICM_20948_Stat_Ok) - { - m_Logger.debug("Reset DMP"); - } - else - { - m_Logger.fatal("Failed to reset DMP"); - return; - } - - // Reset FIFO - if(imu.resetFIFO() == ICM_20948_Stat_Ok) - { - m_Logger.debug("Reset FIFO"); - } - else - { - m_Logger.fatal("Failed to reset FIFO"); - return; - } + // Enable the FIFO + if (imu.enableFIFO() == ICM_20948_Stat_Ok) { + m_Logger.debug("FIFO Enabled"); + } else { + m_Logger.fatal("Failed to enable FIFO"); + return; + } + + // Enable the DMP + if (imu.enableDMP() == ICM_20948_Stat_Ok) { + m_Logger.debug("DMP Enabled"); + } else { + m_Logger.fatal("Failed to enable DMP"); + return; + } + + // Reset DMP + if (imu.resetDMP() == ICM_20948_Stat_Ok) { + m_Logger.debug("Reset DMP"); + } else { + m_Logger.fatal("Failed to reset DMP"); + return; + } + + // Reset FIFO + if (imu.resetFIFO() == ICM_20948_Stat_Ok) { + m_Logger.debug("Reset FIFO"); + } else { + m_Logger.fatal("Failed to reset FIFO"); + return; + } } -void ICM20948Sensor::connectSensor() -{ - #ifdef DEBUG_SENSOR - imu.enableDebugging(Serial); - #endif - // SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if something else throw a error - boolean isOnSecondAddress = false; - - if (addr == 0x68) { - isOnSecondAddress = false; - } else if (addr == 0x69){ - isOnSecondAddress = true; - } else { - m_Logger.fatal("I2C Address not supported by ICM20948 library: 0x%02x", addr); - return; - } - - ICM_20948_Status_e imu_err = imu.begin(Wire, isOnSecondAddress); - if (imu_err != ICM_20948_Stat_Ok) { - m_Logger.fatal("Can't connect to ICM20948 at address 0x%02x, error code: 0x%02x", addr, imu_err); - ledManager.pattern(50, 50, 200); - return; - } +void ICM20948Sensor::connectSensor() { +#ifdef DEBUG_SENSOR + imu.enableDebugging(Serial); +#endif + // SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if + // something else throw a error + boolean isOnSecondAddress = false; + + if (addr == 0x68) { + isOnSecondAddress = false; + } else if (addr == 0x69) { + isOnSecondAddress = true; + } else { + m_Logger.fatal("I2C Address not supported by ICM20948 library: 0x%02x", addr); + return; + } + + ICM_20948_Status_e imu_err = imu.begin(Wire, isOnSecondAddress); + if (imu_err != ICM_20948_Stat_Ok) { + m_Logger.fatal( + "Can't connect to ICM20948 at address 0x%02x, error code: 0x%02x", + addr, + imu_err + ); + ledManager.pattern(50, 50, 200); + return; + } } -void ICM20948Sensor::startMotionLoop() -{ - lastData = millis(); - working = true; +void ICM20948Sensor::startMotionLoop() { + lastData = millis(); + working = true; } -void ICM20948Sensor::checkSensorTimeout() -{ - unsigned long currenttime = millis(); - if(lastData + 2000 < currenttime) { - working = false; - m_Logger.error("Sensor timeout I2C Address 0x%02x delaytime: %d ms", addr, currenttime-lastData ); - networkConnection.sendSensorError(this->sensorId, 1); - lastData = currenttime; - } +void ICM20948Sensor::checkSensorTimeout() { + unsigned long currenttime = millis(); + if (lastData + 2000 < currenttime) { + working = false; + m_Logger.error( + "Sensor timeout I2C Address 0x%02x delaytime: %d ms", + addr, + currenttime - lastData + ); + networkConnection.sendSensorError(this->sensorId, 1); + lastData = currenttime; + } } -void ICM20948Sensor::readRotation() -{ - #if(USE_6_AXIS) - { - if (((dmpData.header & DMP_header_bitmap_Quat6) > 0) && hasdata) - { - // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. - // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values. - // The quaternion data is scaled by 2^30. - // Scale to +/- 1 - double q1 = ((double)dmpData.Quat6.Data.Q1) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 - double q2 = ((double)dmpData.Quat6.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 - double q3 = ((double)dmpData.Quat6.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 - double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - Quat nRotation(q1, q2, q3, q0); // x, y, z, w - - #if SEND_ACCELERATION - calculateAccelerationWithoutGravity(&nRotation); - #endif - - setFusedRotation(nRotation); - lastData = millis(); - } - } - #else - { - if(((dmpData.header & DMP_header_bitmap_Quat9) > 0) && hasdata) - { - // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. - // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values. - // The quaternion data is scaled by 2^30. - // Scale to +/- 1 - double q1 = ((double)dmpData.Quat9.Data.Q1) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 - double q2 = ((double)dmpData.Quat9.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 - double q3 = ((double)dmpData.Quat9.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 - double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - Quat nRotation(q1, q2, q3, q0); // x, y, z, w - - #if SEND_ACCELERATION - calculateAccelerationWithoutGravity(&nRotation); - #endif - - setFusedRotation(nRotation); - lastData = millis(); - } - } - #endif +void ICM20948Sensor::readRotation() { +#if (USE_6_AXIS) + { + if (((dmpData.header & DMP_header_bitmap_Quat6) > 0) && hasdata) { + // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. + // In case of drift, the sum will not add to 1, therefore, quaternion data + // need to be corrected with right bias values. The quaternion data is + // scaled by 2^30. Scale to +/- 1 + double q1 + = ((double)dmpData.Quat6.Data.Q1) + / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q2 + = ((double)dmpData.Quat6.Data.Q2) + / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q3 + = ((double)dmpData.Quat6.Data.Q3) + / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); + Quat nRotation(q1, q2, q3, q0); // x, y, z, w + +#if SEND_ACCELERATION + calculateAccelerationWithoutGravity(&nRotation); +#endif + + setFusedRotation(nRotation); + lastData = millis(); + } + } +#else + { + if (((dmpData.header & DMP_header_bitmap_Quat9) > 0) && hasdata) { + // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. + // In case of drift, the sum will not add to 1, therefore, quaternion data + // need to be corrected with right bias values. The quaternion data is + // scaled by 2^30. Scale to +/- 1 + double q1 + = ((double)dmpData.Quat9.Data.Q1) + / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q2 + = ((double)dmpData.Quat9.Data.Q2) + / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q3 + = ((double)dmpData.Quat9.Data.Q3) + / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); + Quat nRotation(q1, q2, q3, q0); // x, y, z, w + +#if SEND_ACCELERATION + calculateAccelerationWithoutGravity(&nRotation); +#endif + + setFusedRotation(nRotation); + lastData = millis(); + } + } +#endif } -void ICM20948Sensor::saveCalibration(bool repeat) -{ - #if(!SAVE_BIAS) - { - return; - } - #endif - #ifdef DEBUG_SENSOR - m_Logger.trace("Saving Bias"); - #endif - - imu.GetBiasGyroX(&m_Config.G[0]); - imu.GetBiasGyroY(&m_Config.G[1]); - imu.GetBiasGyroZ(&m_Config.G[2]); - - imu.GetBiasAccelX(&m_Config.A[0]); - imu.GetBiasAccelY(&m_Config.A[1]); - imu.GetBiasAccelZ(&m_Config.A[2]); - - #if !USE_6_AXIS - imu.GetBiasCPassX(&m_Config.C[0]); - imu.GetBiasCPassY(&m_Config.C[1]); - imu.GetBiasCPassZ(&m_Config.C[2]); - #endif - - #ifdef DEBUG_SENSOR - m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.G)); - m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.A)); - #if !USE_6_AXIS - m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.C)); - #endif - #endif - - SlimeVR::Configuration::SensorConfig config; - config.type = SlimeVR::Configuration::SensorConfigType::ICM20948; - config.data.icm20948 = m_Config; - configuration.setSensor(sensorId, config); - configuration.save(); - - if (repeat) { - bias_save_counter++; - // Possible: Could make it repeat the final timer value if any of the biases are still 0. Save strategy could be improved. - if (sizeof(bias_save_periods) != bias_save_counter) { - globalTimer.in( - bias_save_periods[bias_save_counter] * 1000, - [](void* arg) -> bool { - ((ICM20948Sensor*)arg)->saveCalibration(true); - return false; - }, - this); - } - } +void ICM20948Sensor::saveCalibration(bool repeat) { +#if (!SAVE_BIAS) + { return; } +#endif +#ifdef DEBUG_SENSOR + m_Logger.trace("Saving Bias"); +#endif + + imu.GetBiasGyroX(&m_Config.G[0]); + imu.GetBiasGyroY(&m_Config.G[1]); + imu.GetBiasGyroZ(&m_Config.G[2]); + + imu.GetBiasAccelX(&m_Config.A[0]); + imu.GetBiasAccelY(&m_Config.A[1]); + imu.GetBiasAccelZ(&m_Config.A[2]); + +#if !USE_6_AXIS + imu.GetBiasCPassX(&m_Config.C[0]); + imu.GetBiasCPassY(&m_Config.C[1]); + imu.GetBiasCPassZ(&m_Config.C[2]); +#endif + +#ifdef DEBUG_SENSOR + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.A)); +#if !USE_6_AXIS + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.C)); +#endif +#endif + + SlimeVR::Configuration::SensorConfig config; + config.type = SlimeVR::Configuration::SensorConfigType::ICM20948; + config.data.icm20948 = m_Config; + configuration.setSensor(sensorId, config); + configuration.save(); + + if (repeat) { + bias_save_counter++; + // Possible: Could make it repeat the final timer value if any of the biases are + // still 0. Save strategy could be improved. + if (sizeof(bias_save_periods) != bias_save_counter) { + globalTimer.in( + bias_save_periods[bias_save_counter] * 1000, + [](void* arg) -> bool { + ((ICM20948Sensor*)arg)->saveCalibration(true); + return false; + }, + this + ); + } + } } -void ICM20948Sensor::loadCalibration() -{ - #if(!LOAD_BIAS) - { - return; - } - #endif - - SlimeVR::Configuration::SensorConfig sensorCalibration = configuration.getSensor(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::SensorConfigType::ICM20948: - m_Config = sensorCalibration.data.icm20948; - break; - - case SlimeVR::Configuration::SensorConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } - - #ifdef DEBUG_SENSOR - m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.G)); - m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.A)); - #if !USE_6_AXIS - m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.C)); - #endif - #endif - - imu.SetBiasGyroX(m_Config.G[0]); - imu.SetBiasGyroY(m_Config.G[1]); - imu.SetBiasGyroZ(m_Config.G[2]); - - imu.SetBiasAccelX(m_Config.A[0]); - imu.SetBiasAccelY(m_Config.A[1]); - imu.SetBiasAccelZ(m_Config.A[2]); - - #if !USE_6_AXIS - imu.SetBiasCPassX(m_Config.C[0]); - imu.SetBiasCPassY(m_Config.C[1]); - imu.SetBiasCPassZ(m_Config.C[2]); - #endif +void ICM20948Sensor::loadCalibration() { +#if (!LOAD_BIAS) + { return; } +#endif + + SlimeVR::Configuration::SensorConfig sensorCalibration + = configuration.getSensor(sensorId); + // If no compatible calibration data is found, the calibration data will just be + // zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::SensorConfigType::ICM20948: + m_Config = sensorCalibration.data.icm20948; + break; + + case SlimeVR::Configuration::SensorConfigType::NONE: + m_Logger.warn( + "No calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn( + "Incompatible calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + } + +#ifdef DEBUG_SENSOR + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.A)); +#if !USE_6_AXIS + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.C)); +#endif +#endif + + imu.SetBiasGyroX(m_Config.G[0]); + imu.SetBiasGyroY(m_Config.G[1]); + imu.SetBiasGyroZ(m_Config.G[2]); + + imu.SetBiasAccelX(m_Config.A[0]); + imu.SetBiasAccelY(m_Config.A[1]); + imu.SetBiasAccelZ(m_Config.A[2]); + +#if !USE_6_AXIS + imu.SetBiasCPassX(m_Config.C[0]); + imu.SetBiasCPassY(m_Config.C[1]); + imu.SetBiasCPassZ(m_Config.C[2]); +#endif } -void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) -{ - #if SEND_ACCELERATION - { - if((dmpData.header & DMP_header_bitmap_Accel) > 0) - { - sfusion.updateQuaternion(*quaternion); - - float Axyz[3] = {(float)this->dmpData.Raw_Accel.Data.X * ASCALE_4G, - (float)this->dmpData.Raw_Accel.Data.Y * ASCALE_4G, - (float)this->dmpData.Raw_Accel.Data.Z * ASCALE_4G - }; - sfusion.updateAcc(Axyz); - - setAcceleration(sfusion.getLinearAccVec()); - } - } - #endif +void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat* quaternion) { +#if SEND_ACCELERATION + { + if ((dmpData.header & DMP_header_bitmap_Accel) > 0) { + sfusion.updateQuaternion(*quaternion); + + float Axyz[3] + = {(float)this->dmpData.Raw_Accel.Data.X * ASCALE_4G, + (float)this->dmpData.Raw_Accel.Data.Y * ASCALE_4G, + (float)this->dmpData.Raw_Accel.Data.Z * ASCALE_4G}; + sfusion.updateAcc(Axyz); + + setAcceleration(sfusion.getLinearAccVec()); + } + } +#endif } -//You need to override the library's initializeDMP to change some settings +// You need to override the library's initializeDMP to change some settings #if OVERRIDEDMPSETUP -// initializeDMP is a weak function. Let's overwrite it so we can increase the sample rate -ICM_20948_Status_e ICM_20948::initializeDMP(void) -{ - // First, let's check if the DMP is available - if (_device._dmp_firmware_available != true) - { - debugPrint(F("ICM_20948::startupDMP: DMP is not available. Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...")); - return ICM_20948_Stat_DMPNotSupported; - } - - ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok; - - #if defined(ICM_20948_USE_DMP) - - // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration - // sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions". - - ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful - - // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer - // to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read - // nine bytes from the mag every sample, starting from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit. - // Next are the six magnetometer readings (little endian). After a dummy byte, the STATUS2 register (0x18) contains the HOFL (Overflow) bit. - // - // But looking very closely at the InvenSense example code, we can see in inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that, - // when the DMP is running, the magnetometer is set to Single Measurement (SM) mode and that ten bytes are read, starting from the reserved - // RSV2 register (0x03). The datasheet does not define what registers 0x04 to 0x0C contain. There is definitely some secret sauce in here... - // The magnetometer data appears to be big endian (not little endian like the HX/Y/Z registers) and starts at register 0x04. - // We had to examine the I2C traffic between the master and the AK09916 on the AUX_DA and AUX_CL pins to discover this... - // - // So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed to i2cControllerConfigurePeripheral are: - // 0: use I2C_SLV0 - // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) - // AK09916_REG_RSV2: we start reading here (0x03). Secret sauce... - // 10: we read 10 bytes each cycle - // true: set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them) - // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral at the sample rate - // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) - // true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm) - // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm) - result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result; - // - // We also need to set up I2C_SLV1 to do the Single Measurement triggering: - // 1: use I2C_SLV1 - // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) - // AK09916_REG_CNTL2: we start writing here (0x31) - // 1: not sure why, but the write does not happen if this is set to zero - // false: clear the I2C_SLV0_RNW ReadNotWrite bit so we write the dataOut byte - // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear - // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) - // false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit - // false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit - // AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample - result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result; - - // Set the I2C Master ODR configuration - // It is not clear why we need to do this... But it appears to be essential! From the datasheet: - // "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor when gyroscope and accelerometer are disabled. - // ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) ) - // When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope ODR. - // If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR." - // Since both gyro and accel are running, setting this register should have no effect. But it does. Maybe because the Gyro and Accel are placed in Low Power Mode (cycled)? - // You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case. - result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3 - uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz - result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register - - // Configure clock source through PWR_MGMT_1 - // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator - result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails - - // Enable accel and gyro sensors through PWR_MGMT_2 - // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 - uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?) - result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register - - // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG - // The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode - result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result; - - // Disable the FIFO - result = enableFIFO(false); if (result > worstResult) worstResult = result; - - // Disable the DMP - result = enableDMP(false); if (result > worstResult) worstResult = result; - - // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1 - // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG - ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors - myFSS.a = gpm4; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) - // gpm2 - // gpm4 - // gpm8 - // gpm16 - myFSS.g = dps2000; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) - // dps250 - // dps500 - // dps1000 - // dps2000 - result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result; - - // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB)) - // We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte... - // The gyro DLPF is enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no effect, but we'll include it anyway - result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result; - - // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2 - // If we see this interrupt, we'll need to reset the FIFO - //result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs - - // Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2 - // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 - uint8_t zero = 0; - result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result; - // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2 - result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result; - - // Turn off data ready interrupt through INT_ENABLE_1 - result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result; - - // Reset FIFO through FIFO_RST - result = resetFIFO(); if (result > worstResult) worstResult = result; - - // Set gyro sample rate divider with GYRO_SMPLRT_DIV - // Set accel sample rate divider with ACCEL_SMPLRT_DIV_2 - ICM_20948_smplrt_t mySmplrt; - //mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13). - //mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13). - mySmplrt.g = 4; // 225Hz - mySmplrt.a = 4; // 225Hz - // mySmplrt.g = 8; // 112Hz - // mySmplrt.a = 8; // 112Hz - result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result; - - // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL - result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS - - // Now load the DMP firmware - result = loadDMPFirmware(); if (result > worstResult) worstResult = result; - - // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL - result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS - - // Set the Hardware Fix Disable register to 0x48 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 - uint8_t fix = 0x48; - result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result; - - // Set the Single FIFO Priority Select register to 0xE4 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 - uint8_t fifoPrio = 0xE4; - result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result; - - // Configure Accel scaling to DMP - // The DMP scales accel raw data internally to align 1g as 2^25 - // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g - const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00}; - result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register - // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g - const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00}; - result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register - - // Configure Compass mount matrix and scale to DMP - // The mount matrix write to DMP register is used to align the compass axes with accel/gyro. - // This mechanism is also used to convert hardware unit to uT. The value is expressed as 1uT = 2^30. - // Each compass axis will be converted as below: - // X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02 - // Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12 - // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22 - // The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU. - // 2^30 / 6.66666 = 161061273 = 0x9999999 - const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; - const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example - const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example - result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; - - // Configure the B2S Mounting Matrix - const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; - const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example - result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; - - // Configure the DMP Gyro Scaling Factor - // @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where - // 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ... - // 10=102.2727Hz sample rate, ... etc. - // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps - //result = setGyroSF(19, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) - result = setGyroSF(4, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) - - - // Configure the Gyro full scale - // 2000dps : 2^28 - // 1000dps : 2^27 - // 500dps : 2^26 - // 250dps : 2^25 - const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28 - result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result; - - // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz) - const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz - //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz - //const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz - result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result; - - // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz) - const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz - //const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz - //const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz - result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result; - - // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz) - const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz - //const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz - //const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz - result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result; - - // Configure the Accel Cal Rate - const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example - result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result; - - // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz. - // Let's set the Compass Time Buffer to 69 (Hz). - const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz - result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result; - - // Enable DMP interrupt - // This would be the most efficient way of getting the DMP data, instead of polling the FIFO - //result = intEnableDMP(true); if (result > worstResult) worstResult = result; +// initializeDMP is a weak function. Let's overwrite it so we can increase the sample +// rate +ICM_20948_Status_e ICM_20948::initializeDMP(void) { + // First, let's check if the DMP is available + if (_device._dmp_firmware_available != true) { + debugPrint( + F("ICM_20948::startupDMP: DMP is not available. Please check that you have " + "uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...") + ); + return ICM_20948_Stat_DMPNotSupported; + } + + ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok; + +#if defined(ICM_20948_USE_DMP) + + // The ICM-20948 is awake and ready but hasn't been configured. Let's step through + // the configuration sequence from InvenSense's _confidential_ Application Note + // "Programming Sequence for DMP Hardware Functions". + + ICM_20948_Status_e result + = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration + // was successful + + // Normally, when the DMP is not enabled, startupMagnetometer (called by + // startupDefault, which is called by begin) configures the AK09916 magnetometer to + // run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's + // I2C_SLV0 is configured to read nine bytes from the mag every sample, starting + // from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit. Next + // are the six magnetometer readings (little endian). After a dummy byte, the + // STATUS2 register (0x18) contains the HOFL (Overflow) bit. + // + // But looking very closely at the InvenSense example code, we can see in + // inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that, when the DMP is + // running, the magnetometer is set to Single Measurement (SM) mode and that ten + // bytes are read, starting from the reserved RSV2 register (0x03). The datasheet + // does not define what registers 0x04 to 0x0C contain. There is definitely some + // secret sauce in here... The magnetometer data appears to be big endian (not + // little endian like the HX/Y/Z registers) and starts at register 0x04. We had to + // examine the I2C traffic between the master and the AK09916 on the AUX_DA and + // AUX_CL pins to discover this... + // + // So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed + // to i2cControllerConfigurePeripheral are: 0: use I2C_SLV0 MAG_AK09916_I2C_ADDR: + // the I2C address of the AK09916 magnetometer (0x0C unshifted) AK09916_REG_RSV2: we + // start reading here (0x03). Secret sauce... 10: we read 10 bytes each cycle true: + // set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them) + // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral + // at the sample rate false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to + // write the register value) true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show + // the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm) + // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag + // (copied from inv_icm20948_resume_akm) + result = i2cControllerConfigurePeripheral( + 0, + MAG_AK09916_I2C_ADDR, + AK09916_REG_RSV2, + 10, + true, + true, + false, + true, + true + ); + if (result > worstResult) { + worstResult = result; + } + // + // We also need to set up I2C_SLV1 to do the Single Measurement triggering: + // 1: use I2C_SLV1 + // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C + // unshifted) AK09916_REG_CNTL2: we start writing here (0x31) 1: not sure why, but + // the write does not happen if this is set to zero false: clear the I2C_SLV0_RNW + // ReadNotWrite bit so we write the dataOut byte true: set the I2C_SLV0_CTRL + // I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear + // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register + // value) false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit false: clear the + // I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit AK09916_mode_single: tell I2C_SLV1 to write + // the Single Measurement command each sample + result = i2cControllerConfigurePeripheral( + 1, + MAG_AK09916_I2C_ADDR, + AK09916_REG_CNTL2, + 1, + false, + true, + false, + false, + false, + AK09916_mode_single + ); + if (result > worstResult) { + worstResult = result; + } + + // Set the I2C Master ODR configuration + // It is not clear why we need to do this... But it appears to be essential! From + // the datasheet: "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor + // when gyroscope and accelerometer are disabled. + // ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) ) + // When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope + // ODR. If gyroscope is disabled, then all sensors (including I2C_MASTER) use the + // accelerometer ODR." + // Since both gyro and accel are running, setting this register should have no + // effect. But it does. Maybe because the Gyro and Accel are placed in Low Power + // Mode (cycled)? You can see by monitoring the Aux I2C pins that the next three + // lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen + // rate: 68.75Hz in this case. + result = setBank(3); + if (result > worstResult) { + worstResult = result; // Select Bank 3 + } + uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz + result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); + if (result > worstResult) { + worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register + } + + // Configure clock source through PWR_MGMT_1 + // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else + // use the Internal oscillator + result = setClockSource(ICM_20948_Clock_Auto); + if (result > worstResult) { + worstResult = result; // This is shorthand: success will be set to false if + // setClockSource fails + } + + // Enable accel and gyro sensors through PWR_MGMT_2 + // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to + // PWR_MGMT_2 + result = setBank(0); + if (result > worstResult) { + worstResult = result; // Select Bank 0 + } + uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?) + result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); + if (result > worstResult) { + worstResult = result; // Write one byte to the PWR_MGMT_2 register + } + + // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG + // The InvenSense Nucleo example initially puts the accel and gyro into low power + // mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power + // Mode + result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); + if (result > worstResult) { + worstResult = result; + } + + // Disable the FIFO + result = enableFIFO(false); + if (result > worstResult) { + worstResult = result; + } + + // Disable the DMP + result = enableDMP(false); + if (result > worstResult) { + worstResult = result; + } + + // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1 + // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG + ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can + // contain values for all configurable sensors + myFSS.a = gpm4; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) + // gpm2 + // gpm4 + // gpm8 + // gpm16 + myFSS.g = dps2000; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) + // dps250 + // dps500 + // dps1000 + // dps2000 + result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); + if (result > worstResult) { + worstResult = result; + } + + // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG + // set to zero = 196.6Hz (3dB)) We found this by going through the SPI data + // generated by ZaneL's Teensy-ICM-20948 library byte by byte... The gyro DLPF is + // enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no + // effect, but we'll include it anyway + result = enableDLPF(ICM_20948_Internal_Gyr, true); + if (result > worstResult) { + worstResult = result; + } + + // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2 + // If we see this interrupt, we'll need to reset the FIFO + // result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = + // result; // Enable the interrupt on all FIFOs + + // Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2 + // Stop the peripheral data from being written to the FIFO by writing zero to + // FIFO_EN_1 + result = setBank(0); + if (result > worstResult) { + worstResult = result; // Select Bank 0 + } + uint8_t zero = 0; + result = write(AGB0_REG_FIFO_EN_1, &zero, 1); + if (result > worstResult) { + worstResult = result; + } + // Stop the accelerometer, gyro and temperature data from being written to the FIFO + // by writing zero to FIFO_EN_2 + result = write(AGB0_REG_FIFO_EN_2, &zero, 1); + if (result > worstResult) { + worstResult = result; + } + + // Turn off data ready interrupt through INT_ENABLE_1 + result = intEnableRawDataReady(false); + if (result > worstResult) { + worstResult = result; + } + + // Reset FIFO through FIFO_RST + result = resetFIFO(); + if (result > worstResult) { + worstResult = result; + } + + // Set gyro sample rate divider with GYRO_SMPLRT_DIV + // Set accel sample rate divider with ACCEL_SMPLRT_DIV_2 + ICM_20948_smplrt_t mySmplrt; + // mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). + // 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13). mySmplrt.a = 19; // ODR is + // computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. + // InvenSense Nucleo example uses 19 (0x13). + mySmplrt.g = 4; // 225Hz + mySmplrt.a = 4; // 225Hz + // mySmplrt.g = 8; // 112Hz + // mySmplrt.a = 8; // 112Hz + result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); + if (result > worstResult) { + worstResult = result; + } + + // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL + result = setDMPstartAddress(); + if (result > worstResult) { + worstResult = result; // Defaults to DMP_START_ADDRESS + } + + // Now load the DMP firmware + result = loadDMPFirmware(); + if (result > worstResult) { + worstResult = result; + } + + // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL + result = setDMPstartAddress(); + if (result > worstResult) { + worstResult = result; // Defaults to DMP_START_ADDRESS + } + + // Set the Hardware Fix Disable register to 0x48 + result = setBank(0); + if (result > worstResult) { + worstResult = result; // Select Bank 0 + } + uint8_t fix = 0x48; + result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); + if (result > worstResult) { + worstResult = result; + } + + // Set the Single FIFO Priority Select register to 0xE4 + result = setBank(0); + if (result > worstResult) { + worstResult = result; // Select Bank 0 + } + uint8_t fifoPrio = 0xE4; + result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); + if (result > worstResult) { + worstResult = result; + } + + // Configure Accel scaling to DMP + // The DMP scales accel raw data internally to align 1g as 2^25 + // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is + // 4g + const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00}; + result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); + if (result > worstResult) { + worstResult = result; // Write accScale to ACC_SCALE DMP register + } + // In order to output hardware unit data as configured FSR write 0x00040000 when FSR + // is 4g + const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00}; + result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); + if (result > worstResult) { + worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register + } + + // Configure Compass mount matrix and scale to DMP + // The mount matrix write to DMP register is used to align the compass axes with + // accel/gyro. This mechanism is also used to convert hardware unit to uT. The value + // is expressed as 1uT = 2^30. Each compass axis will be converted as below: X = + // raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02 Y = raw_x * + // CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12 Z = raw_x * + // CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22 The AK09916 produces a + // 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 + // ADU. 2^30 / 6.66666 = 161061273 = 0x9999999 + const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; + const unsigned char mountMultiplierPlus[4] + = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example + const unsigned char mountMultiplierMinus[4] + = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example + result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); + if (result > worstResult) { + worstResult = result; + } + + // Configure the B2S Mounting Matrix + const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; + const unsigned char b2sMountMultiplierPlus[4] + = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example + result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) { + worstResult = result; + } + result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); + if (result > worstResult) { + worstResult = result; + } + + // Configure the DMP Gyro Scaling Factor + // @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where + // 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, + // ... 10=102.2727Hz sample rate, ... etc. + // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps + // result = setGyroSF(19, 3); if (result > worstResult) worstResult = result; // 19 + // = 55Hz (see above), 3 = 2000dps (see above) + result = setGyroSF(4, 3); + if (result > worstResult) { + worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) + } + + // Configure the Gyro full scale + // 2000dps : 2^28 + // 1000dps : 2^27 + // 500dps : 2^26 + // 250dps : 2^25 + const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28 + result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); + if (result > worstResult) { + worstResult = result; + } + + // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz) + const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz + // const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz + // const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz + result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); + if (result > worstResult) { + worstResult = result; + } + + // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 + // (56Hz) + const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz + // const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz + // const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz + result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); + if (result > worstResult) { + worstResult = result; + } + + // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz) + const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz + // const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz + // const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz + result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); + if (result > worstResult) { + worstResult = result; + } + + // Configure the Accel Cal Rate + const unsigned char accelCalRate[4] + = {0x00, 0x00}; // Value taken from InvenSense Nucleo example + result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); + if (result > worstResult) { + worstResult = result; + } + + // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) + // sets the magnetometer read rate to 68.75Hz. Let's set the Compass Time Buffer to + // 69 (Hz). + const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz + result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); + if (result > worstResult) { + worstResult = result; + } + + // Enable DMP interrupt + // This would be the most efficient way of getting the DMP data, instead of polling + // the FIFO + // result = intEnableDMP(true); if (result > worstResult) worstResult = result; #endif - return worstResult; + return worstResult; } -#endif // OVERRIDEDMPSETUP +#endif // OVERRIDEDMPSETUP diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index d5f4f57c4..5e938b346 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -1,85 +1,98 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_ICM20948SENSOR_H_ #define SLIMEVR_ICM20948SENSOR_H_ #include -#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 uint8_t Address = 0x68; + static constexpr auto TypeID = ImuID::ICM20948; + static constexpr uint8_t Address = 0x68; - ICM20948Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, 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(); - } + ICM20948Sensor( + uint8_t id, + uint8_t addrSuppl, + float rotation, + uint8_t sclPin, + 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 sendData() override final; - void startCalibration(int calibrationType) override final; + void motionLoop() override final; + void sendData() override final; + void startCalibration(int calibrationType) override final; private: - void calculateAccelerationWithoutGravity(Quat *quaternion); - unsigned long lastData = 0; - int bias_save_counter = 0; - bool hasdata = false; -// Performance test -/* - uint8_t cntbuf = 0; - int32_t cntrounds = 0; - unsigned long lastData2 = 0; -*/ + void calculateAccelerationWithoutGravity(Quat* quaternion); + unsigned long lastData = 0; + int bias_save_counter = 0; + bool hasdata = false; + // Performance test + /* + uint8_t cntbuf = 0; + int32_t cntrounds = 0; + unsigned long lastData2 = 0; + */ - #define DMPNUMBERTODOUBLECONVERTER 1073741824.0; +#define DMPNUMBERTODOUBLECONVERTER 1073741824.0; - ICM_20948_I2C imu; - ICM_20948_Device_t pdev; - icm_20948_DMP_data_t dmpData{}; - icm_20948_DMP_data_t dmpDataTemp{}; + ICM_20948_I2C imu; + ICM_20948_Device_t pdev; + icm_20948_DMP_data_t dmpData{}; + 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 loadCalibration(); - void startCalibrationAutoSave(); - void startDMP(); - void connectSensor(); - void startMotionLoop(); - void checkSensorTimeout(); - void readRotation(); - void readFIFOToEnd(); + void saveCalibration(bool repeat); + void loadCalibration(); + void startCalibrationAutoSave(); + void startDMP(); + void connectSensor(); + void startMotionLoop(); + void checkSensorTimeout(); + void readRotation(); + void readFIFOToEnd(); #define OVERRIDEDMPSETUP true - // TapDetector tapDetector; + // TapDetector tapDetector; }; -#endif // SLIMEVR_ICM20948SENSOR_H_ +#endif // SLIMEVR_ICM20948SENSOR_H_ diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index f7d49cce8..4480c78db 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "globals.h" @@ -29,174 +29,207 @@ #include "MPU6050_6Axis_MotionApps20.h" #endif -#include "mpu6050sensor.h" #include -#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; - -void MPU6050Sensor::motionSetup() -{ - imu.initialize(addr); - if (!imu.testConnection()) - { - m_Logger.fatal("Can't connect to %s (reported device ID 0x%02x) at address 0x%02x", getIMUNameByType(sensorType), imu.getDeviceID(), addr); - return; - } - - m_Logger.info("Connected to %s (reported device ID 0x%02x) at address 0x%02x", getIMUNameByType(sensorType), imu.getDeviceID(), addr); +constexpr float ASCALE_2G + = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY; + +void MPU6050Sensor::motionSetup() { + imu.initialize(addr); + if (!imu.testConnection()) { + m_Logger.fatal( + "Can't connect to %s (reported device ID 0x%02x) at address 0x%02x", + getIMUNameByType(sensorType), + imu.getDeviceID(), + addr + ); + return; + } + + 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 - { - SlimeVR::Configuration::SensorConfig sensorCalibration = configuration.getCalibration(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::SensorConfigType::MPU6050: - m_Config = sensorCalibration.data.mpu6050; - break; - - case SlimeVR::Configuration::SensorConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } - } + // Initialize the configuration + { + SlimeVR::Configuration::SensorConfig sensorCalibration + = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be + // zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::SensorConfigType::MPU6050: + m_Config = sensorCalibration.data.mpu6050; + break; + + case SlimeVR::Configuration::SensorConfigType::NONE: + m_Logger.warn( + "No calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn( + "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..."); - // Do a quick and dirty calibration. As the imu warms up the offsets will change 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.CalibrateAccel(6); - imu.PrintActiveOffsets(); -#endif // IMU_MPU6050_RUNTIME_CALIBRATION - - ledManager.pattern(50, 50, 5); - - // turn on the DMP, now that it's ready - m_Logger.debug("Enabling DMP..."); - imu.setDMPEnabled(true); - - // TODO: Add interrupt support - // mpuIntStatus = imu.getIntStatus(); - - // 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..."); - dmpReady = true; - - // get expected DMP packet size for later comparison - packetSize = imu.dmpGetFIFOPacketSize(); - - working = true; - } - else - { - // ERROR! - // 1 = initial memory load failed - // 2 = DMP configuration updates failed - // (if it's going to break, usually the code will be 1) - m_Logger.error("DMP Initialization failed (code %d)", devStatus); - } + 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 + delay(1000); // A small sleep to give the users a chance to stop it from moving + + imu.CalibrateGyro(6); + imu.CalibrateAccel(6); + imu.PrintActiveOffsets(); +#endif // IMU_MPU6050_RUNTIME_CALIBRATION + + ledManager.pattern(50, 50, 5); + + // turn on the DMP, now that it's ready + m_Logger.debug("Enabling DMP..."); + imu.setDMPEnabled(true); + + // TODO: Add interrupt support + // mpuIntStatus = imu.getIntStatus(); + + // 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..."); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = imu.dmpGetFIFOPacketSize(); + + working = true; + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (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; - imu.getRotation(&rX, &rY, &rZ); - imu.getAcceleration(&aX, &aY, &aZ); - - networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); - } + { + int16_t rX, rY, rZ, aX, aY, aZ; + imu.getRotation(&rX, &rY, &rZ); + imu.getAcceleration(&aX, &aY, &aZ); + + networkConnection.sendInspectionRawIMUData( + sensorId, + rX, + rY, + rZ, + 255, + aX, + aY, + aZ, + 255, + 0, + 0, + 0, + 255 + ); + } #endif - if (!dmpReady) - return; + if (!dmpReady) { + return; + } - if (imu.dmpGetCurrentFIFOPacket(fifoBuffer)) - { - imu.dmpGetQuaternion(&rawQuat, fifoBuffer); - hadData = true; + if (imu.dmpGetCurrentFIFOPacket(fifoBuffer)) { + imu.dmpGetQuaternion(&rawQuat, fifoBuffer); + hadData = true; - sfusion.updateQuaternion(rawQuat); + sfusion.updateQuaternion(rawQuat); - setFusedRotation(sfusion.getQuaternionQuat()); + setFusedRotation(sfusion.getQuaternionQuat()); - #if SEND_ACCELERATION - { - this->imu.dmpGetAccel(&this->rawAccel, this->fifoBuffer); +#if SEND_ACCELERATION + { + this->imu.dmpGetAccel(&this->rawAccel, this->fifoBuffer); - float Axyz[3] = {(float)rawAccel.x * ASCALE_2G, - (float)rawAccel.y * ASCALE_2G, - (float)rawAccel.z * ASCALE_2G }; + float Axyz[3] + = {(float)rawAccel.x * 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"); -#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.CalibrateGyro(6); - imu.CalibrateAccel(6); - imu.setDMPEnabled(true); - - m_Logger.debug("Gathered baseline gyro reading"); - m_Logger.debug("Starting offset finder"); - switch (calibrationType) - { - case CALIBRATION_TYPE_INTERNAL_ACCEL: - imu.CalibrateAccel(10); - m_Config.A_B[0] = imu.getXAccelOffset(); - m_Config.A_B[1] = imu.getYAccelOffset(); - m_Config.A_B[2] = imu.getZAccelOffset(); - break; - case CALIBRATION_TYPE_INTERNAL_GYRO: - imu.CalibrateGyro(10); - m_Config.G_off[0] = imu.getXGyroOffset(); - m_Config.G_off[1] = imu.getYGyroOffset(); - m_Config.G_off[2] = imu.getZGyroOffset(); - break; - } - - SlimeVR::Configuration::SensorConfig calibration; - calibration.type = SlimeVR::Configuration::SensorConfigType::MPU6050; - calibration.data.mpu6050 = m_Config; - configuration.setCalibration(sensorId, calibration); - configuration.save(); - - m_Logger.info("Calibration finished"); -#endif // !IMU_MPU6050_RUNTIME_CALIBRATION - - ledManager.off(); + m_Logger.info( + "MPU is using automatic runtime calibration. Place down the device and it " + "should automatically calibrate after a few seconds" + ); +#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.CalibrateGyro(6); + imu.CalibrateAccel(6); + imu.setDMPEnabled(true); + + m_Logger.debug("Gathered baseline gyro reading"); + m_Logger.debug("Starting offset finder"); + switch (calibrationType) { + case CALIBRATION_TYPE_INTERNAL_ACCEL: + imu.CalibrateAccel(10); + m_Config.A_B[0] = imu.getXAccelOffset(); + m_Config.A_B[1] = imu.getYAccelOffset(); + m_Config.A_B[2] = imu.getZAccelOffset(); + break; + case CALIBRATION_TYPE_INTERNAL_GYRO: + imu.CalibrateGyro(10); + m_Config.G_off[0] = imu.getXGyroOffset(); + m_Config.G_off[1] = imu.getYGyroOffset(); + m_Config.G_off[2] = imu.getZGyroOffset(); + break; + } + + SlimeVR::Configuration::SensorConfig calibration; + calibration.type = SlimeVR::Configuration::SensorConfigType::MPU6050; + calibration.data.mpu6050 = m_Config; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + + m_Logger.info("Calibration finished"); +#endif // !IMU_MPU6050_RUNTIME_CALIBRATION + + ledManager.off(); } diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index 926bd24b7..ad46a3999 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -1,62 +1,78 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_MPU6050SENSOR_H #define SENSORS_MPU6050SENSOR_H -#include "sensor.h" #include + #include "SensorFusionDMP.h" +#include "sensor.h" -class MPU6050Sensor : public Sensor -{ +class MPU6050Sensor : public Sensor { public: - static constexpr auto TypeID = ImuID::MPU6050; - static constexpr uint8_t Address = 0x68; + static constexpr auto TypeID = ImuID::MPU6050; + static constexpr uint8_t Address = 0x68; - MPU6050Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, 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; + MPU6050Sensor( + uint8_t id, + uint8_t addrSuppl, + float rotation, + uint8_t sclPin, + 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{}; - Quaternion rawQuat{}; - VectorInt16 rawAccel{}; - // MPU dmp control/status vars - bool dmpReady = false; // set true if DMP init was successful - uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU - uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) - uint16_t packetSize; // expected DMP packet size (default is 42 bytes) - uint16_t fifoCount; // count of all bytes currently in FIFO - uint8_t fifoBuffer[64]{}; // FIFO storage buffer - - SlimeVR::Sensors::SensorFusionDMP sfusion; + MPU6050 imu{}; + Quaternion rawQuat{}; + VectorInt16 rawAccel{}; + // MPU dmp control/status vars + bool dmpReady = false; // set true if DMP init was successful + uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU + uint8_t devStatus; // return status after each device operation (0 = success, !0 = + // error) + uint16_t packetSize; // expected DMP packet size (default is 42 bytes) + uint16_t fifoCount; // count of all bytes currently in FIFO + uint8_t fifoBuffer[64]{}; // FIFO storage buffer + + SlimeVR::Sensors::SensorFusionDMP sfusion; #ifndef IMU_MPU6050_RUNTIME_CALIBRATION - SlimeVR::Configuration::MPU6050SensorConfig m_Config = {}; + SlimeVR::Configuration::MPU6050SensorConfig m_Config = {}; #endif }; diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 84a7da63b..6e4bc9bc4 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -1,421 +1,491 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain, S.J. Remington & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain, S.J. Remington & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "mpu9250sensor.h" -#include "globals.h" -#include "helper_3dmath.h" + #include + +#include "GlobalVars.h" #include "calibration.h" +#include "globals.h" +#include "helper_3dmath.h" #include "magneto1.4.h" -#include "GlobalVars.h" #if MPU_USE_DMPMAG #include "dmpmag.h" #endif -//#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 -//#endif +// #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 +// #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 - imu.initialize(addr); - if(!imu.testConnection()) { - m_Logger.fatal("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); - - int16_t ax,ay,az; - - // turn on while flip back to calibrate. then, flip again after 5 seconds. - // TODO: Move calibration invoke after calibrate button on slimeVR server available - imu.getAcceleration(&ax, &ay, &az); - float g_az = (float)az / 16384; // For 2G sensitivity - if(g_az < -0.75f) { - ledManager.on(); - m_Logger.info("Flip front to confirm start calibration"); - delay(5000); - ledManager.off(); - - imu.getAcceleration(&ax, &ay, &az); - g_az = (float)az / 16384; - if(g_az > 0.75f) { - m_Logger.debug("Starting calibration..."); - startCalibration(0); - } - } - - // Initialize the configuration - { - SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorConfig.type) { - case SlimeVR::Configuration::SensorConfigType::MPU9250: - m_Config = sensorConfig.data.mpu9250; - break; - - case SlimeVR::Configuration::SensorConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } - } + // initialize device + imu.initialize(addr); + if (!imu.testConnection()) { + m_Logger.fatal( + "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 + ); + + int16_t ax, ay, az; + + // turn on while flip back to calibrate. then, flip again after 5 seconds. + // TODO: Move calibration invoke after calibrate button on slimeVR server available + imu.getAcceleration(&ax, &ay, &az); + float g_az = (float)az / 16384; // For 2G sensitivity + if (g_az < -0.75f) { + ledManager.on(); + m_Logger.info("Flip front to confirm start calibration"); + delay(5000); + ledManager.off(); + + imu.getAcceleration(&ax, &ay, &az); + g_az = (float)az / 16384; + if (g_az > 0.75f) { + m_Logger.debug("Starting calibration..."); + startCalibration(0); + } + } + + // Initialize the configuration + { + SlimeVR::Configuration::SensorConfig sensorConfig + = configuration.getSensor(sensorId); + // If no compatible calibration data is found, the calibration data will just be + // zero-ed out + switch (sensorConfig.type) { + case SlimeVR::Configuration::SensorConfigType::MPU9250: + m_Config = sensorConfig.data.mpu9250; + break; + + case SlimeVR::Configuration::SensorConfigType::NONE: + m_Logger.warn( + "No calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn( + "Incompatible calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + } + } #if MPU_USE_DMPMAG - uint8_t devStatus = imu.dmpInitialize(); - if(devStatus == 0){ - ledManager.pattern(50, 50, 5); - - // turn on the DMP, now that it's ready - m_Logger.debug("Enabling DMP..."); - imu.setDMPEnabled(true); - - // TODO: Add interrupt support - // mpuIntStatus = imu.getIntStatus(); - - // 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..."); - dmpReady = true; - - // get expected DMP packet size for later comparison - packetSize = imu.dmpGetFIFOPacketSize(); - working = true; - } else { - // ERROR! - // 1 = initial memory load failed - // 2 = DMP configuration updates failed - // (if it's going to break, usually the code will be 1) - m_Logger.error("DMP Initialization failed (code %d)", devStatus); - } + uint8_t devStatus = imu.dmpInitialize(); + if (devStatus == 0) { + ledManager.pattern(50, 50, 5); + + // turn on the DMP, now that it's ready + m_Logger.debug("Enabling DMP..."); + imu.setDMPEnabled(true); + + // TODO: Add interrupt support + // mpuIntStatus = imu.getIntStatus(); + + // 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..."); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = imu.dmpGetFIFOPacketSize(); + working = true; + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + 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. - imu.setAccelFIFOEnabled(true); - imu.setXGyroFIFOEnabled(true); - imu.setYGyroFIFOEnabled(true); - imu.setZGyroFIFOEnabled(true); - imu.setSlave0FIFOEnabled(true); + // NOTE: could probably combine these into less total writes, but this should work, + // and isn't time critical. + imu.setAccelFIFOEnabled(true); + imu.setXGyroFIFOEnabled(true); + imu.setYGyroFIFOEnabled(true); + imu.setZGyroFIFOEnabled(true); + imu.setSlave0FIFOEnabled(true); - // Set a rate we prefer - imu.setRate(MPU9250_SAMPLE_DIV); // 8khz / (1 + MPU9250_SAMPLE_DIV) + // Set a rate we prefer + imu.setRate(MPU9250_SAMPLE_DIV); // 8khz / (1 + MPU9250_SAMPLE_DIV) - imu.resetFIFO(); - imu.setFIFOEnabled(true); + imu.resetFIFO(); + 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; - imu.getRotation(&rX, &rY, &rZ); - imu.getAcceleration(&aX, &aY, &aZ); - imu.getMagnetometer(&mX, &mY, &mZ); - - networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); - } + { + int16_t rX, rY, rZ, aX, aY, aZ, mX, mY, mZ; + imu.getRotation(&rX, &rY, &rZ); + imu.getAcceleration(&aX, &aY, &aZ); + imu.getMagnetometer(&mX, &mY, &mZ); + + networkConnection.sendInspectionRawIMUData( + sensorId, + rX, + rY, + rZ, + 255, + aX, + aY, + aZ, + 255, + mX, + mY, + mZ, + 255 + ); + } #endif #if MPU_USE_DMPMAG - // Update quaternion - if(!dmpReady) - return; - Quaternion rawQuat{}; - uint8_t dmpPacket[packetSize]; - if(!imu.GetCurrentFIFOPacket(dmpPacket, packetSize)) return; - if(imu.dmpGetQuaternion(&rawQuat, dmpPacket)) return; // FIFO CORRUPTED - hadData = true; - - sfusion.updateQuaternion(rawQuat); - - int16_t temp[3]; - imu.getMagnetometer(&temp[0], &temp[1], &temp[2]); - parseMagData(temp); - - sfusion.updateMag(Mxyz); - #if SEND_ACCELERATION - { - int16_t atemp[3]; - this->imu.dmpGetAccel(atemp, dmpPacket); - parseAccelData(atemp); - - sfusion.updateAcc(Axyz); - } - #endif + // Update quaternion + if (!dmpReady) { + return; + } + Quaternion rawQuat{}; + uint8_t dmpPacket[packetSize]; + if (!imu.GetCurrentFIFOPacket(dmpPacket, packetSize)) { + return; + } + if (imu.dmpGetQuaternion(&rawQuat, dmpPacket)) { + return; // FIFO CORRUPTED + } + hadData = true; + + sfusion.updateQuaternion(rawQuat); + + int16_t temp[3]; + imu.getMagnetometer(&temp[0], &temp[1], &temp[2]); + parseMagData(temp); + + sfusion.updateMag(Mxyz); +#if SEND_ACCELERATION + { + int16_t atemp[3]; + this->imu.dmpGetAccel(atemp, dmpPacket); + parseAccelData(atemp); + + 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); - } + 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 + 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 - constexpr int calibrationSamples = 300; - - // Blink calibrating led before user should rotate the sensor - m_Logger.info("Gently rotate the device while it's gathering magnetometer data"); - ledManager.pattern(15, 300, 3000/310); - MagnetoCalibration *magneto = new MagnetoCalibration(); - for (int i = 0; i < calibrationSamples; i++) { - ledManager.on(); - int16_t mx,my,mz; - imu.getMagnetometer(&mx, &my, &mz); - magneto->sample(my, mx, -mz); - - ledManager.off(); - delay(250); - } - m_Logger.debug("Calculating calibration data..."); - - float M_BAinv[4][3]; - magneto->current_calibration(M_BAinv); - delete magneto; - - m_Logger.debug("[INFO] Magnetometer calibration matrix:"); - m_Logger.debug("{"); - 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_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("}"); + // with DMP, we just need mag data + constexpr int calibrationSamples = 300; + + // Blink calibrating led before user should rotate the sensor + m_Logger.info("Gently rotate the device while it's gathering magnetometer data"); + ledManager.pattern(15, 300, 3000 / 310); + MagnetoCalibration* magneto = new MagnetoCalibration(); + for (int i = 0; i < calibrationSamples; i++) { + ledManager.on(); + int16_t mx, my, mz; + imu.getMagnetometer(&mx, &my, &mz); + magneto->sample(my, mx, -mz); + + ledManager.off(); + delay(250); + } + m_Logger.debug("Calculating calibration data..."); + + float M_BAinv[4][3]; + magneto->current_calibration(M_BAinv); + delete magneto; + + m_Logger.debug("[INFO] Magnetometer calibration matrix:"); + m_Logger.debug("{"); + 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_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("}"); #else - m_Logger.debug("Gathering raw data for device calibration..."); - constexpr int calibrationSamples = 300; - // Reset values - Gxyz[0] = 0; - Gxyz[1] = 0; - Gxyz[2] = 0; - - // Wait for sensor to calm down before calibration - m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); - delay(2000); - - union fifo_sample_raw buf; - - imu.resetFIFO(); // fifo is sure to have filled up in the seconds of delay, don't try reading it. - for (int i = 0; i < calibrationSamples; i++) { - // wait for new sample - while (!getNextSample(&buf, nullptr)) { ; } - - Gxyz[0] += float(buf.sample.gyro[0]); - Gxyz[1] += float(buf.sample.gyro[1]); - Gxyz[2] += float(buf.sample.gyro[2]); - } - Gxyz[0] /= calibrationSamples; - Gxyz[1] /= calibrationSamples; - Gxyz[2] /= calibrationSamples; + m_Logger.debug("Gathering raw data for device calibration..."); + constexpr int calibrationSamples = 300; + // Reset values + Gxyz[0] = 0; + Gxyz[1] = 0; + Gxyz[2] = 0; + + // Wait for sensor to calm down before calibration + m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); + delay(2000); + + union fifo_sample_raw buf; + + imu.resetFIFO( + ); // fifo is sure to have filled up in the seconds of delay, don't try reading it. + for (int i = 0; i < calibrationSamples; i++) { + // wait for new sample + while (!getNextSample(&buf, nullptr)) { + ; + } + + Gxyz[0] += float(buf.sample.gyro[0]); + Gxyz[1] += float(buf.sample.gyro[1]); + Gxyz[2] += float(buf.sample.gyro[2]); + } + Gxyz[0] /= calibrationSamples; + Gxyz[1] /= 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? - m_Config.G_off[0] = Gxyz[0]; - m_Config.G_off[1] = Gxyz[1]; - m_Config.G_off[2] = Gxyz[2]; - - // Blink calibrating led before user should rotate the sensor - m_Logger.info("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_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. - for (int i = 0; i < calibrationSamples; i++) { - ledManager.on(); - int16_t ax,ay,az,gx,gy,gz,mx,my,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(); - delay(250); - } - m_Logger.debug("Calculating calibration data..."); - - float A_BAinv[4][3]; - magneto_acc->current_calibration(A_BAinv); - delete magneto_acc; - - float M_BAinv[4][3]; - magneto_mag->current_calibration(M_BAinv); - delete magneto_mag; - - m_Logger.debug("Finished Calculate Calibration data"); - m_Logger.debug("Accelerometer calibration matrix:"); - m_Logger.debug("{"); - for (int i = 0; i < 3; i++) - { - m_Config.A_B[i] = A_BAinv[0][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[2][i] = A_BAinv[3][i]; - m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); - } - m_Logger.debug("}"); - m_Logger.debug("[INFO] Magnetometer calibration matrix:"); - m_Logger.debug("{"); - 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_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("}"); + // TODO: use offset registers? + m_Config.G_off[0] = Gxyz[0]; + m_Config.G_off[1] = Gxyz[1]; + m_Config.G_off[2] = Gxyz[2]; + + // Blink calibrating led before user should rotate the sensor + m_Logger.info( + "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_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. + for (int i = 0; i < calibrationSamples; i++) { + ledManager.on(); + int16_t ax, ay, az, gx, gy, gz, mx, my, 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(); + delay(250); + } + m_Logger.debug("Calculating calibration data..."); + + float A_BAinv[4][3]; + magneto_acc->current_calibration(A_BAinv); + delete magneto_acc; + + float M_BAinv[4][3]; + magneto_mag->current_calibration(M_BAinv); + delete magneto_mag; + + m_Logger.debug("Finished Calculate Calibration data"); + m_Logger.debug("Accelerometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + m_Config.A_B[i] = A_BAinv[0][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[2][i] = A_BAinv[3][i]; + m_Logger.debug( + " %f, %f, %f, %f", + A_BAinv[0][i], + A_BAinv[1][i], + A_BAinv[2][i], + A_BAinv[3][i] + ); + } + m_Logger.debug("}"); + m_Logger.debug("[INFO] Magnetometer calibration matrix:"); + m_Logger.debug("{"); + 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_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; - config.type = SlimeVR::Configuration::SensorConfigType::MPU9250; - config.data.mpu9250 = m_Config; - configuration.setSensor(sensorId, config); - configuration.save(); + SlimeVR::Configuration::SensorConfig config; + config.type = SlimeVR::Configuration::SensorConfigType::MPU9250; + config.data.mpu9250 = m_Config; + configuration.setSensor(sensorId, config); + configuration.save(); - ledManager.off(); - m_Logger.debug("Saved the calibration data"); + ledManager.off(); + m_Logger.debug("Saved the calibration data"); - m_Logger.info("Calibration data gathered"); - // fifo will certainly have overflown due to magnetometer calibration, reset it. - imu.resetFIFO(); + m_Logger.info("Calibration data gathered"); + // fifo will certainly have overflown due to magnetometer calibration, reset it. + imu.resetFIFO(); } void MPU9250Sensor::parseMagData(int16_t data[3]) { - // reading *little* endian int16 - Mxyz[0] = (float)data[1]; // my - Mxyz[1] = (float)data[0]; // mx - Mxyz[2] = -(float)data[2]; // mz - - float temp[3]; - - //apply offsets and scale factors from Magneto - for (unsigned i = 0; i < 3; i++) { - temp[i] = (Mxyz[i] - m_Config.M_B[i]); - } - - for (unsigned i = 0; i < 3; i++) { - #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]; - #else - Mxyz[i] = temp[i]; - #endif - } + // reading *little* endian int16 + Mxyz[0] = (float)data[1]; // my + Mxyz[1] = (float)data[0]; // mx + Mxyz[2] = -(float)data[2]; // mz + + float temp[3]; + + // apply offsets and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { + temp[i] = (Mxyz[i] - m_Config.M_B[i]); + } + + for (unsigned i = 0; i < 3; i++) { +#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]; +#else + Mxyz[i] = temp[i]; +#endif + } } void MPU9250Sensor::parseAccelData(int16_t data[3]) { - // reading big endian int16 - Axyz[0] = (float)data[0]; - Axyz[1] = (float)data[1]; - Axyz[2] = (float)data[2]; - - #if !MPU_USE_DMPMAG - float temp[3]; - #endif - - //apply offsets (bias) and scale factors from Magneto - for (unsigned i = 0; i < 3; i++) { - #if !MPU_USE_DMPMAG - temp[i] = (Axyz[i] - m_Config.A_B[i]); - } - - for (unsigned i = 0; i < 3; i++) { - #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]; - #else - Axyz[i] = temp[i]; - #endif - #endif - Axyz[i] *= ASCALE_2G; - } + // reading big endian int16 + Axyz[0] = (float)data[0]; + Axyz[1] = (float)data[1]; + Axyz[2] = (float)data[2]; + +#if !MPU_USE_DMPMAG + float temp[3]; +#endif + + // apply offsets (bias) and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { +#if !MPU_USE_DMPMAG + temp[i] = (Axyz[i] - m_Config.A_B[i]); + } + + for (unsigned i = 0; i < 3; i++) { +#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]; +#else + Axyz[i] = temp[i]; +#endif +#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 - Gxyz[0] = ((float)data[0] - m_Config.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s - Gxyz[1] = ((float)data[1] - m_Config.G_off[1]) * gscale; - Gxyz[2] = ((float)data[2] - m_Config.G_off[2]) * gscale; + // reading big endian int16 + Gxyz[0] = ((float)data[0] - m_Config.G_off[0]) + * gscale; // 250 LSB(d/s) default to radians/s + 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__ - // byteswap the big endian integers - for (unsigned iii = 0; iii < 12; iii += 2) { - uint8_t tmp = sample->raw[iii + 0]; - sample->raw[iii + 0] = sample->raw[iii + 1]; - sample->raw[iii + 1] = tmp; - } - #elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ - // byteswap the little endian integers - for (unsigned iii = 12; iii < 18; iii += 2) { - uint8_t tmp = sample->raw[iii + 0]; - sample->raw[iii + 0] = sample->raw[iii + 1]; - sample->raw[iii + 1] = tmp; - } - #else - #error "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 - memmove(&sample->sample, sample->raw, sensor_data_len); +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + // byteswap the big endian integers + for (unsigned iii = 0; iii < 12; iii += 2) { + uint8_t tmp = sample->raw[iii + 0]; + sample->raw[iii + 0] = sample->raw[iii + 1]; + sample->raw[iii + 1] = tmp; + } +#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ + // byteswap the little endian integers + for (unsigned iii = 12; iii < 18; iii += 2) { + uint8_t tmp = sample->raw[iii + 0]; + sample->raw[iii + 0] = sample->raw[iii + 1]; + sample->raw[iii + 1] = tmp; + } +#else +#error \ + "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 + 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?) -// TODO: strict aliasing might not be violated if we just read directly into a fifo_sample*. +// we could read into a properly aligned array of fifo_samples (and not require a +// 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) { - uint16_t count = imu.getFIFOCount(); - if (count < sensor_data_len) { - // no samples to read - remaining_count = 0; - return false; - } - - if (remaining_count) { - *remaining_count = (count / sensor_data_len) - 1; - } - - imu.getFIFOBytes(buffer->raw, sensor_data_len); - swapFifoData(buffer); - return true; +bool MPU9250Sensor::getNextSample( + union fifo_sample_raw* buffer, + uint16_t* remaining_count +) { + uint16_t count = imu.getFIFOCount(); + if (count < sensor_data_len) { + // no samples to read + remaining_count = 0; + return false; + } + + if (remaining_count) { + *remaining_count = (count / sensor_data_len) - 1; + } + + imu.getFIFOBytes(buffer->raw, sensor_data_len); + swapFifoData(buffer); + return true; } diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index 02b28a3cf..d6d0b727d 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -1,37 +1,38 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_MPU9250SENSOR_H #define SENSORS_MPU9250SENSOR_H -#include "sensor.h" -#include "logging/Logger.h" - #include +#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 uint8_t Address = 0x68; - - MPU9250Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) - : Sensor("MPU9250Sensor", 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(); + static constexpr auto TypeID = ImuID::MPU9250; + static constexpr uint8_t Address = 0x68; + + MPU9250Sensor( + uint8_t id, + uint8_t addrSuppl, + float rotation, + uint8_t sclPin, + uint8_t sdaPin, + uint8_t + ) + : Sensor( + "MPU9250Sensor", + 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{}; - bool dmpReady = false; // set true if DMP init was successful - // TODO: actually check interrupt status - // uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU - uint16_t packetSize; // expected DMP packet size (default is 42 bytes) - - #if MPU_USE_DMPMAG - SlimeVR::Sensors::SensorFusionDMP sfusion; - #else - SlimeVR::Sensors::SensorFusion sfusion; - #endif - - // raw data and scaled as vector - float Axyz[3]{}; - float Gxyz[3]{}; - float Mxyz[3]{}; - VectorInt16 rawAccel{}; - Quat correction{0, 0, 0, 0}; - - SlimeVR::Configuration::MPU9250SensorConfig m_Config = {}; - - // outputs to respective member variables - void parseAccelData(int16_t data[3]); - void parseGyroData(int16_t data[3]); - void parseMagData(int16_t data[3]); - - // 6 bytes for gyro, 6 bytes for accel, 7 bytes for magnetometer - static constexpr uint16_t sensor_data_len = 19; - - struct fifo_sample { - int16_t accel[3]; - int16_t gyro[3]; - int16_t mag[3]; - uint8_t mag_status; - }; - - // acts as a memory space for getNextSample. upon success, can read from the sample member - // TODO: this may be overcomplicated, we may be able to just use fifo_sample and i misunderstood strict aliasing rules. - 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. - bool getNextSample(union fifo_sample_raw *buffer, uint16_t *remaining_count); - static void swapFifoData(union fifo_sample_raw* sample); + MPU9250 imu{}; + bool dmpReady = false; // set true if DMP init was successful + // TODO: actually check interrupt status + // uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU + uint16_t packetSize; // expected DMP packet size (default is 42 bytes) + +#if MPU_USE_DMPMAG + SlimeVR::Sensors::SensorFusionDMP sfusion; +#else + SlimeVR::Sensors::SensorFusion sfusion; +#endif + + // raw data and scaled as vector + float Axyz[3]{}; + float Gxyz[3]{}; + float Mxyz[3]{}; + VectorInt16 rawAccel{}; + Quat correction{0, 0, 0, 0}; + + SlimeVR::Configuration::MPU9250SensorConfig m_Config = {}; + + // outputs to respective member variables + void parseAccelData(int16_t data[3]); + void parseGyroData(int16_t data[3]); + void parseMagData(int16_t data[3]); + + // 6 bytes for gyro, 6 bytes for accel, 7 bytes for magnetometer + static constexpr uint16_t sensor_data_len = 19; + + struct fifo_sample { + int16_t accel[3]; + int16_t gyro[3]; + int16_t mag[3]; + uint8_t mag_status; + }; + + // acts as a memory space for getNextSample. upon success, can read from the sample + // member + // TODO: this may be overcomplicated, we may be able to just use fifo_sample and i + // misunderstood strict aliasing rules. + 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. + bool getNextSample(union fifo_sample_raw* buffer, uint16_t* remaining_count); + static void swapFifoData(union fifo_sample_raw* sample); }; #endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index deaccfc96..db9573daa 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -1,117 +1,136 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "sensor.h" -#include "GlobalVars.h" + #include + +#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; - newAcceleration = true; + acceleration = a; + newAcceleration = true; } void Sensor::setFusedRotation(Quat r) { - fusedRotation = r * sensorOffset; - bool changed = OPTIMIZE_UPDATES ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) : true; - if (ENABLE_INSPECTION || changed) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } + fusedRotation = r * sensorOffset; + bool changed = OPTIMIZE_UPDATES + ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) + : true; + if (ENABLE_INSPECTION || changed) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } } void Sensor::sendData() { - if (newFusedRotation) { - newFusedRotation = false; - networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); + if (newFusedRotation) { + newFusedRotation = false; + 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) { - newAcceleration = false; - networkConnection.sendSensorAcceleration(sensorId, acceleration); - } + if (newAcceleration) { + newAcceleration = false; + 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::printDebugTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +void Sensor::printTemperatureCalibrationState() { + 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) { - switch(imuType) { - case ImuID::MPU9250: - return "MPU9250"; - case ImuID::MPU6500: - return "MPU6500"; - case ImuID::BNO080: - return "BNO080"; - case ImuID::BNO085: - return "BNO085"; - case ImuID::BNO055: - return "BNO055"; - case ImuID::MPU6050: - return "MPU6050"; - case ImuID::BNO086: - return "BNO086"; - case ImuID::BMI160: - return "BMI160"; - case ImuID::ICM20948: - return "ICM20948"; - case ImuID::ICM42688: - return "ICM42688"; - case ImuID::BMI270: - return "BMI270"; - case ImuID::LSM6DS3TRC: - return "LSM6DS3TRC"; - case ImuID::LSM6DSV: - return "LSM6DSV"; - case ImuID::LSM6DSO: - return "LSM6DSO"; - case ImuID::LSM6DSR: - return "LSM6DSR"; - case ImuID::Unknown: - case ImuID::Empty: - return "UNKNOWN"; - } - return "Unknown"; +const char* getIMUNameByType(ImuID imuType) { + switch (imuType) { + case ImuID::MPU9250: + return "MPU9250"; + case ImuID::MPU6500: + return "MPU6500"; + case ImuID::BNO080: + return "BNO080"; + case ImuID::BNO085: + return "BNO085"; + case ImuID::BNO055: + return "BNO055"; + case ImuID::MPU6050: + return "MPU6050"; + case ImuID::BNO086: + return "BNO086"; + case ImuID::BMI160: + return "BMI160"; + case ImuID::ICM20948: + return "ICM20948"; + case ImuID::ICM42688: + return "ICM42688"; + case ImuID::BMI270: + return "BMI270"; + case ImuID::LSM6DS3TRC: + return "LSM6DS3TRC"; + case ImuID::LSM6DSV: + return "LSM6DSV"; + case ImuID::LSM6DSO: + return "LSM6DSO"; + case ImuID::LSM6DSR: + return "LSM6DSR"; + case ImuID::Unknown: + case ImuID::Empty: + return "UNKNOWN"; + } + return "Unknown"; } diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index d678ba400..6b52177b4 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -27,6 +27,7 @@ #include #include #include + #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) - : addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName)), - sclPin(sclpin), sdaPin(sdapin) - { + Sensor( + const char* sensorName, + 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 @@ class Sensor virtual void saveTemperatureCalibration(); virtual void setFlag(uint16_t flagId, bool state){}; virtual uint16_t getSensorConfigData(); - bool isWorking() { - return working; - }; - bool getHadData() const { - return hadData; - }; - bool isValid() { - return sclPin != sdaPin; - }; - bool isMagEnabled() { - 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; - }; + bool isWorking() { return working; }; + bool getHadData() const { return hadData; }; + bool isValid() { return sclPin != sdaPin; }; + bool isMagEnabled() { 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 @@ class Sensor void printTemperatureCalibrationUnsupported(); }; -const char * getIMUNameByType(ImuID imuType); +const char* getIMUNameByType(ImuID imuType); -#endif // SLIMEVR_SENSOR_H_ +#endif // SLIMEVR_SENSOR_H_ diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h index 0282e32f4..e2c37f1e8 100644 --- a/src/sensors/sensoraddresses.h +++ b/src/sensors/sensoraddresses.h @@ -1,5 +1,5 @@ -// those variables are used as "supplement" to base IMU address coming directly from IMU driver -// they are remaining to keep backward compatibility with old style of defines.h +// those variables are used as "supplement" to base IMU address coming directly from IMU +// 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 diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index b5f49e52e..7938e0ca8 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -1,36 +1,37 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Tailsy13 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include #include +#include +#include +#include #include + #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 -struct BMI270 -{ - static constexpr uint8_t Address = 0x68; - static constexpr auto Name = "BMI270"; - static constexpr auto Type = ImuID::BMI270; - - static constexpr float GyrTs=1.0/400.0; - static constexpr float AccTs=1.0/100.0; - - static constexpr float MagTs=1.0/100; - - static constexpr float GyroSensitivity = 32.768f; - static constexpr float AccelSensitivity = 2048.0f; - - struct MotionlessCalibrationData - { - bool valid; - uint8_t x, y, z; - }; - - I2CImpl i2c; - SlimeVR::Logging::Logger &logger; - int8_t zxFactor; - BMI270(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger), zxFactor(0) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x00; - static constexpr uint8_t value = 0x24; - }; - static constexpr uint8_t TempData = 0x22; - - struct Cmd { - static constexpr uint8_t reg = 0x7e; - static constexpr uint8_t valueSwReset = 0xb6; - static constexpr uint8_t valueFifoFlush = 0xb0; - static constexpr uint8_t valueGTrigger = 0x02; - }; - - struct PwrConf { - static constexpr uint8_t reg = 0x7c; - static constexpr uint8_t valueNoPowerSaving = 0x0; - static constexpr uint8_t valueFifoSelfWakeup = 0x2; - }; - - struct PwrCtrl { - static constexpr uint8_t reg = 0x7d; - static constexpr uint8_t valueOff = 0x0; - static constexpr uint8_t valueGyrAccTempOn = 0b1110; // aux off - static constexpr uint8_t valueAccOn = 0b0100; // aux, gyr, temp off - }; - - struct InitCtrl { - static constexpr uint8_t reg = 0x59; - static constexpr uint8_t valueStartInit = 0x00; - static constexpr uint8_t valueEndInit = 0x01; - }; - - static constexpr uint8_t InitAddr = 0x5b; - static constexpr uint8_t InitData = 0x5e; - - struct InternalStatus { - static constexpr uint8_t reg = 0x21; - static constexpr uint8_t initializedBit = 0x01; - }; - - struct GyrConf { - static constexpr uint8_t reg = 0x42; - - static constexpr uint8_t rate25Hz = 6; - static constexpr uint8_t rate50Hz = 7; - static constexpr uint8_t rate100Hz = 8; - static constexpr uint8_t rate200Hz = 9; - static constexpr uint8_t rate400Hz = 10; - static constexpr uint8_t rate800Hz = 11; - static constexpr uint8_t rate1600Hz = 12; - static constexpr uint8_t rate3200Hz = 13; - - static constexpr uint8_t DLPFModeOsr4 = 0 << 4; - static constexpr uint8_t DLPFModeOsr2 = 1 << 4; - static constexpr uint8_t DLPFModeNorm = 2 << 4; - - static constexpr uint8_t noisePerfMode = 1 << 6; - static constexpr uint8_t filterHighPerfMode = 1 << 7; - - static constexpr uint8_t value = rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode; - }; - - struct GyrRange { - static constexpr uint8_t reg = 0x43; - - static constexpr uint8_t range125dps = 4; - static constexpr uint8_t range250dps = 3; - static constexpr uint8_t range500dps = 2; - static constexpr uint8_t range1000dps = 1; - static constexpr uint8_t range2000dps = 0; - - static constexpr uint8_t value = range1000dps; - }; - - 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 rate3_1Hz = 3; - static constexpr uint8_t rate6_25Hz = 4; - static constexpr uint8_t rate12_5Hz = 5; - static constexpr uint8_t rate25Hz = 6; - static constexpr uint8_t rate50Hz = 7; - static constexpr uint8_t rate100Hz = 8; - static constexpr uint8_t rate200Hz = 9; - static constexpr uint8_t rate400Hz = 10; - 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 DLPFModeAvg4 = 2 << 4; - static constexpr uint8_t DLPFModeAvg8 = 3 << 4; - - static constexpr uint8_t filterHighPerfMode = 1 << 7; - - static constexpr uint8_t value = rate100Hz | DLPFModeAvg4 | filterHighPerfMode; - }; - - struct AccRange { - static constexpr uint8_t reg = 0x41; - - static constexpr uint8_t range2G = 0; - static constexpr uint8_t range4G = 1; - static constexpr uint8_t range8G = 2; - 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 FifoConfig1 { - static constexpr uint8_t reg = 0x49; - static constexpr uint8_t value = (1 << 4) | (1 << 6) | (1 << 7); // header en, acc en, gyr en - }; - - struct GyrCrtConf { - static constexpr uint8_t reg = 0x69; - static constexpr uint8_t valueRunning = (1 << 2); // crt_running = 1 - static constexpr uint8_t valueStopped = 0x0; // crt_running = 0 - }; - - struct GTrig1 { // on feature page 1! - static constexpr uint8_t reg = 0x32; - static constexpr uint16_t valueTriggerCRT = (1 << 8); // select=crt - }; - - struct GyrGainStatus { // on feature page 0! - static constexpr uint8_t reg = 0x38; - static constexpr uint8_t statusOffset = 3; - }; - - struct Offset6 { // on feature page 0! - static constexpr uint8_t reg = 0x77; - static constexpr uint8_t value = (1 << 7); // gyr_gain_en = 1 - }; - - static constexpr uint8_t FeatPage = 0x2f; - - static constexpr uint8_t GyrUserGain = 0x78; // undocumented reg, got from official bmi270 driver - - static constexpr uint8_t FifoCount = 0x24; - static constexpr uint8_t FifoData = 0x26; - static constexpr uint8_t RaGyrCas = 0x3c; // on feature page 0! - }; - - struct Fifo { - static constexpr uint8_t ModeMask = 0b11000000; - static constexpr uint8_t SkipFrame = 0b01000000; - static constexpr uint8_t DataFrame = 0b10000000; - - static constexpr uint8_t GyrDataBit = 0b00001000; - static constexpr uint8_t AccelDataBit = 0b00000100; - }; - - bool restartAndInit() { - // perform initialization step - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); - delay(12); - // disable power saving - i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); - delay(1); - - // firmware upload - i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); - for (uint16_t pos=0; pos> 1; // convert current position to words - const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00); - i2c.writeReg16(Regs::InitAddr, position); - // write actual payload chunk - const uint16_t burstWrite = std::min(sizeof(bmi270_firmware) - pos, I2CImpl::MaxTransactionLength); - i2c.writeBytes(Regs::InitData, burstWrite, const_cast(bmi270_firmware + pos)); - pos += burstWrite; - } - i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit); - delay(140); - - // leave fifo_self_wakeup enabled - i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup); - // check if IMU initialized correctly - if (!(i2c.readReg(Regs::InternalStatus::reg) & Regs::InternalStatus::initializedBit)) - { - // firmware upload fail or sensor not initialized - return false; - } - - // read zx factor used to reduce gyro cross-sensitivity error - const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas); - const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80; - zxFactor = static_cast(zx_factor_reg | sign_byte); - return true; - } - - void setNormalConfig(MotionlessCalibrationData &gyroSensitivity) - { - i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value); - i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value); - - i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value); - i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value); - - if (gyroSensitivity.valid) - { - i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value); - i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x); - } - - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); - delay(100); // power up delay - i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); - i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); - - delay(4); - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush); - delay(2); - } - - bool initialize(MotionlessCalibrationData &gyroSensitivity) - { - if (!restartAndInit()) { - return false; - } - - setNormalConfig(gyroSensitivity); - - return true; - } - - void motionlessCalibration(MotionlessCalibrationData &gyroSensitivity) - { - // perfrom gyroscope motionless sensitivity calibration (CRT) - // need to start from clean state according to spec - restartAndInit(); - // only Accel ON - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn); - delay(100); - i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning); - i2c.writeReg(Regs::FeatPage, 1); - i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT); - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger); - delay(200); - - while(i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) { - logger.info("CRT running. Do not move tracker!"); - delay(200); - } - - i2c.writeReg(Regs::FeatPage, 0); - - uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg) >> Regs::GyrGainStatus::statusOffset; - // turn gyroscope back on - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); - delay(100); - - if (status != 0) { - logger.error("CRT failed with status 0x%x. Recalibrate again to enable CRT.", status); - if (status == 0x03) { - logger.error("Reason: tracker was moved during CRT!"); - } - } - else { - std::array crt_values; - i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data()); - logger.debug("CRT finished successfully, result 0x%x, 0x%x, 0x%x", crt_values[0], crt_values[1], crt_values[2]); - gyroSensitivity.valid = true; - gyroSensitivity.x = crt_values[0]; - gyroSensitivity.y = crt_values[1]; - gyroSensitivity.z = crt_values[2]; - } - - setNormalConfig(gyroSensitivity); - } - - float getDirectTemp() const - { - // middle value is 23 degrees C (0x0000) - // temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 degrees C (0x7FFF) - constexpr float TempStep = 128. / 65535; - const auto value = static_cast(i2c.readReg16(Regs::TempData)); - return static_cast(value) * TempStep + 23.0f; - } - - using FifoBuffer = std::array; - FifoBuffer read_buffer; - - template - inline T getFromFifo(uint32_t &position, FifoBuffer& fifo) { - T to_ret; - std::memcpy(&to_ret, &fifo[position], sizeof(T)); - position += sizeof(T); - return to_ret; - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); - - const auto bytes_to_read = std::min(static_cast(read_buffer.size()), - static_cast(fifo_bytes)); - i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); - - for (uint32_t i=0u; i(i, read_buffer); - if ((header & Fifo::ModeMask) == Fifo::SkipFrame && (i - bytes_to_read) >= 1) { - getFromFifo(i, read_buffer); // skip 1 byte - } - else if ((header & Fifo::ModeMask) == Fifo::DataFrame) { - const uint8_t required_length = - (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) + - ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit)) * 6; - if (i - bytes_to_read < required_length) { - // incomplete frame, will be re-read next time - break; - } - if (header & Fifo::GyrDataBit) { - int16_t gyro[3]; - gyro[0] = getFromFifo(i, read_buffer); - gyro[1] = getFromFifo(i, read_buffer); - gyro[2] = getFromFifo(i, read_buffer); - using ShortLimit = std::numeric_limits; - // apply zx factor, todo: this awful line should be simplified and validated - gyro[0] = std::clamp(static_cast(gyro[0]) - static_cast((static_cast(zxFactor) * gyro[2]) / 512), - static_cast(ShortLimit::min()), static_cast(ShortLimit::max())); - processGyroSample(gyro, GyrTs); - } - - if (header & Fifo::AccelDataBit) { - int16_t accel[3]; - accel[0] = getFromFifo(i, read_buffer); - accel[1] = getFromFifo(i, read_buffer); - accel[2] = getFromFifo(i, read_buffer); - processAccelSample(accel, AccTs); - } - } - } - } - +struct BMI270 { + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "BMI270"; + static constexpr auto Type = ImuID::BMI270; + + static constexpr float GyrTs = 1.0 / 400.0; + static constexpr float AccTs = 1.0 / 100.0; + + static constexpr float MagTs = 1.0 / 100; + + static constexpr float GyroSensitivity = 32.768f; + static constexpr float AccelSensitivity = 2048.0f; + + struct MotionlessCalibrationData { + bool valid; + uint8_t x, y, z; + }; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + int8_t zxFactor; + BMI270(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) + , zxFactor(0) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x00; + static constexpr uint8_t value = 0x24; + }; + static constexpr uint8_t TempData = 0x22; + + struct Cmd { + static constexpr uint8_t reg = 0x7e; + static constexpr uint8_t valueSwReset = 0xb6; + static constexpr uint8_t valueFifoFlush = 0xb0; + static constexpr uint8_t valueGTrigger = 0x02; + }; + + struct PwrConf { + static constexpr uint8_t reg = 0x7c; + static constexpr uint8_t valueNoPowerSaving = 0x0; + static constexpr uint8_t valueFifoSelfWakeup = 0x2; + }; + + struct PwrCtrl { + static constexpr uint8_t reg = 0x7d; + static constexpr uint8_t valueOff = 0x0; + static constexpr uint8_t valueGyrAccTempOn = 0b1110; // aux off + static constexpr uint8_t valueAccOn = 0b0100; // aux, gyr, temp off + }; + + struct InitCtrl { + static constexpr uint8_t reg = 0x59; + static constexpr uint8_t valueStartInit = 0x00; + static constexpr uint8_t valueEndInit = 0x01; + }; + + static constexpr uint8_t InitAddr = 0x5b; + static constexpr uint8_t InitData = 0x5e; + + struct InternalStatus { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t initializedBit = 0x01; + }; + + struct GyrConf { + static constexpr uint8_t reg = 0x42; + + static constexpr uint8_t rate25Hz = 6; + static constexpr uint8_t rate50Hz = 7; + static constexpr uint8_t rate100Hz = 8; + static constexpr uint8_t rate200Hz = 9; + static constexpr uint8_t rate400Hz = 10; + static constexpr uint8_t rate800Hz = 11; + static constexpr uint8_t rate1600Hz = 12; + static constexpr uint8_t rate3200Hz = 13; + + static constexpr uint8_t DLPFModeOsr4 = 0 << 4; + static constexpr uint8_t DLPFModeOsr2 = 1 << 4; + static constexpr uint8_t DLPFModeNorm = 2 << 4; + + static constexpr uint8_t noisePerfMode = 1 << 6; + static constexpr uint8_t filterHighPerfMode = 1 << 7; + + static constexpr uint8_t value + = rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode; + }; + + struct GyrRange { + static constexpr uint8_t reg = 0x43; + + static constexpr uint8_t range125dps = 4; + static constexpr uint8_t range250dps = 3; + static constexpr uint8_t range500dps = 2; + static constexpr uint8_t range1000dps = 1; + static constexpr uint8_t range2000dps = 0; + + static constexpr uint8_t value = range1000dps; + }; + + 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 rate3_1Hz = 3; + static constexpr uint8_t rate6_25Hz = 4; + static constexpr uint8_t rate12_5Hz = 5; + static constexpr uint8_t rate25Hz = 6; + static constexpr uint8_t rate50Hz = 7; + static constexpr uint8_t rate100Hz = 8; + static constexpr uint8_t rate200Hz = 9; + static constexpr uint8_t rate400Hz = 10; + 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 DLPFModeAvg4 = 2 << 4; + static constexpr uint8_t DLPFModeAvg8 = 3 << 4; + + static constexpr uint8_t filterHighPerfMode = 1 << 7; + + static constexpr uint8_t value + = rate100Hz | DLPFModeAvg4 | filterHighPerfMode; + }; + + struct AccRange { + static constexpr uint8_t reg = 0x41; + + static constexpr uint8_t range2G = 0; + static constexpr uint8_t range4G = 1; + static constexpr uint8_t range8G = 2; + 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 FifoConfig1 { + static constexpr uint8_t reg = 0x49; + static constexpr uint8_t value + = (1 << 4) | (1 << 6) | (1 << 7); // header en, acc en, gyr en + }; + + struct GyrCrtConf { + static constexpr uint8_t reg = 0x69; + static constexpr uint8_t valueRunning = (1 << 2); // crt_running = 1 + static constexpr uint8_t valueStopped = 0x0; // crt_running = 0 + }; + + struct GTrig1 { // on feature page 1! + static constexpr uint8_t reg = 0x32; + static constexpr uint16_t valueTriggerCRT = (1 << 8); // select=crt + }; + + struct GyrGainStatus { // on feature page 0! + static constexpr uint8_t reg = 0x38; + static constexpr uint8_t statusOffset = 3; + }; + + struct Offset6 { // on feature page 0! + static constexpr uint8_t reg = 0x77; + static constexpr uint8_t value = (1 << 7); // gyr_gain_en = 1 + }; + + static constexpr uint8_t FeatPage = 0x2f; + + static constexpr uint8_t GyrUserGain + = 0x78; // undocumented reg, got from official bmi270 driver + + static constexpr uint8_t FifoCount = 0x24; + static constexpr uint8_t FifoData = 0x26; + static constexpr uint8_t RaGyrCas = 0x3c; // on feature page 0! + }; + + struct Fifo { + static constexpr uint8_t ModeMask = 0b11000000; + static constexpr uint8_t SkipFrame = 0b01000000; + static constexpr uint8_t DataFrame = 0b10000000; + + static constexpr uint8_t GyrDataBit = 0b00001000; + static constexpr uint8_t AccelDataBit = 0b00000100; + }; + + bool restartAndInit() { + // perform initialization step + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); + delay(12); + // disable power saving + i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); + delay(1); + + // firmware upload + i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); + for (uint16_t pos = 0; pos < sizeof(bmi270_firmware);) { + // tell the device current position + + // this thing is little endian, but it requires address in bizzare form + // LSB register is only 4 bits, while MSB register is 8bits + // also value requested is in words (16bit) not in bytes (8bit) + + const uint16_t pos_words = pos >> 1; // convert current position to words + const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00); + i2c.writeReg16(Regs::InitAddr, position); + // write actual payload chunk + const uint16_t burstWrite = std::min( + sizeof(bmi270_firmware) - pos, + I2CImpl::MaxTransactionLength + ); + i2c.writeBytes( + Regs::InitData, + burstWrite, + const_cast(bmi270_firmware + pos) + ); + pos += burstWrite; + } + i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit); + delay(140); + + // leave fifo_self_wakeup enabled + i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup); + // check if IMU initialized correctly + if (!(i2c.readReg(Regs::InternalStatus::reg) + & Regs::InternalStatus::initializedBit)) { + // firmware upload fail or sensor not initialized + return false; + } + + // read zx factor used to reduce gyro cross-sensitivity error + const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas); + const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80; + zxFactor = static_cast(zx_factor_reg | sign_byte); + return true; + } + + void setNormalConfig(MotionlessCalibrationData& gyroSensitivity) { + i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value); + i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value); + + i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value); + i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value); + + if (gyroSensitivity.valid) { + i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value); + i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x); + } + + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + delay(100); // power up delay + i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); + + delay(4); + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush); + delay(2); + } + + bool initialize(MotionlessCalibrationData& gyroSensitivity) { + if (!restartAndInit()) { + return false; + } + + setNormalConfig(gyroSensitivity); + + return true; + } + + void motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) { + // perfrom gyroscope motionless sensitivity calibration (CRT) + // need to start from clean state according to spec + restartAndInit(); + // only Accel ON + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn); + delay(100); + i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning); + i2c.writeReg(Regs::FeatPage, 1); + i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT); + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger); + delay(200); + + while (i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) { + logger.info("CRT running. Do not move tracker!"); + delay(200); + } + + i2c.writeReg(Regs::FeatPage, 0); + + uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg) + >> Regs::GyrGainStatus::statusOffset; + // turn gyroscope back on + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + delay(100); + + if (status != 0) { + logger.error( + "CRT failed with status 0x%x. Recalibrate again to enable CRT.", + status + ); + if (status == 0x03) { + logger.error("Reason: tracker was moved during CRT!"); + } + } else { + std::array crt_values; + i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data()); + logger.debug( + "CRT finished successfully, result 0x%x, 0x%x, 0x%x", + crt_values[0], + crt_values[1], + crt_values[2] + ); + gyroSensitivity.valid = true; + gyroSensitivity.x = crt_values[0]; + gyroSensitivity.y = crt_values[1]; + gyroSensitivity.z = crt_values[2]; + } + + setNormalConfig(gyroSensitivity); + } + + float getDirectTemp() const { + // middle value is 23 degrees C (0x0000) + // temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 + // degrees C (0x7FFF) + constexpr float TempStep = 128. / 65535; + const auto value = static_cast(i2c.readReg16(Regs::TempData)); + return static_cast(value) * TempStep + 23.0f; + } + + using FifoBuffer = std::array; + FifoBuffer read_buffer; + + template + inline T getFromFifo(uint32_t& position, FifoBuffer& fifo) { + T to_ret; + std::memcpy(&to_ret, &fifo[position], sizeof(T)); + position += sizeof(T); + return to_ret; + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ); + i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + + for (uint32_t i = 0u; i < bytes_to_read;) { + const uint8_t header = getFromFifo(i, read_buffer); + if ((header & Fifo::ModeMask) == Fifo::SkipFrame + && (i - bytes_to_read) >= 1) { + getFromFifo(i, read_buffer); // skip 1 byte + } else if ((header & Fifo::ModeMask) == Fifo::DataFrame) { + const uint8_t required_length + = (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) + + ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit)) + * 6; + if (i - bytes_to_read < required_length) { + // incomplete frame, will be re-read next time + break; + } + if (header & Fifo::GyrDataBit) { + int16_t gyro[3]; + gyro[0] = getFromFifo(i, read_buffer); + gyro[1] = getFromFifo(i, read_buffer); + gyro[2] = getFromFifo(i, read_buffer); + using ShortLimit = std::numeric_limits; + // apply zx factor, todo: this awful line should be simplified and + // validated + gyro[0] = std::clamp( + static_cast(gyro[0]) + - static_cast( + (static_cast(zxFactor) * gyro[2]) / 512 + ), + static_cast(ShortLimit::min()), + static_cast(ShortLimit::max()) + ); + processGyroSample(gyro, GyrTs); + } + + if (header & Fifo::AccelDataBit) { + int16_t accel[3]; + accel[0] = getFromFifo(i, read_buffer); + accel[1] = getFromFifo(i, read_buffer); + accel[2] = getFromFifo(i, read_buffer); + processAccelSample(accel, AccTs); + } + } + } + } }; -} // namespace \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/bmi270fw.h b/src/sensors/softfusion/drivers/bmi270fw.h index 87dda78e8..08479c8e4 100644 --- a/src/sensors/softfusion/drivers/bmi270fw.h +++ b/src/sensors/softfusion/drivers/bmi270fw.h @@ -1,478 +1,631 @@ /** -* Firmware extracted from BMI270 library. -* https://github.com/boschsensortec/BMI270-Sensor-API/blob/master/bmi270.c -* -* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -*/ + * Firmware extracted from BMI270 library. + * https://github.com/boschsensortec/BMI270-Sensor-API/blob/master/bmi270.c + * + * Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ #include -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { const uint8_t bmi270_firmware[] = { - 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc, - 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, - 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0x3b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x01, 0x00, 0x22, - 0x00, 0x75, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xb3, 0x43, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xe0, 0x5f, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, - 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0xfa, 0x00, 0x96, 0x00, 0x4b, 0x09, 0x11, 0x00, 0x11, 0x00, 0x02, 0x00, - 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, - 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, - 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x05, 0x00, 0xee, - 0x06, 0x04, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x04, 0x00, 0xa8, 0x05, 0xee, 0x06, 0x00, 0x04, 0xbc, 0x02, 0xb3, 0x00, - 0x85, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0xb4, 0x00, 0x01, 0x00, 0xb9, 0x00, 0x01, 0x00, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x80, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xde, - 0x00, 0xeb, 0x00, 0xda, 0x00, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0xc9, 0x01, 0x1e, 0xf2, - 0x80, 0x00, 0x3f, 0xff, 0x19, 0xf4, 0x58, 0xf5, 0x66, 0xf5, 0x64, 0xf5, 0xc0, 0xf1, 0xf0, 0x00, 0xe0, 0x00, 0xcd, - 0x01, 0xd3, 0x01, 0xdb, 0x01, 0xff, 0x7f, 0xff, 0x01, 0xe4, 0x00, 0x74, 0xf7, 0xf3, 0x00, 0xfa, 0x00, 0xff, 0x3f, - 0xca, 0x03, 0x6c, 0x38, 0x56, 0xfe, 0x44, 0xfd, 0xbc, 0x02, 0xf9, 0x06, 0x00, 0xfc, 0x12, 0x02, 0xae, 0x01, 0x58, - 0xfa, 0x9a, 0xfd, 0x77, 0x05, 0xbb, 0x02, 0x96, 0x01, 0x95, 0x01, 0x7f, 0x01, 0x82, 0x01, 0x89, 0x01, 0x87, 0x01, - 0x88, 0x01, 0x8a, 0x01, 0x8c, 0x01, 0x8f, 0x01, 0x8d, 0x01, 0x92, 0x01, 0x91, 0x01, 0xdd, 0x00, 0x9f, 0x01, 0x7e, - 0x01, 0xdb, 0x00, 0xb6, 0x01, 0x70, 0x69, 0x26, 0xd3, 0x9c, 0x07, 0x1f, 0x05, 0x9d, 0x00, 0x00, 0x08, 0xbc, 0x05, - 0x37, 0xfa, 0xa2, 0x01, 0xaa, 0x01, 0xa1, 0x01, 0xa8, 0x01, 0xa0, 0x01, 0xa8, 0x05, 0xb4, 0x01, 0xb4, 0x01, 0xce, - 0x00, 0xd0, 0x00, 0xfc, 0x00, 0xc5, 0x01, 0xff, 0xfb, 0xb1, 0x00, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xfc, 0xf5, - 0xcd, 0x01, 0xa0, 0x00, 0x5f, 0xff, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0x6d, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, - 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x67, 0x0f, 0x5b, 0x0f, 0x61, 0x0f, 0x80, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x83, 0x0f, - 0x86, 0x00, 0x72, 0x0f, 0x85, 0x0f, 0xc6, 0xf1, 0x7f, 0x0f, 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x87, - 0x0f, 0x8a, 0x0f, 0xff, 0x03, 0xf0, 0x3f, 0x8b, 0x00, 0x8e, 0x00, 0x90, 0x00, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, - 0xcb, 0x01, 0x20, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x50, 0x98, 0x2e, - 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, 0xf0, 0x7f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, - 0x2e, 0x01, 0x80, 0x08, 0xa2, 0xfb, 0x2f, 0x98, 0x2e, 0xba, 0x03, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0xee, 0x00, - 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x01, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x07, - 0xcc, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x27, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x05, 0x52, 0x98, 0x2e, 0xc7, 0xc1, - 0x03, 0x2e, 0xe9, 0x00, 0x40, 0xb2, 0xf0, 0x7f, 0x08, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x00, - 0x30, 0x21, 0x2e, 0xe9, 0x00, 0x98, 0x2e, 0xb4, 0xb1, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x05, 0x50, - 0x98, 0x2e, 0x4d, 0xc3, 0x05, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xf9, 0xb4, 0x98, 0x2e, 0x54, 0xb2, 0x98, - 0x2e, 0x67, 0xb6, 0x98, 0x2e, 0x17, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x2e, 0xef, 0x00, 0x00, 0xb2, - 0x04, 0x2f, 0x98, 0x2e, 0x7a, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xef, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xae, 0x0b, - 0x2f, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x05, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, - 0x10, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x01, 0x2e, 0x7d, 0x00, 0x00, 0x90, 0x90, 0x2e, 0xf1, 0x02, 0x01, 0x2e, 0xd7, - 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, - 0x00, 0xb2, 0x12, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, - 0x2e, 0x81, 0x0d, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, - 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x09, 0x03, 0x01, 0x2e, 0x7c, 0x00, 0x01, - 0x31, 0x01, 0x08, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x81, 0x30, - 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x98, - 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0f, 0x58, 0x23, 0x2f, 0x07, 0x90, 0x09, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, - 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0f, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, - 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, - 0x98, 0x2e, 0xc3, 0xb7, 0x21, 0x2d, 0x61, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x98, 0x2e, 0xc3, 0xb7, 0x00, 0x30, 0x21, - 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xe1, 0x7f, 0x50, 0x30, 0x98, 0x2e, 0xfa, 0x03, 0x0f, 0x52, 0x07, 0x50, 0x50, 0x42, - 0x70, 0x30, 0x0d, 0x54, 0x42, 0x42, 0x7e, 0x82, 0xe2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0xd4, - 0x00, 0x10, 0x30, 0x98, 0x2e, 0xc3, 0xb7, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x01, 0x2e, 0xd4, 0x00, - 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, 0x76, 0x00, 0x0b, 0x54, 0x07, 0x52, 0xe0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xe1, - 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, 0x2f, 0x21, 0x2e, 0xd4, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xaf, 0xb7, 0x50, 0x32, - 0x98, 0x2e, 0xfa, 0x03, 0x05, 0x2d, 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x00, 0x30, 0x21, - 0x2e, 0x7c, 0x00, 0x18, 0x2d, 0x01, 0x2e, 0xd4, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, - 0xd4, 0x00, 0x3f, 0x80, 0x03, 0xa2, 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, - 0x2e, 0xce, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x01, 0x2e, 0x77, 0x00, - 0x00, 0xb2, 0x24, 0x2f, 0x98, 0x2e, 0xf5, 0xcb, 0x03, 0x2e, 0xd5, 0x00, 0x11, 0x54, 0x01, 0x0a, 0xbc, 0x84, 0x83, - 0x86, 0x21, 0x2e, 0xc9, 0x01, 0xe0, 0x40, 0x13, 0x52, 0xc4, 0x40, 0x82, 0x40, 0xa8, 0xb9, 0x52, 0x42, 0x43, 0xbe, - 0x53, 0x42, 0x04, 0x0a, 0x50, 0x42, 0xe1, 0x7f, 0xf0, 0x31, 0x41, 0x40, 0xf2, 0x6f, 0x25, 0xbd, 0x08, 0x08, 0x02, - 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, 0xbc, 0xd1, 0x6f, 0xe2, 0x6f, 0x08, 0x0a, 0x80, 0x42, 0x98, 0x2e, - 0x58, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x21, 0x2e, 0x77, 0x00, 0x21, 0x2e, 0xdd, 0x00, 0x80, 0x2e, 0xf4, - 0x01, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0xec, 0x01, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, - 0xfb, 0x6f, 0x01, 0x30, 0x71, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0xc0, 0x2e, 0x01, 0x42, 0xf0, 0x5f, 0x80, - 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, - 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x06, 0x32, 0x0f, 0x2e, 0x61, 0xf5, 0xfe, 0x09, 0xc0, 0xb3, 0x04, - 0x2f, 0x17, 0x30, 0x2f, 0x2e, 0xef, 0x00, 0x2d, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, - 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, - 0x2e, 0xd4, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, - 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0x01, 0x2e, 0x77, 0xf7, 0x09, 0xbc, 0x0f, 0xb8, 0x00, 0xb2, 0x10, - 0x50, 0xfb, 0x7f, 0x10, 0x30, 0x0b, 0x2f, 0x03, 0x2e, 0x8a, 0x00, 0x96, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x05, 0x2f, - 0x03, 0x2e, 0x68, 0xf7, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x07, 0x2f, 0x03, 0x2e, 0x7e, 0x00, 0x41, 0x90, 0x01, - 0x2f, 0x98, 0x2e, 0xdc, 0x03, 0x03, 0x2c, 0x00, 0x30, 0x21, 0x2e, 0x7e, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, - 0x20, 0x50, 0xe0, 0x7f, 0xfb, 0x7f, 0x00, 0x2e, 0x27, 0x50, 0x98, 0x2e, 0x3b, 0xc8, 0x29, 0x50, 0x98, 0x2e, 0xa7, - 0xc8, 0x01, 0x50, 0x98, 0x2e, 0x55, 0xcc, 0xe1, 0x6f, 0x2b, 0x50, 0x98, 0x2e, 0xe0, 0xc9, 0xfb, 0x6f, 0x00, 0x30, - 0xe0, 0x5f, 0x21, 0x2e, 0x7e, 0x00, 0xb8, 0x2e, 0x73, 0x50, 0x01, 0x30, 0x57, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, - 0x2f, 0xb8, 0x2e, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, 0xf1, 0x90, 0x50, 0xf7, 0x7f, - 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0x35, - 0xb7, 0x00, 0xb2, 0x90, 0x2e, 0x97, 0xb0, 0x03, 0x2e, 0x8f, 0x00, 0x07, 0x2e, 0x91, 0x00, 0x05, 0x2e, 0xb1, 0x00, - 0x3f, 0xba, 0x9f, 0xb8, 0x01, 0x2e, 0xb1, 0x00, 0xa3, 0xbd, 0x4c, 0x0a, 0x05, 0x2e, 0xb1, 0x00, 0x04, 0xbe, 0xbf, - 0xb9, 0xcb, 0x0a, 0x4f, 0xba, 0x22, 0xbd, 0x01, 0x2e, 0xb3, 0x00, 0xdc, 0x0a, 0x2f, 0xb9, 0x03, 0x2e, 0xb8, 0x00, - 0x0a, 0xbe, 0x9a, 0x0a, 0xcf, 0xb9, 0x9b, 0xbc, 0x01, 0x2e, 0x97, 0x00, 0x9f, 0xb8, 0x93, 0x0a, 0x0f, 0xbc, 0x91, - 0x0a, 0x0f, 0xb8, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, 0xb9, 0x01, 0x2e, - 0x19, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x07, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xa2, 0x03, 0x2f, 0x01, - 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x19, 0x50, 0x05, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x05, 0x2e, 0x78, 0x00, - 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x25, 0x2e, 0xdd, 0x00, 0x98, 0x2e, 0x3e, 0xb7, 0x00, - 0xb2, 0x02, 0x30, 0x01, 0x30, 0x04, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x00, 0x2f, 0x21, 0x30, 0x01, 0x2e, - 0xea, 0x00, 0x08, 0x1a, 0x0e, 0x2f, 0x23, 0x2e, 0xea, 0x00, 0x33, 0x30, 0x1b, 0x50, 0x0b, 0x09, 0x01, 0x40, 0x17, - 0x56, 0x46, 0xbe, 0x4b, 0x08, 0x4c, 0x0a, 0x01, 0x42, 0x0a, 0x80, 0x15, 0x52, 0x01, 0x42, 0x00, 0x2e, 0x01, 0x2e, - 0x18, 0x00, 0x00, 0xb2, 0x1f, 0x2f, 0x03, 0x2e, 0xc0, 0xf5, 0xf0, 0x30, 0x48, 0x08, 0x47, 0xaa, 0x74, 0x30, 0x07, - 0x2e, 0x7a, 0x00, 0x61, 0x22, 0x4b, 0x1a, 0x05, 0x2f, 0x07, 0x2e, 0x66, 0xf5, 0xbf, 0xbd, 0xbf, 0xb9, 0xc0, 0x90, - 0x0b, 0x2f, 0x1d, 0x56, 0x2b, 0x30, 0xd2, 0x42, 0xdb, 0x42, 0x01, 0x04, 0xc2, 0x42, 0x04, 0xbd, 0xfe, 0x80, 0x81, - 0x84, 0x23, 0x2e, 0x7a, 0x00, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0xd6, 0x00, 0x81, 0x84, - 0x25, 0x2e, 0xd6, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x8a, 0x00, 0x0b, 0x50, 0x90, 0x08, 0x80, - 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, - 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, - 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0x90, 0x7f, 0xe5, 0x7f, 0xd4, 0x7f, - 0xc3, 0x7f, 0xb1, 0x7f, 0xa2, 0x7f, 0x87, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, - 0x7f, 0x98, 0x2e, 0x35, 0xb7, 0x02, 0x30, 0x63, 0x6f, 0x15, 0x52, 0x50, 0x7f, 0x62, 0x7f, 0x5a, 0x2c, 0x02, 0x32, - 0x1a, 0x09, 0x00, 0xb3, 0x14, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x09, 0x2e, 0x18, 0x00, 0x00, 0x91, 0x0c, 0x2f, 0x43, - 0x7f, 0x98, 0x2e, 0x97, 0xb7, 0x1f, 0x50, 0x02, 0x8a, 0x02, 0x32, 0x04, 0x30, 0x25, 0x2e, 0x64, 0xf5, 0x15, 0x52, - 0x50, 0x6f, 0x43, 0x6f, 0x44, 0x43, 0x25, 0x2e, 0x60, 0xf5, 0xd9, 0x08, 0xc0, 0xb2, 0x36, 0x2f, 0x98, 0x2e, 0x3e, - 0xb7, 0x00, 0xb2, 0x06, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x02, 0x2f, 0x50, 0x6f, 0x00, 0x90, 0x0a, 0x2f, - 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x19, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x79, 0x00, 0x00, 0x30, 0x98, 0x2e, 0xdc, - 0x03, 0x13, 0x2d, 0x01, 0x2e, 0xc3, 0xf5, 0x0c, 0xbc, 0x0f, 0xb8, 0x12, 0x30, 0x10, 0x04, 0x03, 0xb0, 0x26, 0x25, - 0x21, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x10, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x02, 0x30, 0x60, 0x7f, 0x25, - 0x2e, 0x79, 0x00, 0x60, 0x6f, 0x00, 0x90, 0x05, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xea, 0x00, 0x15, 0x50, 0x21, 0x2e, - 0x64, 0xf5, 0x15, 0x52, 0x23, 0x2e, 0x60, 0xf5, 0x02, 0x32, 0x50, 0x6f, 0x00, 0x90, 0x02, 0x2f, 0x03, 0x30, 0x27, - 0x2e, 0x78, 0x00, 0x07, 0x2e, 0x60, 0xf5, 0x1a, 0x09, 0x00, 0x91, 0xa3, 0x2f, 0x19, 0x09, 0x00, 0x91, 0xa0, 0x2f, - 0x90, 0x6f, 0xa2, 0x6f, 0xb1, 0x6f, 0xc3, 0x6f, 0xd4, 0x6f, 0xe5, 0x6f, 0x7b, 0x6f, 0xf6, 0x6f, 0x87, 0x6f, 0x40, - 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x7c, 0x00, - 0x0f, 0x2e, 0x7c, 0x00, 0xbe, 0x09, 0xa2, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, - 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x23, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, - 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, - 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0x89, 0x00, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, - 0x00, 0x90, 0x23, 0x2e, 0xd8, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0xd4, 0x00, 0x44, 0xb2, 0x05, - 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x7c, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, - 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, - 0x2e, 0x49, 0xf1, 0x25, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0x35, 0xb7, 0x00, 0xb2, 0x10, 0x30, - 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xaf, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7c, - 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x7d, 0x00, - 0x23, 0x2e, 0x7c, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, - 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x60, 0x51, 0x0a, 0x25, 0x36, 0x88, - 0xf4, 0x7f, 0xeb, 0x7f, 0x00, 0x32, 0x31, 0x52, 0x32, 0x30, 0x13, 0x30, 0x98, 0x2e, 0x15, 0xcb, 0x0a, 0x25, 0x33, - 0x84, 0xd2, 0x7f, 0x43, 0x30, 0x05, 0x50, 0x2d, 0x52, 0x98, 0x2e, 0x95, 0xc1, 0xd2, 0x6f, 0x27, 0x52, 0x98, 0x2e, - 0xd7, 0xc7, 0x2a, 0x25, 0xb0, 0x86, 0xc0, 0x7f, 0xd3, 0x7f, 0xaf, 0x84, 0x29, 0x50, 0xf1, 0x6f, 0x98, 0x2e, 0x4d, - 0xc8, 0x2a, 0x25, 0xae, 0x8a, 0xaa, 0x88, 0xf2, 0x6e, 0x2b, 0x50, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x7f, 0x98, 0x2e, - 0xb6, 0xc8, 0xe0, 0x6e, 0x00, 0xb2, 0x32, 0x2f, 0x33, 0x54, 0x83, 0x86, 0xf1, 0x6f, 0xc3, 0x7f, 0x04, 0x30, 0x30, - 0x30, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0xe3, 0x30, 0xc5, 0x6f, 0x56, 0x40, 0x45, 0x41, 0x28, 0x08, 0x03, 0x14, - 0x0e, 0xb4, 0x08, 0xbc, 0x82, 0x40, 0x10, 0x0a, 0x2f, 0x54, 0x26, 0x05, 0x91, 0x7f, 0x44, 0x28, 0xa3, 0x7f, 0x98, - 0x2e, 0xd9, 0xc0, 0x08, 0xb9, 0x33, 0x30, 0x53, 0x09, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x6f, 0x83, 0x17, 0x47, 0x40, - 0x6c, 0x15, 0xb2, 0x6f, 0xbe, 0x09, 0x75, 0x0b, 0x90, 0x42, 0x45, 0x42, 0x51, 0x0e, 0x32, 0xbc, 0x02, 0x89, 0xa1, - 0x6f, 0x7e, 0x86, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0x04, 0x30, 0x91, 0x6f, 0xd6, 0x2f, 0xeb, 0x6f, 0xa0, 0x5e, - 0xb8, 0x2e, 0x03, 0x2e, 0x97, 0x00, 0x1b, 0xbc, 0x60, 0x50, 0x9f, 0xbc, 0x0c, 0xb8, 0xf0, 0x7f, 0x40, 0xb2, 0xeb, - 0x7f, 0x2b, 0x2f, 0x03, 0x2e, 0x7f, 0x00, 0x41, 0x40, 0x01, 0x2e, 0xc8, 0x00, 0x01, 0x1a, 0x11, 0x2f, 0x37, 0x58, - 0x23, 0x2e, 0xc8, 0x00, 0x10, 0x41, 0xa0, 0x7f, 0x38, 0x81, 0x01, 0x41, 0xd0, 0x7f, 0xb1, 0x7f, 0x98, 0x2e, 0x64, - 0xcf, 0xd0, 0x6f, 0x07, 0x80, 0xa1, 0x6f, 0x11, 0x42, 0x00, 0x2e, 0xb1, 0x6f, 0x01, 0x42, 0x11, 0x30, 0x01, 0x2e, - 0xfc, 0x00, 0x00, 0xa8, 0x03, 0x30, 0xcb, 0x22, 0x4a, 0x25, 0x01, 0x2e, 0x7f, 0x00, 0x3c, 0x89, 0x35, 0x52, 0x05, - 0x54, 0x98, 0x2e, 0xc4, 0xce, 0xc1, 0x6f, 0xf0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x04, 0x2d, 0x01, 0x30, 0xf0, 0x6f, - 0x98, 0x2e, 0x95, 0xcf, 0xeb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, 0x03, 0x2e, 0xb3, 0x00, 0x02, 0x32, 0xf0, 0x30, 0x03, - 0x31, 0x30, 0x50, 0x8a, 0x08, 0x08, 0x08, 0xcb, 0x08, 0xe0, 0x7f, 0x80, 0xb2, 0xf3, 0x7f, 0xdb, 0x7f, 0x25, 0x2f, - 0x03, 0x2e, 0xca, 0x00, 0x41, 0x90, 0x04, 0x2f, 0x01, 0x30, 0x23, 0x2e, 0xca, 0x00, 0x98, 0x2e, 0x3f, 0x03, 0xc0, - 0xb2, 0x05, 0x2f, 0x03, 0x2e, 0xda, 0x00, 0x00, 0x30, 0x41, 0x04, 0x23, 0x2e, 0xda, 0x00, 0x98, 0x2e, 0x92, 0xb2, - 0x10, 0x25, 0xf0, 0x6f, 0x00, 0xb2, 0x05, 0x2f, 0x01, 0x2e, 0xda, 0x00, 0x02, 0x30, 0x10, 0x04, 0x21, 0x2e, 0xda, - 0x00, 0x40, 0xb2, 0x01, 0x2f, 0x23, 0x2e, 0xc8, 0x01, 0xdb, 0x6f, 0xe0, 0x6f, 0xd0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, - 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0xca, 0x00, 0xdb, 0x6f, 0xd0, 0x5f, 0xb8, - 0x2e, 0xd0, 0x50, 0x0a, 0x25, 0x33, 0x84, 0x55, 0x50, 0xd2, 0x7f, 0xe2, 0x7f, 0x03, 0x8c, 0xc0, 0x7f, 0xbb, 0x7f, - 0x00, 0x30, 0x05, 0x5a, 0x39, 0x54, 0x51, 0x41, 0xa5, 0x7f, 0x96, 0x7f, 0x80, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, - 0x30, 0xf5, 0x7f, 0x20, 0x25, 0x91, 0x6f, 0x3b, 0x58, 0x3d, 0x5c, 0x3b, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xc1, 0x6f, - 0xd5, 0x6f, 0x52, 0x40, 0x50, 0x43, 0xc1, 0x7f, 0xd5, 0x7f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, - 0x2e, 0x74, 0xc0, 0x86, 0x6f, 0x30, 0x28, 0x92, 0x6f, 0x82, 0x8c, 0xa5, 0x6f, 0x6f, 0x52, 0x69, 0x0e, 0x39, 0x54, - 0xdb, 0x2f, 0x19, 0xa0, 0x15, 0x30, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x81, 0x01, 0x0a, 0x2d, 0x01, 0x2e, 0x81, - 0x01, 0x05, 0x28, 0x42, 0x36, 0x21, 0x2e, 0x81, 0x01, 0x02, 0x0e, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, - 0x12, 0x30, 0x01, 0x40, 0x98, 0x2e, 0xfe, 0xc9, 0x51, 0x6f, 0x0b, 0x5c, 0x8e, 0x0e, 0x3b, 0x6f, 0x57, 0x58, 0x02, - 0x30, 0x21, 0x2e, 0x95, 0x01, 0x45, 0x6f, 0x2a, 0x8d, 0xd2, 0x7f, 0xcb, 0x7f, 0x13, 0x2f, 0x02, 0x30, 0x3f, 0x50, - 0xd2, 0x7f, 0xa8, 0x0e, 0x0e, 0x2f, 0xc0, 0x6f, 0x53, 0x54, 0x02, 0x00, 0x51, 0x54, 0x42, 0x0e, 0x10, 0x30, 0x59, - 0x52, 0x02, 0x30, 0x01, 0x2f, 0x00, 0x2e, 0x03, 0x2d, 0x50, 0x42, 0x42, 0x42, 0x12, 0x30, 0xd2, 0x7f, 0x80, 0xb2, - 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x80, 0x01, 0x12, 0x2d, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x80, 0x05, 0x2e, 0x80, - 0x01, 0x11, 0x30, 0x91, 0x28, 0x00, 0x40, 0x25, 0x2e, 0x80, 0x01, 0x10, 0x0e, 0x05, 0x2f, 0x01, 0x2e, 0x7f, 0x01, - 0x01, 0x90, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x00, 0x2e, 0xa0, 0x41, 0x01, 0x90, 0xa6, 0x7f, 0x90, 0x2e, 0xe3, - 0xb4, 0x01, 0x2e, 0x95, 0x01, 0x00, 0xa8, 0x90, 0x2e, 0xe3, 0xb4, 0x5b, 0x54, 0x95, 0x80, 0x82, 0x40, 0x80, 0xb2, - 0x02, 0x40, 0x2d, 0x8c, 0x3f, 0x52, 0x96, 0x7f, 0x90, 0x2e, 0xc2, 0xb3, 0x29, 0x0e, 0x76, 0x2f, 0x01, 0x2e, 0xc9, - 0x00, 0x00, 0x40, 0x81, 0x28, 0x45, 0x52, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x5d, 0x54, 0x80, 0x7f, 0x00, 0x2e, - 0xa1, 0x40, 0x72, 0x7f, 0x82, 0x80, 0x82, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, - 0xc0, 0x62, 0x6f, 0x05, 0x30, 0x87, 0x40, 0xc0, 0x91, 0x04, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x83, 0x01, 0x80, 0xb2, - 0x14, 0x30, 0x00, 0x2f, 0x04, 0x30, 0x05, 0x2e, 0xc9, 0x00, 0x73, 0x6f, 0x81, 0x40, 0xe2, 0x40, 0x69, 0x04, 0x11, - 0x0f, 0xe1, 0x40, 0x16, 0x30, 0xfe, 0x29, 0xcb, 0x40, 0x02, 0x2f, 0x83, 0x6f, 0x83, 0x0f, 0x22, 0x2f, 0x47, 0x56, - 0x13, 0x0f, 0x12, 0x30, 0x77, 0x2f, 0x49, 0x54, 0x42, 0x0e, 0x12, 0x30, 0x73, 0x2f, 0x00, 0x91, 0x0a, 0x2f, 0x01, - 0x2e, 0x8b, 0x01, 0x19, 0xa8, 0x02, 0x30, 0x6c, 0x2f, 0x63, 0x50, 0x00, 0x2e, 0x17, 0x42, 0x05, 0x42, 0x68, 0x2c, - 0x12, 0x30, 0x0b, 0x25, 0x08, 0x0f, 0x50, 0x30, 0x02, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x03, 0x2d, 0x40, 0x30, 0x21, - 0x2e, 0x83, 0x01, 0x2b, 0x2e, 0x85, 0x01, 0x5a, 0x2c, 0x12, 0x30, 0x00, 0x91, 0x2b, 0x25, 0x04, 0x2f, 0x63, 0x50, - 0x02, 0x30, 0x17, 0x42, 0x17, 0x2c, 0x02, 0x42, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x05, - 0x2e, 0xc9, 0x00, 0x81, 0x84, 0x5b, 0x30, 0x82, 0x40, 0x37, 0x2e, 0x83, 0x01, 0x02, 0x0e, 0x07, 0x2f, 0x5f, 0x52, - 0x40, 0x30, 0x62, 0x40, 0x41, 0x40, 0x91, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x05, 0x30, 0x2b, 0x2e, 0x85, - 0x01, 0x12, 0x30, 0x36, 0x2c, 0x16, 0x30, 0x15, 0x25, 0x81, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, - 0x74, 0xc0, 0x19, 0xa2, 0x16, 0x30, 0x15, 0x2f, 0x05, 0x2e, 0x97, 0x01, 0x80, 0x6f, 0x82, 0x0e, 0x05, 0x2f, 0x01, - 0x2e, 0x86, 0x01, 0x06, 0x28, 0x21, 0x2e, 0x86, 0x01, 0x0b, 0x2d, 0x03, 0x2e, 0x87, 0x01, 0x5f, 0x54, 0x4e, 0x28, - 0x91, 0x42, 0x00, 0x2e, 0x82, 0x40, 0x90, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x88, 0x01, 0x02, 0x30, 0x13, 0x2c, 0x05, - 0x30, 0xc0, 0x6f, 0x08, 0x1c, 0xa8, 0x0f, 0x16, 0x30, 0x05, 0x30, 0x5b, 0x50, 0x09, 0x2f, 0x02, 0x80, 0x2d, 0x2e, - 0x82, 0x01, 0x05, 0x42, 0x05, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x3e, 0x80, 0x00, 0x2e, 0x06, 0x42, 0x02, 0x30, 0x90, - 0x6f, 0x3e, 0x88, 0x01, 0x40, 0x04, 0x41, 0x4c, 0x28, 0x01, 0x42, 0x07, 0x80, 0x10, 0x25, 0x24, 0x40, 0x00, 0x40, - 0x00, 0xa8, 0xf5, 0x22, 0x23, 0x29, 0x44, 0x42, 0x7a, 0x82, 0x7e, 0x88, 0x43, 0x40, 0x04, 0x41, 0x00, 0xab, 0xf5, - 0x23, 0xdf, 0x28, 0x43, 0x42, 0xd9, 0xa0, 0x14, 0x2f, 0x00, 0x90, 0x02, 0x2f, 0xd2, 0x6f, 0x81, 0xb2, 0x05, 0x2f, - 0x63, 0x54, 0x06, 0x28, 0x90, 0x42, 0x85, 0x42, 0x09, 0x2c, 0x02, 0x30, 0x5b, 0x50, 0x03, 0x80, 0x29, 0x2e, 0x7e, - 0x01, 0x2b, 0x2e, 0x82, 0x01, 0x05, 0x42, 0x12, 0x30, 0x2b, 0x2e, 0x83, 0x01, 0x45, 0x82, 0x00, 0x2e, 0x40, 0x40, - 0x7a, 0x82, 0x02, 0xa0, 0x08, 0x2f, 0x63, 0x50, 0x3b, 0x30, 0x15, 0x42, 0x05, 0x42, 0x37, 0x80, 0x37, 0x2e, 0x7e, - 0x01, 0x05, 0x42, 0x12, 0x30, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x8c, 0x40, 0x40, 0x84, 0x41, 0x7a, 0x8c, 0x04, 0x0f, - 0x03, 0x2f, 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa4, 0x04, 0x2f, 0x2b, 0x2e, 0x82, 0x01, 0x98, 0x2e, 0xf3, 0x03, 0x12, - 0x30, 0x81, 0x90, 0x61, 0x52, 0x08, 0x2f, 0x65, 0x42, 0x65, 0x42, 0x43, 0x80, 0x39, 0x84, 0x82, 0x88, 0x05, 0x42, - 0x45, 0x42, 0x85, 0x42, 0x05, 0x43, 0x00, 0x2e, 0x80, 0x41, 0x00, 0x90, 0x90, 0x2e, 0xe1, 0xb4, 0x65, 0x54, 0xc1, - 0x6f, 0x80, 0x40, 0x00, 0xb2, 0x43, 0x58, 0x69, 0x50, 0x44, 0x2f, 0x55, 0x5c, 0xb7, 0x87, 0x8c, 0x0f, 0x0d, 0x2e, - 0x96, 0x01, 0xc4, 0x40, 0x36, 0x2f, 0x41, 0x56, 0x8b, 0x0e, 0x2a, 0x2f, 0x0b, 0x52, 0xa1, 0x0e, 0x0a, 0x2f, 0x05, - 0x2e, 0x8f, 0x01, 0x14, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x4b, 0x54, 0x02, 0x0f, 0x69, 0x50, 0x05, 0x30, 0x65, 0x54, - 0x15, 0x2f, 0x03, 0x2e, 0x8e, 0x01, 0x4d, 0x5c, 0x8e, 0x0f, 0x3a, 0x2f, 0x05, 0x2e, 0x8f, 0x01, 0x98, 0x2e, 0xfe, - 0xc9, 0x4f, 0x54, 0x82, 0x0f, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x30, 0x2f, 0x6d, 0x52, 0x15, 0x30, 0x42, 0x8c, - 0x45, 0x42, 0x04, 0x30, 0x2b, 0x2c, 0x84, 0x43, 0x6b, 0x52, 0x42, 0x8c, 0x00, 0x2e, 0x85, 0x43, 0x15, 0x30, 0x24, - 0x2c, 0x45, 0x42, 0x8e, 0x0f, 0x20, 0x2f, 0x0d, 0x2e, 0x8e, 0x01, 0xb1, 0x0e, 0x1c, 0x2f, 0x23, 0x2e, 0x8e, 0x01, - 0x1a, 0x2d, 0x0e, 0x0e, 0x17, 0x2f, 0xa1, 0x0f, 0x15, 0x2f, 0x23, 0x2e, 0x8d, 0x01, 0x13, 0x2d, 0x98, 0x2e, 0x74, - 0xc0, 0x43, 0x54, 0xc2, 0x0e, 0x0a, 0x2f, 0x65, 0x50, 0x04, 0x80, 0x0b, 0x30, 0x06, 0x82, 0x0b, 0x42, 0x79, 0x80, - 0x41, 0x40, 0x12, 0x30, 0x25, 0x2e, 0x8c, 0x01, 0x01, 0x42, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x84, 0x82, 0x43, - 0x84, 0xbe, 0x8c, 0x84, 0x40, 0x86, 0x41, 0x26, 0x29, 0x94, 0x42, 0xbe, 0x8e, 0xd5, 0x7f, 0x19, 0xa1, 0x43, 0x40, - 0x0b, 0x2e, 0x8c, 0x01, 0x84, 0x40, 0xc7, 0x41, 0x5d, 0x29, 0x27, 0x29, 0x45, 0x42, 0x84, 0x42, 0xc2, 0x7f, 0x01, - 0x2f, 0xc0, 0xb3, 0x1d, 0x2f, 0x05, 0x2e, 0x94, 0x01, 0x99, 0xa0, 0x01, 0x2f, 0x80, 0xb3, 0x13, 0x2f, 0x80, 0xb3, - 0x18, 0x2f, 0xc0, 0xb3, 0x16, 0x2f, 0x12, 0x40, 0x01, 0x40, 0x92, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x92, 0x6f, 0x10, - 0x0f, 0x20, 0x30, 0x03, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x0a, 0x2d, 0x21, 0x2e, 0x7e, 0x01, 0x07, 0x2d, - 0x20, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x03, 0x2d, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0xc2, 0x6f, 0x01, 0x2e, 0xc9, - 0x00, 0xbc, 0x84, 0x02, 0x80, 0x82, 0x40, 0x00, 0x40, 0x90, 0x0e, 0xd5, 0x6f, 0x02, 0x2f, 0x15, 0x30, 0x98, 0x2e, - 0xf3, 0x03, 0x41, 0x91, 0x05, 0x30, 0x07, 0x2f, 0x67, 0x50, 0x3d, 0x80, 0x2b, 0x2e, 0x8f, 0x01, 0x05, 0x42, 0x04, - 0x80, 0x00, 0x2e, 0x05, 0x42, 0x02, 0x2c, 0x00, 0x30, 0x00, 0x30, 0xa2, 0x6f, 0x98, 0x8a, 0x86, 0x40, 0x80, 0xa7, - 0x05, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0xc0, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x06, 0x25, 0x1a, 0x25, 0xe2, 0x6f, 0x76, - 0x82, 0x96, 0x40, 0x56, 0x43, 0x51, 0x0e, 0xfb, 0x2f, 0xbb, 0x6f, 0x30, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xb8, 0x00, - 0x01, 0x31, 0x41, 0x08, 0x40, 0xb2, 0x20, 0x50, 0xf2, 0x30, 0x02, 0x08, 0xfb, 0x7f, 0x01, 0x30, 0x10, 0x2f, 0x05, - 0x2e, 0xcc, 0x00, 0x81, 0x90, 0xe0, 0x7f, 0x03, 0x2f, 0x23, 0x2e, 0xcc, 0x00, 0x98, 0x2e, 0x55, 0xb6, 0x98, 0x2e, - 0x1d, 0xb5, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, 0x98, 0x2e, 0x95, 0xcf, 0x10, - 0x30, 0x21, 0x2e, 0xcc, 0x00, 0xfb, 0x6f, 0xe0, 0x5f, 0xb8, 0x2e, 0x00, 0x51, 0x05, 0x58, 0xeb, 0x7f, 0x2a, 0x25, - 0x89, 0x52, 0x6f, 0x5a, 0x89, 0x50, 0x13, 0x41, 0x06, 0x40, 0xb3, 0x01, 0x16, 0x42, 0xcb, 0x16, 0x06, 0x40, 0xf3, - 0x02, 0x13, 0x42, 0x65, 0x0e, 0xf5, 0x2f, 0x05, 0x40, 0x14, 0x30, 0x2c, 0x29, 0x04, 0x42, 0x08, 0xa1, 0x00, 0x30, - 0x90, 0x2e, 0x52, 0xb6, 0xb3, 0x88, 0xb0, 0x8a, 0xb6, 0x84, 0xa4, 0x7f, 0xc4, 0x7f, 0xb5, 0x7f, 0xd5, 0x7f, 0x92, - 0x7f, 0x73, 0x30, 0x04, 0x30, 0x55, 0x40, 0x42, 0x40, 0x8a, 0x17, 0xf3, 0x08, 0x6b, 0x01, 0x90, 0x02, 0x53, 0xb8, - 0x4b, 0x82, 0xad, 0xbe, 0x71, 0x7f, 0x45, 0x0a, 0x09, 0x54, 0x84, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0xa3, 0x6f, 0x7b, - 0x54, 0xd0, 0x42, 0xa3, 0x7f, 0xf2, 0x7f, 0x60, 0x7f, 0x20, 0x25, 0x71, 0x6f, 0x75, 0x5a, 0x77, 0x58, 0x79, 0x5c, - 0x75, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0x62, 0x6f, 0x50, 0x42, 0xb1, 0x7f, 0xb3, 0x30, 0x10, 0x25, 0x98, - 0x2e, 0x0f, 0xca, 0x84, 0x6f, 0x20, 0x29, 0x71, 0x6f, 0x92, 0x6f, 0xa5, 0x6f, 0x76, 0x82, 0x6a, 0x0e, 0x73, 0x30, - 0x00, 0x30, 0xd0, 0x2f, 0xd2, 0x6f, 0xd1, 0x7f, 0xb4, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x02, - 0x0a, 0xc2, 0x6f, 0xc0, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x42, 0x0a, 0xc0, 0x6f, 0x08, 0x17, - 0x41, 0x18, 0x89, 0x16, 0xe1, 0x18, 0xd0, 0x18, 0xa1, 0x7f, 0x27, 0x25, 0x16, 0x25, 0x98, 0x2e, 0x79, 0xc0, 0x8b, - 0x54, 0x90, 0x7f, 0xb3, 0x30, 0x82, 0x40, 0x80, 0x90, 0x0d, 0x2f, 0x7d, 0x52, 0x92, 0x6f, 0x98, 0x2e, 0x0f, 0xca, - 0xb2, 0x6f, 0x90, 0x0e, 0x06, 0x2f, 0x8b, 0x50, 0x14, 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x14, 0x42, 0x12, 0x42, 0x01, - 0x42, 0x00, 0x2e, 0x31, 0x6f, 0x98, 0x2e, 0x74, 0xc0, 0x41, 0x6f, 0x80, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x82, 0x6f, - 0x10, 0x04, 0x43, 0x52, 0x01, 0x0f, 0x05, 0x2e, 0xcb, 0x00, 0x00, 0x30, 0x04, 0x30, 0x21, 0x2f, 0x51, 0x6f, 0x43, - 0x58, 0x8c, 0x0e, 0x04, 0x30, 0x1c, 0x2f, 0x85, 0x88, 0x41, 0x6f, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, 0x16, 0x2f, - 0x84, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x0f, 0x2f, 0x82, 0x88, 0x31, 0x6f, 0x04, - 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x08, 0x2f, 0x83, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, - 0x02, 0x2f, 0x21, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x00, 0x91, 0x14, 0x2f, 0x03, 0x2e, 0xa1, 0x01, 0x41, 0x90, 0x0e, - 0x2f, 0x03, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x4c, 0x28, 0x23, 0x2e, 0xad, 0x01, 0x46, 0xa0, 0x06, 0x2f, 0x81, 0x84, - 0x8d, 0x52, 0x48, 0x82, 0x82, 0x40, 0x21, 0x2e, 0xa1, 0x01, 0x42, 0x42, 0x5c, 0x2c, 0x02, 0x30, 0x05, 0x2e, 0xaa, - 0x01, 0x80, 0xb2, 0x02, 0x30, 0x55, 0x2f, 0x03, 0x2e, 0xa9, 0x01, 0x92, 0x6f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, - 0xb2, 0x6f, 0x90, 0x0f, 0x00, 0x30, 0x02, 0x30, 0x4a, 0x2f, 0xa2, 0x6f, 0x87, 0x52, 0x91, 0x00, 0x85, 0x52, 0x51, - 0x0e, 0x02, 0x2f, 0x00, 0x2e, 0x43, 0x2c, 0x02, 0x30, 0xc2, 0x6f, 0x7f, 0x52, 0x91, 0x0e, 0x02, 0x30, 0x3c, 0x2f, - 0x51, 0x6f, 0x81, 0x54, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x32, - 0x6f, 0xc0, 0x7f, 0xb3, 0x30, 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x42, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, 0x12, 0x25, - 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x28, 0x83, 0x52, 0x98, 0x2e, 0xfe, 0xc9, 0xc2, 0x6f, 0x90, 0x0f, 0x00, - 0x30, 0x02, 0x30, 0x1d, 0x2f, 0x05, 0x2e, 0xa1, 0x01, 0x80, 0xb2, 0x12, 0x30, 0x0f, 0x2f, 0x42, 0x6f, 0x03, 0x2e, - 0xab, 0x01, 0x91, 0x0e, 0x02, 0x30, 0x12, 0x2f, 0x52, 0x6f, 0x03, 0x2e, 0xac, 0x01, 0x91, 0x0f, 0x02, 0x30, 0x0c, - 0x2f, 0x21, 0x2e, 0xaa, 0x01, 0x0a, 0x2c, 0x12, 0x30, 0x03, 0x2e, 0xcb, 0x00, 0x8d, 0x58, 0x08, 0x89, 0x41, 0x40, - 0x11, 0x43, 0x00, 0x43, 0x25, 0x2e, 0xa1, 0x01, 0xd4, 0x6f, 0x8f, 0x52, 0x00, 0x43, 0x3a, 0x89, 0x00, 0x2e, 0x10, - 0x43, 0x10, 0x43, 0x61, 0x0e, 0xfb, 0x2f, 0x03, 0x2e, 0xa0, 0x01, 0x11, 0x1a, 0x02, 0x2f, 0x02, 0x25, 0x21, 0x2e, - 0xa0, 0x01, 0xeb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x91, 0x52, 0x10, 0x30, 0x02, 0x30, 0x95, 0x56, 0x52, 0x42, 0x4b, - 0x0e, 0xfc, 0x2f, 0x8d, 0x54, 0x88, 0x82, 0x93, 0x56, 0x80, 0x42, 0x53, 0x42, 0x40, 0x42, 0x42, 0x86, 0x83, 0x54, - 0xc0, 0x2e, 0xc2, 0x42, 0x00, 0x2e, 0xa3, 0x52, 0x00, 0x51, 0x52, 0x40, 0x47, 0x40, 0x1a, 0x25, 0x01, 0x2e, 0x97, - 0x00, 0x8f, 0xbe, 0x72, 0x86, 0xfb, 0x7f, 0x0b, 0x30, 0x7c, 0xbf, 0xa5, 0x50, 0x10, 0x08, 0xdf, 0xba, 0x70, 0x88, - 0xf8, 0xbf, 0xcb, 0x42, 0xd3, 0x7f, 0x6c, 0xbb, 0xfc, 0xbb, 0xc5, 0x0a, 0x90, 0x7f, 0x1b, 0x7f, 0x0b, 0x43, 0xc0, - 0xb2, 0xe5, 0x7f, 0xb7, 0x7f, 0xa6, 0x7f, 0xc4, 0x7f, 0x90, 0x2e, 0x1c, 0xb7, 0x07, 0x2e, 0xd2, 0x00, 0xc0, 0xb2, - 0x0b, 0x2f, 0x97, 0x52, 0x01, 0x2e, 0xcd, 0x00, 0x82, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x0b, 0x30, 0x37, 0x2e, 0xd2, - 0x00, 0x82, 0x6f, 0x90, 0x6f, 0x1a, 0x25, 0x00, 0xb2, 0x8b, 0x7f, 0x14, 0x2f, 0xa6, 0xbd, 0x25, 0xbd, 0xb6, 0xb9, - 0x2f, 0xb9, 0x80, 0xb2, 0xd4, 0xb0, 0x0c, 0x2f, 0x99, 0x54, 0x9b, 0x56, 0x0b, 0x30, 0x0b, 0x2e, 0xb1, 0x00, 0xa1, - 0x58, 0x9b, 0x42, 0xdb, 0x42, 0x6c, 0x09, 0x2b, 0x2e, 0xb1, 0x00, 0x8b, 0x42, 0xcb, 0x42, 0x86, 0x7f, 0x73, 0x84, - 0xa7, 0x56, 0xc3, 0x08, 0x39, 0x52, 0x05, 0x50, 0x72, 0x7f, 0x63, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0xe1, 0x6f, 0x62, - 0x6f, 0xd1, 0x0a, 0x01, 0x2e, 0xcd, 0x00, 0xd5, 0x6f, 0xc4, 0x6f, 0x72, 0x6f, 0x97, 0x52, 0x9d, 0x5c, 0x98, 0x2e, - 0x06, 0xcd, 0x23, 0x6f, 0x90, 0x6f, 0x99, 0x52, 0xc0, 0xb2, 0x04, 0xbd, 0x54, 0x40, 0xaf, 0xb9, 0x45, 0x40, 0xe1, - 0x7f, 0x02, 0x30, 0x06, 0x2f, 0xc0, 0xb2, 0x02, 0x30, 0x03, 0x2f, 0x9b, 0x5c, 0x12, 0x30, 0x94, 0x43, 0x85, 0x43, - 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x06, 0x6f, 0x26, 0x01, 0x16, 0x6f, 0x6e, 0x03, 0x45, 0x42, 0xc0, - 0x90, 0x29, 0x2e, 0xce, 0x00, 0x9b, 0x52, 0x14, 0x2f, 0x9b, 0x5c, 0x00, 0x2e, 0x93, 0x41, 0x86, 0x41, 0xe3, 0x04, - 0xae, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x86, 0x6f, 0x73, 0x0f, 0x07, 0x2f, 0x83, 0x6f, 0xc0, - 0xb2, 0x04, 0x2f, 0x54, 0x42, 0x45, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, - 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0xb2, 0x0a, 0x2f, 0x01, 0x2e, 0xfc, 0x00, 0x05, 0x2e, 0xc7, 0x01, 0x10, - 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xc7, 0x01, 0x03, 0x2d, 0x02, 0x2c, 0x01, 0x30, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, - 0x95, 0xcf, 0xd1, 0x6f, 0xa0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0x9f, 0x52, 0x01, 0x2e, 0xce, 0x00, 0x82, - 0x40, 0x50, 0x42, 0x0c, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, 0x2e, 0xd2, 0x00, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, - 0x95, 0xcf, 0xa0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x83, - 0x86, 0x01, 0x30, 0x00, 0x30, 0x94, 0x40, 0x24, 0x18, 0x06, 0x00, 0x53, 0x0e, 0x4f, 0x02, 0xf9, 0x2f, 0xb8, 0x2e, - 0xa9, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0xab, - 0x52, 0x53, 0x3c, 0x52, 0x40, 0x40, 0x40, 0x4b, 0x00, 0x82, 0x16, 0x26, 0xb9, 0x01, 0xb8, 0x41, 0x40, 0x10, 0x08, - 0x97, 0xb8, 0x01, 0x08, 0xc0, 0x2e, 0x11, 0x30, 0x01, 0x08, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, - 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x9f, 0x50, 0x10, 0x50, 0xad, 0x52, - 0x05, 0x2e, 0xd3, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xa5, - 0xb7, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0xd9, 0x00, 0x00, 0xb2, 0xfb, 0x6f, 0x0b, 0x2f, 0x01, 0x2e, 0x69, 0xf7, - 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd9, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0x80, 0x2e, 0x7a, - 0xb7, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xc0, 0xf8, 0x03, 0x2e, 0xfc, 0xf5, 0x15, 0x54, 0xaf, 0x56, 0x82, 0x08, - 0x0b, 0x2e, 0x69, 0xf7, 0xcb, 0x0a, 0xb1, 0x58, 0x80, 0x90, 0xdd, 0xbe, 0x4c, 0x08, 0x5f, 0xb9, 0x59, 0x22, 0x80, - 0x90, 0x07, 0x2f, 0x03, 0x34, 0xc3, 0x08, 0xf2, 0x3a, 0x0a, 0x08, 0x02, 0x35, 0xc0, 0x90, 0x4a, 0x0a, 0x48, 0x22, - 0xc0, 0x2e, 0x23, 0x2e, 0xfc, 0xf5, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x10, - 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x21, 0x2e, 0xcc, 0x00, 0x21, 0x2e, 0xca, 0x00, 0xb8, 0x2e, 0x03, 0x2e, 0xd3, 0x00, - 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0xd3, 0x00, 0x03, 0xbc, 0x21, - 0x2e, 0xd5, 0x00, 0x03, 0x2e, 0xd5, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x05, 0x2f, - 0x05, 0x2e, 0xd8, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0xd9, 0x00, 0x11, - 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, - 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x7c, - 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xc3, 0xb7, 0x40, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, - 0x80, 0x2e, 0xaf, 0xb7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, - 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, - 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0xb3, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, - 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0xb5, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, - 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0xb7, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, - 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, - 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x2d, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, - 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0xb7, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0xb7, 0x52, 0x2d, - 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0xbf, 0x56, - 0xb9, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0xbf, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, - 0x30, 0xc1, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, - 0xd8, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, - 0x2e, 0x77, 0xf7, 0xbd, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0xbb, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, - 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, - 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, - 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, - 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0xc3, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, - 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, - 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, - 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0xcb, 0x58, 0x32, 0x87, 0xc4, - 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0xc5, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, - 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0xc7, 0x52, 0xc9, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, - 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, - 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, - 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, - 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, - 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, - 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, - 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, - 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, - 0x2e, 0x79, 0xca, 0x55, 0x6f, 0xb7, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, - 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, - 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, - 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0xd1, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, - 0x2f, 0x09, 0x2e, 0xd8, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0xcb, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, - 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, - 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, - 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, - 0x2d, 0xcd, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, - 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0xcf, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x15, - 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, - 0x01, 0x2e, 0xd4, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, - 0x90, 0x56, 0x2f, 0xd7, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, - 0xd9, 0x54, 0xd7, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, - 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x15, 0x54, 0x4a, 0x0e, - 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x85, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, - 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xce, 0xb7, - 0xdd, 0x52, 0xd3, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0xdb, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, - 0x30, 0x27, 0x2e, 0xd7, 0x00, 0x37, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0xd6, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, - 0x30, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0x7b, - 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0xd5, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0xd4, 0x00, - 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, - 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xb5, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, - 0x4b, 0x08, 0xdf, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0xcb, - 0x52, 0xe1, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, - 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0xd1, 0x50, 0x03, - 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, - 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, - 0x2e, 0xd4, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd6, 0x00, 0x21, 0x2e, - 0xd7, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0xd7, 0x00, 0x03, 0x2e, 0xd6, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, - 0x0e, 0xb8, 0x2e, 0xe3, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, - 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0xe3, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0xe5, - 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, - 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0xdd, 0x52, 0x00, - 0x30, 0x40, 0x42, 0x7c, 0x86, 0xb9, 0x52, 0x09, 0x2e, 0x70, 0x0f, 0xbf, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, - 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0xd7, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, - 0x82, 0x05, 0x2e, 0x7d, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x89, 0x00, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, - 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x71, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, - 0xf7, 0xbd, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0xe7, 0x54, 0x50, 0x42, 0x4a, 0x0e, - 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0xe5, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, - 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, - 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, - 0x2f, 0x03, 0x2e, 0x86, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x86, 0x0f, 0x02, 0x2c, 0x00, 0x30, - 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, - 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x47, 0x58, 0xc2, 0x6f, 0x94, 0x09, - 0xeb, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xe9, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, - 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, - 0xf5, 0x50, 0x31, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, - 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0xd3, 0x00, 0x23, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, - 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, - 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, - 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, 0x2e, 0xa5, 0xb7, 0x00, 0x2e, 0x00, 0x2e, 0xd0, - 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, - 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xed, 0x50, 0x98, - 0x2e, 0x44, 0xcb, 0xef, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xf1, 0x50, 0x98, 0x2e, 0x53, 0xc7, 0x35, 0x50, 0x98, 0x2e, - 0x64, 0xcf, 0x10, 0x30, 0x98, 0x2e, 0xdc, 0x03, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0xab, 0x33, 0x0b, - 0x42, 0x37, 0x80, 0x01, 0x30, 0x01, 0x42, 0xf3, 0x37, 0xf7, 0x52, 0xfb, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, - 0x8b, 0x31, 0x09, 0x2e, 0x5e, 0xf7, 0xf9, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, - 0x84, 0x0b, 0x40, 0x33, 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x58, 0xb7, 0xd1, 0x6f, - 0x80, 0x30, 0x40, 0x42, 0x03, 0x30, 0xe0, 0x6f, 0xf3, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, - 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0xc1, 0x4a, 0x00, 0x00, - 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, - 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1 + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, + 0x91, 0x03, 0x80, 0x2e, 0xbc, 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, + 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0x21, 0x2e, + 0x6a, 0xf5, 0x80, 0x2e, 0x3b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x01, 0x00, + 0x22, 0x00, 0x75, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xb3, 0x43, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xe0, 0x5f, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x05, 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0xfa, 0x00, + 0x96, 0x00, 0x4b, 0x09, 0x11, 0x00, 0x11, 0x00, 0x02, 0x00, 0x2d, 0x01, 0xd4, 0x7b, + 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, + 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, + 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, + 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x05, 0x00, 0xee, 0x06, 0x04, 0x00, + 0xc8, 0x00, 0x00, 0x00, 0x04, 0x00, 0xa8, 0x05, 0xee, 0x06, 0x00, 0x04, 0xbc, 0x02, + 0xb3, 0x00, 0x85, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb4, 0x00, 0x01, 0x00, + 0xb9, 0x00, 0x01, 0x00, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x80, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xde, 0x00, 0xeb, 0x00, 0xda, 0x00, + 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0xc9, 0x01, 0x1e, 0xf2, + 0x80, 0x00, 0x3f, 0xff, 0x19, 0xf4, 0x58, 0xf5, 0x66, 0xf5, 0x64, 0xf5, 0xc0, 0xf1, + 0xf0, 0x00, 0xe0, 0x00, 0xcd, 0x01, 0xd3, 0x01, 0xdb, 0x01, 0xff, 0x7f, 0xff, 0x01, + 0xe4, 0x00, 0x74, 0xf7, 0xf3, 0x00, 0xfa, 0x00, 0xff, 0x3f, 0xca, 0x03, 0x6c, 0x38, + 0x56, 0xfe, 0x44, 0xfd, 0xbc, 0x02, 0xf9, 0x06, 0x00, 0xfc, 0x12, 0x02, 0xae, 0x01, + 0x58, 0xfa, 0x9a, 0xfd, 0x77, 0x05, 0xbb, 0x02, 0x96, 0x01, 0x95, 0x01, 0x7f, 0x01, + 0x82, 0x01, 0x89, 0x01, 0x87, 0x01, 0x88, 0x01, 0x8a, 0x01, 0x8c, 0x01, 0x8f, 0x01, + 0x8d, 0x01, 0x92, 0x01, 0x91, 0x01, 0xdd, 0x00, 0x9f, 0x01, 0x7e, 0x01, 0xdb, 0x00, + 0xb6, 0x01, 0x70, 0x69, 0x26, 0xd3, 0x9c, 0x07, 0x1f, 0x05, 0x9d, 0x00, 0x00, 0x08, + 0xbc, 0x05, 0x37, 0xfa, 0xa2, 0x01, 0xaa, 0x01, 0xa1, 0x01, 0xa8, 0x01, 0xa0, 0x01, + 0xa8, 0x05, 0xb4, 0x01, 0xb4, 0x01, 0xce, 0x00, 0xd0, 0x00, 0xfc, 0x00, 0xc5, 0x01, + 0xff, 0xfb, 0xb1, 0x00, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xfc, 0xf5, 0xcd, 0x01, + 0xa0, 0x00, 0x5f, 0xff, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0x6d, 0x0f, 0xeb, 0x00, + 0x7f, 0xff, 0xc2, 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x67, 0x0f, 0x5b, 0x0f, 0x61, 0x0f, + 0x80, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x83, 0x0f, 0x86, 0x00, 0x72, 0x0f, 0x85, 0x0f, + 0xc6, 0xf1, 0x7f, 0x0f, 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x87, 0x0f, + 0x8a, 0x0f, 0xff, 0x03, 0xf0, 0x3f, 0x8b, 0x00, 0x8e, 0x00, 0x90, 0x00, 0xb9, 0x00, + 0x2d, 0xf5, 0xca, 0xf5, 0xcb, 0x01, 0x20, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x30, 0x50, 0x98, 0x2e, 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, + 0xfa, 0x03, 0x00, 0x30, 0xf0, 0x7f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, 0x2e, + 0x01, 0x80, 0x08, 0xa2, 0xfb, 0x2f, 0x98, 0x2e, 0xba, 0x03, 0x21, 0x2e, 0x19, 0x00, + 0x01, 0x2e, 0xee, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, + 0x03, 0x2f, 0x01, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x07, 0xcc, 0x01, 0x2e, 0xdd, 0x00, + 0x00, 0xb2, 0x27, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x05, 0x52, 0x98, 0x2e, 0xc7, 0xc1, + 0x03, 0x2e, 0xe9, 0x00, 0x40, 0xb2, 0xf0, 0x7f, 0x08, 0x2f, 0x01, 0x2e, 0x19, 0x00, + 0x00, 0xb2, 0x04, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xe9, 0x00, 0x98, 0x2e, 0xb4, 0xb1, + 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x05, 0x50, 0x98, 0x2e, 0x4d, 0xc3, + 0x05, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xf9, 0xb4, 0x98, 0x2e, 0x54, 0xb2, + 0x98, 0x2e, 0x67, 0xb6, 0x98, 0x2e, 0x17, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, + 0x01, 0x2e, 0xef, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x7a, 0xb7, 0x00, 0x30, + 0x21, 0x2e, 0xef, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xae, 0x0b, 0x2f, 0x01, 0x2e, + 0xdd, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x05, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, + 0x02, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x01, 0x2e, 0x7d, 0x00, 0x00, 0x90, + 0x90, 0x2e, 0xf1, 0x02, 0x01, 0x2e, 0xd7, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, + 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, + 0x12, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, + 0x09, 0x2d, 0x98, 0x2e, 0x81, 0x0d, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0x90, 0x02, 0x2f, + 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, + 0x7c, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x09, 0x03, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x31, + 0x01, 0x08, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, 0x21, 0x2e, + 0x77, 0x00, 0x81, 0x30, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x61, 0x2f, + 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x98, 0xbc, 0x98, 0xb8, 0x05, 0xb2, + 0x0f, 0x58, 0x23, 0x2f, 0x07, 0x90, 0x09, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, + 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0f, 0x56, + 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, 0x30, 0x05, 0x2e, + 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, 0x98, 0x2e, 0xc3, 0xb7, + 0x21, 0x2d, 0x61, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x98, 0x2e, 0xc3, 0xb7, 0x00, 0x30, + 0x21, 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xe1, 0x7f, 0x50, 0x30, 0x98, 0x2e, 0xfa, 0x03, + 0x0f, 0x52, 0x07, 0x50, 0x50, 0x42, 0x70, 0x30, 0x0d, 0x54, 0x42, 0x42, 0x7e, 0x82, + 0xe2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0xd4, 0x00, 0x10, 0x30, + 0x98, 0x2e, 0xc3, 0xb7, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x01, 0x2e, + 0xd4, 0x00, 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, 0x76, 0x00, 0x0b, 0x54, 0x07, 0x52, + 0xe0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xe1, 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, 0x2f, + 0x21, 0x2e, 0xd4, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xaf, 0xb7, 0x50, 0x32, 0x98, 0x2e, + 0xfa, 0x03, 0x05, 0x2d, 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0xd4, 0x00, + 0x00, 0x30, 0x21, 0x2e, 0x7c, 0x00, 0x18, 0x2d, 0x01, 0x2e, 0xd4, 0x00, 0x03, 0xaa, + 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, 0xd4, 0x00, 0x3f, 0x80, 0x03, 0xa2, + 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, 0x2e, + 0xce, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, + 0x01, 0x2e, 0x77, 0x00, 0x00, 0xb2, 0x24, 0x2f, 0x98, 0x2e, 0xf5, 0xcb, 0x03, 0x2e, + 0xd5, 0x00, 0x11, 0x54, 0x01, 0x0a, 0xbc, 0x84, 0x83, 0x86, 0x21, 0x2e, 0xc9, 0x01, + 0xe0, 0x40, 0x13, 0x52, 0xc4, 0x40, 0x82, 0x40, 0xa8, 0xb9, 0x52, 0x42, 0x43, 0xbe, + 0x53, 0x42, 0x04, 0x0a, 0x50, 0x42, 0xe1, 0x7f, 0xf0, 0x31, 0x41, 0x40, 0xf2, 0x6f, + 0x25, 0xbd, 0x08, 0x08, 0x02, 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, 0xbc, + 0xd1, 0x6f, 0xe2, 0x6f, 0x08, 0x0a, 0x80, 0x42, 0x98, 0x2e, 0x58, 0xb7, 0x00, 0x30, + 0x21, 0x2e, 0xee, 0x00, 0x21, 0x2e, 0x77, 0x00, 0x21, 0x2e, 0xdd, 0x00, 0x80, 0x2e, + 0xf4, 0x01, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0xec, 0x01, 0x10, 0x50, 0xfb, 0x7f, + 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, 0xfb, 0x6f, 0x01, 0x30, 0x71, 0x54, 0x11, 0x42, + 0x42, 0x0e, 0xfc, 0x2f, 0xc0, 0x2e, 0x01, 0x42, 0xf0, 0x5f, 0x80, 0x2e, 0x00, 0xc1, + 0xfd, 0x2d, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x9a, 0x01, 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x06, 0x32, + 0x0f, 0x2e, 0x61, 0xf5, 0xfe, 0x09, 0xc0, 0xb3, 0x04, 0x2f, 0x17, 0x30, 0x2f, 0x2e, + 0xef, 0x00, 0x2d, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, + 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, + 0x80, 0xb3, 0x06, 0x2f, 0x0d, 0x2e, 0xd4, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, + 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, + 0xe0, 0x5f, 0xc8, 0x2e, 0x01, 0x2e, 0x77, 0xf7, 0x09, 0xbc, 0x0f, 0xb8, 0x00, 0xb2, + 0x10, 0x50, 0xfb, 0x7f, 0x10, 0x30, 0x0b, 0x2f, 0x03, 0x2e, 0x8a, 0x00, 0x96, 0xbc, + 0x9f, 0xb8, 0x40, 0xb2, 0x05, 0x2f, 0x03, 0x2e, 0x68, 0xf7, 0x9e, 0xbc, 0x9f, 0xb8, + 0x40, 0xb2, 0x07, 0x2f, 0x03, 0x2e, 0x7e, 0x00, 0x41, 0x90, 0x01, 0x2f, 0x98, 0x2e, + 0xdc, 0x03, 0x03, 0x2c, 0x00, 0x30, 0x21, 0x2e, 0x7e, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, + 0xb8, 0x2e, 0x20, 0x50, 0xe0, 0x7f, 0xfb, 0x7f, 0x00, 0x2e, 0x27, 0x50, 0x98, 0x2e, + 0x3b, 0xc8, 0x29, 0x50, 0x98, 0x2e, 0xa7, 0xc8, 0x01, 0x50, 0x98, 0x2e, 0x55, 0xcc, + 0xe1, 0x6f, 0x2b, 0x50, 0x98, 0x2e, 0xe0, 0xc9, 0xfb, 0x6f, 0x00, 0x30, 0xe0, 0x5f, + 0x21, 0x2e, 0x7e, 0x00, 0xb8, 0x2e, 0x73, 0x50, 0x01, 0x30, 0x57, 0x54, 0x11, 0x42, + 0x42, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, + 0x21, 0x2e, 0x4a, 0xf1, 0x90, 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, + 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0x35, 0xb7, + 0x00, 0xb2, 0x90, 0x2e, 0x97, 0xb0, 0x03, 0x2e, 0x8f, 0x00, 0x07, 0x2e, 0x91, 0x00, + 0x05, 0x2e, 0xb1, 0x00, 0x3f, 0xba, 0x9f, 0xb8, 0x01, 0x2e, 0xb1, 0x00, 0xa3, 0xbd, + 0x4c, 0x0a, 0x05, 0x2e, 0xb1, 0x00, 0x04, 0xbe, 0xbf, 0xb9, 0xcb, 0x0a, 0x4f, 0xba, + 0x22, 0xbd, 0x01, 0x2e, 0xb3, 0x00, 0xdc, 0x0a, 0x2f, 0xb9, 0x03, 0x2e, 0xb8, 0x00, + 0x0a, 0xbe, 0x9a, 0x0a, 0xcf, 0xb9, 0x9b, 0xbc, 0x01, 0x2e, 0x97, 0x00, 0x9f, 0xb8, + 0x93, 0x0a, 0x0f, 0xbc, 0x91, 0x0a, 0x0f, 0xb8, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, + 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, 0xb9, 0x01, 0x2e, 0x19, 0x00, 0x31, 0x30, + 0x8a, 0x04, 0x00, 0x90, 0x07, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xa2, 0x03, 0x2f, + 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x19, 0x50, 0x05, 0x52, 0x98, 0x2e, + 0x4d, 0xb7, 0x05, 0x2e, 0x78, 0x00, 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0x21, 0x2e, + 0x78, 0x00, 0x25, 0x2e, 0xdd, 0x00, 0x98, 0x2e, 0x3e, 0xb7, 0x00, 0xb2, 0x02, 0x30, + 0x01, 0x30, 0x04, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x00, 0x2f, 0x21, 0x30, + 0x01, 0x2e, 0xea, 0x00, 0x08, 0x1a, 0x0e, 0x2f, 0x23, 0x2e, 0xea, 0x00, 0x33, 0x30, + 0x1b, 0x50, 0x0b, 0x09, 0x01, 0x40, 0x17, 0x56, 0x46, 0xbe, 0x4b, 0x08, 0x4c, 0x0a, + 0x01, 0x42, 0x0a, 0x80, 0x15, 0x52, 0x01, 0x42, 0x00, 0x2e, 0x01, 0x2e, 0x18, 0x00, + 0x00, 0xb2, 0x1f, 0x2f, 0x03, 0x2e, 0xc0, 0xf5, 0xf0, 0x30, 0x48, 0x08, 0x47, 0xaa, + 0x74, 0x30, 0x07, 0x2e, 0x7a, 0x00, 0x61, 0x22, 0x4b, 0x1a, 0x05, 0x2f, 0x07, 0x2e, + 0x66, 0xf5, 0xbf, 0xbd, 0xbf, 0xb9, 0xc0, 0x90, 0x0b, 0x2f, 0x1d, 0x56, 0x2b, 0x30, + 0xd2, 0x42, 0xdb, 0x42, 0x01, 0x04, 0xc2, 0x42, 0x04, 0xbd, 0xfe, 0x80, 0x81, 0x84, + 0x23, 0x2e, 0x7a, 0x00, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, + 0xd6, 0x00, 0x81, 0x84, 0x25, 0x2e, 0xd6, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, + 0x05, 0x2e, 0x8a, 0x00, 0x0b, 0x50, 0x90, 0x08, 0x80, 0xb2, 0x0b, 0x2f, 0x05, 0x2e, + 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, + 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, + 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, + 0xc8, 0x2e, 0xc0, 0x50, 0x90, 0x7f, 0xe5, 0x7f, 0xd4, 0x7f, 0xc3, 0x7f, 0xb1, 0x7f, + 0xa2, 0x7f, 0x87, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, + 0x60, 0x7f, 0x98, 0x2e, 0x35, 0xb7, 0x02, 0x30, 0x63, 0x6f, 0x15, 0x52, 0x50, 0x7f, + 0x62, 0x7f, 0x5a, 0x2c, 0x02, 0x32, 0x1a, 0x09, 0x00, 0xb3, 0x14, 0x2f, 0x00, 0xb2, + 0x03, 0x2f, 0x09, 0x2e, 0x18, 0x00, 0x00, 0x91, 0x0c, 0x2f, 0x43, 0x7f, 0x98, 0x2e, + 0x97, 0xb7, 0x1f, 0x50, 0x02, 0x8a, 0x02, 0x32, 0x04, 0x30, 0x25, 0x2e, 0x64, 0xf5, + 0x15, 0x52, 0x50, 0x6f, 0x43, 0x6f, 0x44, 0x43, 0x25, 0x2e, 0x60, 0xf5, 0xd9, 0x08, + 0xc0, 0xb2, 0x36, 0x2f, 0x98, 0x2e, 0x3e, 0xb7, 0x00, 0xb2, 0x06, 0x2f, 0x01, 0x2e, + 0x19, 0x00, 0x00, 0xb2, 0x02, 0x2f, 0x50, 0x6f, 0x00, 0x90, 0x0a, 0x2f, 0x01, 0x2e, + 0x79, 0x00, 0x00, 0x90, 0x19, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x79, 0x00, 0x00, 0x30, + 0x98, 0x2e, 0xdc, 0x03, 0x13, 0x2d, 0x01, 0x2e, 0xc3, 0xf5, 0x0c, 0xbc, 0x0f, 0xb8, + 0x12, 0x30, 0x10, 0x04, 0x03, 0xb0, 0x26, 0x25, 0x21, 0x50, 0x03, 0x52, 0x98, 0x2e, + 0x4d, 0xb7, 0x10, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x02, 0x30, 0x60, 0x7f, 0x25, 0x2e, + 0x79, 0x00, 0x60, 0x6f, 0x00, 0x90, 0x05, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xea, 0x00, + 0x15, 0x50, 0x21, 0x2e, 0x64, 0xf5, 0x15, 0x52, 0x23, 0x2e, 0x60, 0xf5, 0x02, 0x32, + 0x50, 0x6f, 0x00, 0x90, 0x02, 0x2f, 0x03, 0x30, 0x27, 0x2e, 0x78, 0x00, 0x07, 0x2e, + 0x60, 0xf5, 0x1a, 0x09, 0x00, 0x91, 0xa3, 0x2f, 0x19, 0x09, 0x00, 0x91, 0xa0, 0x2f, + 0x90, 0x6f, 0xa2, 0x6f, 0xb1, 0x6f, 0xc3, 0x6f, 0xd4, 0x6f, 0xe5, 0x6f, 0x7b, 0x6f, + 0xf6, 0x6f, 0x87, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, + 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x7c, 0x00, 0x0f, 0x2e, 0x7c, 0x00, + 0xbe, 0x09, 0xa2, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, + 0x91, 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x23, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, + 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, 0x2e, + 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x42, 0x2f, + 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0x89, 0x00, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, + 0x0f, 0xb8, 0x00, 0x90, 0x23, 0x2e, 0xd8, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, + 0x03, 0x2e, 0xd4, 0x00, 0x44, 0xb2, 0x05, 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, + 0x21, 0x2e, 0x7c, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, 0x9f, 0xb8, + 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, + 0x0e, 0x2f, 0x03, 0x2e, 0x49, 0xf1, 0x25, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, + 0x98, 0x2e, 0x35, 0xb7, 0x00, 0xb2, 0x10, 0x30, 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, + 0xd4, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xaf, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7c, 0x00, + 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, + 0x21, 0x2e, 0x7d, 0x00, 0x23, 0x2e, 0x7c, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, + 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, + 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x60, 0x51, 0x0a, 0x25, 0x36, 0x88, + 0xf4, 0x7f, 0xeb, 0x7f, 0x00, 0x32, 0x31, 0x52, 0x32, 0x30, 0x13, 0x30, 0x98, 0x2e, + 0x15, 0xcb, 0x0a, 0x25, 0x33, 0x84, 0xd2, 0x7f, 0x43, 0x30, 0x05, 0x50, 0x2d, 0x52, + 0x98, 0x2e, 0x95, 0xc1, 0xd2, 0x6f, 0x27, 0x52, 0x98, 0x2e, 0xd7, 0xc7, 0x2a, 0x25, + 0xb0, 0x86, 0xc0, 0x7f, 0xd3, 0x7f, 0xaf, 0x84, 0x29, 0x50, 0xf1, 0x6f, 0x98, 0x2e, + 0x4d, 0xc8, 0x2a, 0x25, 0xae, 0x8a, 0xaa, 0x88, 0xf2, 0x6e, 0x2b, 0x50, 0xc1, 0x6f, + 0xd3, 0x6f, 0xf4, 0x7f, 0x98, 0x2e, 0xb6, 0xc8, 0xe0, 0x6e, 0x00, 0xb2, 0x32, 0x2f, + 0x33, 0x54, 0x83, 0x86, 0xf1, 0x6f, 0xc3, 0x7f, 0x04, 0x30, 0x30, 0x30, 0xf4, 0x7f, + 0xd0, 0x7f, 0xb2, 0x7f, 0xe3, 0x30, 0xc5, 0x6f, 0x56, 0x40, 0x45, 0x41, 0x28, 0x08, + 0x03, 0x14, 0x0e, 0xb4, 0x08, 0xbc, 0x82, 0x40, 0x10, 0x0a, 0x2f, 0x54, 0x26, 0x05, + 0x91, 0x7f, 0x44, 0x28, 0xa3, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x08, 0xb9, 0x33, 0x30, + 0x53, 0x09, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x6f, 0x83, 0x17, 0x47, 0x40, 0x6c, 0x15, + 0xb2, 0x6f, 0xbe, 0x09, 0x75, 0x0b, 0x90, 0x42, 0x45, 0x42, 0x51, 0x0e, 0x32, 0xbc, + 0x02, 0x89, 0xa1, 0x6f, 0x7e, 0x86, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0x04, 0x30, + 0x91, 0x6f, 0xd6, 0x2f, 0xeb, 0x6f, 0xa0, 0x5e, 0xb8, 0x2e, 0x03, 0x2e, 0x97, 0x00, + 0x1b, 0xbc, 0x60, 0x50, 0x9f, 0xbc, 0x0c, 0xb8, 0xf0, 0x7f, 0x40, 0xb2, 0xeb, 0x7f, + 0x2b, 0x2f, 0x03, 0x2e, 0x7f, 0x00, 0x41, 0x40, 0x01, 0x2e, 0xc8, 0x00, 0x01, 0x1a, + 0x11, 0x2f, 0x37, 0x58, 0x23, 0x2e, 0xc8, 0x00, 0x10, 0x41, 0xa0, 0x7f, 0x38, 0x81, + 0x01, 0x41, 0xd0, 0x7f, 0xb1, 0x7f, 0x98, 0x2e, 0x64, 0xcf, 0xd0, 0x6f, 0x07, 0x80, + 0xa1, 0x6f, 0x11, 0x42, 0x00, 0x2e, 0xb1, 0x6f, 0x01, 0x42, 0x11, 0x30, 0x01, 0x2e, + 0xfc, 0x00, 0x00, 0xa8, 0x03, 0x30, 0xcb, 0x22, 0x4a, 0x25, 0x01, 0x2e, 0x7f, 0x00, + 0x3c, 0x89, 0x35, 0x52, 0x05, 0x54, 0x98, 0x2e, 0xc4, 0xce, 0xc1, 0x6f, 0xf0, 0x6f, + 0x98, 0x2e, 0x95, 0xcf, 0x04, 0x2d, 0x01, 0x30, 0xf0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, + 0xeb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, 0x03, 0x2e, 0xb3, 0x00, 0x02, 0x32, 0xf0, 0x30, + 0x03, 0x31, 0x30, 0x50, 0x8a, 0x08, 0x08, 0x08, 0xcb, 0x08, 0xe0, 0x7f, 0x80, 0xb2, + 0xf3, 0x7f, 0xdb, 0x7f, 0x25, 0x2f, 0x03, 0x2e, 0xca, 0x00, 0x41, 0x90, 0x04, 0x2f, + 0x01, 0x30, 0x23, 0x2e, 0xca, 0x00, 0x98, 0x2e, 0x3f, 0x03, 0xc0, 0xb2, 0x05, 0x2f, + 0x03, 0x2e, 0xda, 0x00, 0x00, 0x30, 0x41, 0x04, 0x23, 0x2e, 0xda, 0x00, 0x98, 0x2e, + 0x92, 0xb2, 0x10, 0x25, 0xf0, 0x6f, 0x00, 0xb2, 0x05, 0x2f, 0x01, 0x2e, 0xda, 0x00, + 0x02, 0x30, 0x10, 0x04, 0x21, 0x2e, 0xda, 0x00, 0x40, 0xb2, 0x01, 0x2f, 0x23, 0x2e, + 0xc8, 0x01, 0xdb, 0x6f, 0xe0, 0x6f, 0xd0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, 0x01, 0x30, + 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0xca, 0x00, 0xdb, 0x6f, + 0xd0, 0x5f, 0xb8, 0x2e, 0xd0, 0x50, 0x0a, 0x25, 0x33, 0x84, 0x55, 0x50, 0xd2, 0x7f, + 0xe2, 0x7f, 0x03, 0x8c, 0xc0, 0x7f, 0xbb, 0x7f, 0x00, 0x30, 0x05, 0x5a, 0x39, 0x54, + 0x51, 0x41, 0xa5, 0x7f, 0x96, 0x7f, 0x80, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, 0x30, + 0xf5, 0x7f, 0x20, 0x25, 0x91, 0x6f, 0x3b, 0x58, 0x3d, 0x5c, 0x3b, 0x56, 0x98, 0x2e, + 0x67, 0xcc, 0xc1, 0x6f, 0xd5, 0x6f, 0x52, 0x40, 0x50, 0x43, 0xc1, 0x7f, 0xd5, 0x7f, + 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x86, 0x6f, + 0x30, 0x28, 0x92, 0x6f, 0x82, 0x8c, 0xa5, 0x6f, 0x6f, 0x52, 0x69, 0x0e, 0x39, 0x54, + 0xdb, 0x2f, 0x19, 0xa0, 0x15, 0x30, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x81, 0x01, + 0x0a, 0x2d, 0x01, 0x2e, 0x81, 0x01, 0x05, 0x28, 0x42, 0x36, 0x21, 0x2e, 0x81, 0x01, + 0x02, 0x0e, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, 0x12, 0x30, 0x01, 0x40, + 0x98, 0x2e, 0xfe, 0xc9, 0x51, 0x6f, 0x0b, 0x5c, 0x8e, 0x0e, 0x3b, 0x6f, 0x57, 0x58, + 0x02, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x45, 0x6f, 0x2a, 0x8d, 0xd2, 0x7f, 0xcb, 0x7f, + 0x13, 0x2f, 0x02, 0x30, 0x3f, 0x50, 0xd2, 0x7f, 0xa8, 0x0e, 0x0e, 0x2f, 0xc0, 0x6f, + 0x53, 0x54, 0x02, 0x00, 0x51, 0x54, 0x42, 0x0e, 0x10, 0x30, 0x59, 0x52, 0x02, 0x30, + 0x01, 0x2f, 0x00, 0x2e, 0x03, 0x2d, 0x50, 0x42, 0x42, 0x42, 0x12, 0x30, 0xd2, 0x7f, + 0x80, 0xb2, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x80, 0x01, 0x12, 0x2d, 0x01, 0x2e, + 0xc9, 0x00, 0x02, 0x80, 0x05, 0x2e, 0x80, 0x01, 0x11, 0x30, 0x91, 0x28, 0x00, 0x40, + 0x25, 0x2e, 0x80, 0x01, 0x10, 0x0e, 0x05, 0x2f, 0x01, 0x2e, 0x7f, 0x01, 0x01, 0x90, + 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x00, 0x2e, 0xa0, 0x41, 0x01, 0x90, 0xa6, 0x7f, + 0x90, 0x2e, 0xe3, 0xb4, 0x01, 0x2e, 0x95, 0x01, 0x00, 0xa8, 0x90, 0x2e, 0xe3, 0xb4, + 0x5b, 0x54, 0x95, 0x80, 0x82, 0x40, 0x80, 0xb2, 0x02, 0x40, 0x2d, 0x8c, 0x3f, 0x52, + 0x96, 0x7f, 0x90, 0x2e, 0xc2, 0xb3, 0x29, 0x0e, 0x76, 0x2f, 0x01, 0x2e, 0xc9, 0x00, + 0x00, 0x40, 0x81, 0x28, 0x45, 0x52, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x5d, 0x54, + 0x80, 0x7f, 0x00, 0x2e, 0xa1, 0x40, 0x72, 0x7f, 0x82, 0x80, 0x82, 0x40, 0x60, 0x7f, + 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x62, 0x6f, 0x05, 0x30, + 0x87, 0x40, 0xc0, 0x91, 0x04, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x83, 0x01, 0x80, 0xb2, + 0x14, 0x30, 0x00, 0x2f, 0x04, 0x30, 0x05, 0x2e, 0xc9, 0x00, 0x73, 0x6f, 0x81, 0x40, + 0xe2, 0x40, 0x69, 0x04, 0x11, 0x0f, 0xe1, 0x40, 0x16, 0x30, 0xfe, 0x29, 0xcb, 0x40, + 0x02, 0x2f, 0x83, 0x6f, 0x83, 0x0f, 0x22, 0x2f, 0x47, 0x56, 0x13, 0x0f, 0x12, 0x30, + 0x77, 0x2f, 0x49, 0x54, 0x42, 0x0e, 0x12, 0x30, 0x73, 0x2f, 0x00, 0x91, 0x0a, 0x2f, + 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa8, 0x02, 0x30, 0x6c, 0x2f, 0x63, 0x50, 0x00, 0x2e, + 0x17, 0x42, 0x05, 0x42, 0x68, 0x2c, 0x12, 0x30, 0x0b, 0x25, 0x08, 0x0f, 0x50, 0x30, + 0x02, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x03, 0x2d, 0x40, 0x30, 0x21, 0x2e, 0x83, 0x01, + 0x2b, 0x2e, 0x85, 0x01, 0x5a, 0x2c, 0x12, 0x30, 0x00, 0x91, 0x2b, 0x25, 0x04, 0x2f, + 0x63, 0x50, 0x02, 0x30, 0x17, 0x42, 0x17, 0x2c, 0x02, 0x42, 0x98, 0x2e, 0xfe, 0xc9, + 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x05, 0x2e, 0xc9, 0x00, 0x81, 0x84, 0x5b, 0x30, + 0x82, 0x40, 0x37, 0x2e, 0x83, 0x01, 0x02, 0x0e, 0x07, 0x2f, 0x5f, 0x52, 0x40, 0x30, + 0x62, 0x40, 0x41, 0x40, 0x91, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x05, 0x30, + 0x2b, 0x2e, 0x85, 0x01, 0x12, 0x30, 0x36, 0x2c, 0x16, 0x30, 0x15, 0x25, 0x81, 0x7f, + 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x19, 0xa2, 0x16, 0x30, + 0x15, 0x2f, 0x05, 0x2e, 0x97, 0x01, 0x80, 0x6f, 0x82, 0x0e, 0x05, 0x2f, 0x01, 0x2e, + 0x86, 0x01, 0x06, 0x28, 0x21, 0x2e, 0x86, 0x01, 0x0b, 0x2d, 0x03, 0x2e, 0x87, 0x01, + 0x5f, 0x54, 0x4e, 0x28, 0x91, 0x42, 0x00, 0x2e, 0x82, 0x40, 0x90, 0x0e, 0x01, 0x2f, + 0x21, 0x2e, 0x88, 0x01, 0x02, 0x30, 0x13, 0x2c, 0x05, 0x30, 0xc0, 0x6f, 0x08, 0x1c, + 0xa8, 0x0f, 0x16, 0x30, 0x05, 0x30, 0x5b, 0x50, 0x09, 0x2f, 0x02, 0x80, 0x2d, 0x2e, + 0x82, 0x01, 0x05, 0x42, 0x05, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x3e, 0x80, 0x00, 0x2e, + 0x06, 0x42, 0x02, 0x30, 0x90, 0x6f, 0x3e, 0x88, 0x01, 0x40, 0x04, 0x41, 0x4c, 0x28, + 0x01, 0x42, 0x07, 0x80, 0x10, 0x25, 0x24, 0x40, 0x00, 0x40, 0x00, 0xa8, 0xf5, 0x22, + 0x23, 0x29, 0x44, 0x42, 0x7a, 0x82, 0x7e, 0x88, 0x43, 0x40, 0x04, 0x41, 0x00, 0xab, + 0xf5, 0x23, 0xdf, 0x28, 0x43, 0x42, 0xd9, 0xa0, 0x14, 0x2f, 0x00, 0x90, 0x02, 0x2f, + 0xd2, 0x6f, 0x81, 0xb2, 0x05, 0x2f, 0x63, 0x54, 0x06, 0x28, 0x90, 0x42, 0x85, 0x42, + 0x09, 0x2c, 0x02, 0x30, 0x5b, 0x50, 0x03, 0x80, 0x29, 0x2e, 0x7e, 0x01, 0x2b, 0x2e, + 0x82, 0x01, 0x05, 0x42, 0x12, 0x30, 0x2b, 0x2e, 0x83, 0x01, 0x45, 0x82, 0x00, 0x2e, + 0x40, 0x40, 0x7a, 0x82, 0x02, 0xa0, 0x08, 0x2f, 0x63, 0x50, 0x3b, 0x30, 0x15, 0x42, + 0x05, 0x42, 0x37, 0x80, 0x37, 0x2e, 0x7e, 0x01, 0x05, 0x42, 0x12, 0x30, 0x01, 0x2e, + 0xc9, 0x00, 0x02, 0x8c, 0x40, 0x40, 0x84, 0x41, 0x7a, 0x8c, 0x04, 0x0f, 0x03, 0x2f, + 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa4, 0x04, 0x2f, 0x2b, 0x2e, 0x82, 0x01, 0x98, 0x2e, + 0xf3, 0x03, 0x12, 0x30, 0x81, 0x90, 0x61, 0x52, 0x08, 0x2f, 0x65, 0x42, 0x65, 0x42, + 0x43, 0x80, 0x39, 0x84, 0x82, 0x88, 0x05, 0x42, 0x45, 0x42, 0x85, 0x42, 0x05, 0x43, + 0x00, 0x2e, 0x80, 0x41, 0x00, 0x90, 0x90, 0x2e, 0xe1, 0xb4, 0x65, 0x54, 0xc1, 0x6f, + 0x80, 0x40, 0x00, 0xb2, 0x43, 0x58, 0x69, 0x50, 0x44, 0x2f, 0x55, 0x5c, 0xb7, 0x87, + 0x8c, 0x0f, 0x0d, 0x2e, 0x96, 0x01, 0xc4, 0x40, 0x36, 0x2f, 0x41, 0x56, 0x8b, 0x0e, + 0x2a, 0x2f, 0x0b, 0x52, 0xa1, 0x0e, 0x0a, 0x2f, 0x05, 0x2e, 0x8f, 0x01, 0x14, 0x25, + 0x98, 0x2e, 0xfe, 0xc9, 0x4b, 0x54, 0x02, 0x0f, 0x69, 0x50, 0x05, 0x30, 0x65, 0x54, + 0x15, 0x2f, 0x03, 0x2e, 0x8e, 0x01, 0x4d, 0x5c, 0x8e, 0x0f, 0x3a, 0x2f, 0x05, 0x2e, + 0x8f, 0x01, 0x98, 0x2e, 0xfe, 0xc9, 0x4f, 0x54, 0x82, 0x0f, 0x05, 0x30, 0x69, 0x50, + 0x65, 0x54, 0x30, 0x2f, 0x6d, 0x52, 0x15, 0x30, 0x42, 0x8c, 0x45, 0x42, 0x04, 0x30, + 0x2b, 0x2c, 0x84, 0x43, 0x6b, 0x52, 0x42, 0x8c, 0x00, 0x2e, 0x85, 0x43, 0x15, 0x30, + 0x24, 0x2c, 0x45, 0x42, 0x8e, 0x0f, 0x20, 0x2f, 0x0d, 0x2e, 0x8e, 0x01, 0xb1, 0x0e, + 0x1c, 0x2f, 0x23, 0x2e, 0x8e, 0x01, 0x1a, 0x2d, 0x0e, 0x0e, 0x17, 0x2f, 0xa1, 0x0f, + 0x15, 0x2f, 0x23, 0x2e, 0x8d, 0x01, 0x13, 0x2d, 0x98, 0x2e, 0x74, 0xc0, 0x43, 0x54, + 0xc2, 0x0e, 0x0a, 0x2f, 0x65, 0x50, 0x04, 0x80, 0x0b, 0x30, 0x06, 0x82, 0x0b, 0x42, + 0x79, 0x80, 0x41, 0x40, 0x12, 0x30, 0x25, 0x2e, 0x8c, 0x01, 0x01, 0x42, 0x05, 0x30, + 0x69, 0x50, 0x65, 0x54, 0x84, 0x82, 0x43, 0x84, 0xbe, 0x8c, 0x84, 0x40, 0x86, 0x41, + 0x26, 0x29, 0x94, 0x42, 0xbe, 0x8e, 0xd5, 0x7f, 0x19, 0xa1, 0x43, 0x40, 0x0b, 0x2e, + 0x8c, 0x01, 0x84, 0x40, 0xc7, 0x41, 0x5d, 0x29, 0x27, 0x29, 0x45, 0x42, 0x84, 0x42, + 0xc2, 0x7f, 0x01, 0x2f, 0xc0, 0xb3, 0x1d, 0x2f, 0x05, 0x2e, 0x94, 0x01, 0x99, 0xa0, + 0x01, 0x2f, 0x80, 0xb3, 0x13, 0x2f, 0x80, 0xb3, 0x18, 0x2f, 0xc0, 0xb3, 0x16, 0x2f, + 0x12, 0x40, 0x01, 0x40, 0x92, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x92, 0x6f, 0x10, 0x0f, + 0x20, 0x30, 0x03, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x0a, 0x2d, 0x21, 0x2e, + 0x7e, 0x01, 0x07, 0x2d, 0x20, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x03, 0x2d, 0x10, 0x30, + 0x21, 0x2e, 0x7e, 0x01, 0xc2, 0x6f, 0x01, 0x2e, 0xc9, 0x00, 0xbc, 0x84, 0x02, 0x80, + 0x82, 0x40, 0x00, 0x40, 0x90, 0x0e, 0xd5, 0x6f, 0x02, 0x2f, 0x15, 0x30, 0x98, 0x2e, + 0xf3, 0x03, 0x41, 0x91, 0x05, 0x30, 0x07, 0x2f, 0x67, 0x50, 0x3d, 0x80, 0x2b, 0x2e, + 0x8f, 0x01, 0x05, 0x42, 0x04, 0x80, 0x00, 0x2e, 0x05, 0x42, 0x02, 0x2c, 0x00, 0x30, + 0x00, 0x30, 0xa2, 0x6f, 0x98, 0x8a, 0x86, 0x40, 0x80, 0xa7, 0x05, 0x2f, 0x98, 0x2e, + 0xf3, 0x03, 0xc0, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x06, 0x25, 0x1a, 0x25, 0xe2, 0x6f, + 0x76, 0x82, 0x96, 0x40, 0x56, 0x43, 0x51, 0x0e, 0xfb, 0x2f, 0xbb, 0x6f, 0x30, 0x5f, + 0xb8, 0x2e, 0x01, 0x2e, 0xb8, 0x00, 0x01, 0x31, 0x41, 0x08, 0x40, 0xb2, 0x20, 0x50, + 0xf2, 0x30, 0x02, 0x08, 0xfb, 0x7f, 0x01, 0x30, 0x10, 0x2f, 0x05, 0x2e, 0xcc, 0x00, + 0x81, 0x90, 0xe0, 0x7f, 0x03, 0x2f, 0x23, 0x2e, 0xcc, 0x00, 0x98, 0x2e, 0x55, 0xb6, + 0x98, 0x2e, 0x1d, 0xb5, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, 0x80, 0x2e, + 0x95, 0xcf, 0x98, 0x2e, 0x95, 0xcf, 0x10, 0x30, 0x21, 0x2e, 0xcc, 0x00, 0xfb, 0x6f, + 0xe0, 0x5f, 0xb8, 0x2e, 0x00, 0x51, 0x05, 0x58, 0xeb, 0x7f, 0x2a, 0x25, 0x89, 0x52, + 0x6f, 0x5a, 0x89, 0x50, 0x13, 0x41, 0x06, 0x40, 0xb3, 0x01, 0x16, 0x42, 0xcb, 0x16, + 0x06, 0x40, 0xf3, 0x02, 0x13, 0x42, 0x65, 0x0e, 0xf5, 0x2f, 0x05, 0x40, 0x14, 0x30, + 0x2c, 0x29, 0x04, 0x42, 0x08, 0xa1, 0x00, 0x30, 0x90, 0x2e, 0x52, 0xb6, 0xb3, 0x88, + 0xb0, 0x8a, 0xb6, 0x84, 0xa4, 0x7f, 0xc4, 0x7f, 0xb5, 0x7f, 0xd5, 0x7f, 0x92, 0x7f, + 0x73, 0x30, 0x04, 0x30, 0x55, 0x40, 0x42, 0x40, 0x8a, 0x17, 0xf3, 0x08, 0x6b, 0x01, + 0x90, 0x02, 0x53, 0xb8, 0x4b, 0x82, 0xad, 0xbe, 0x71, 0x7f, 0x45, 0x0a, 0x09, 0x54, + 0x84, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0xa3, 0x6f, 0x7b, 0x54, 0xd0, 0x42, 0xa3, 0x7f, + 0xf2, 0x7f, 0x60, 0x7f, 0x20, 0x25, 0x71, 0x6f, 0x75, 0x5a, 0x77, 0x58, 0x79, 0x5c, + 0x75, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0x62, 0x6f, 0x50, 0x42, 0xb1, 0x7f, + 0xb3, 0x30, 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x84, 0x6f, 0x20, 0x29, 0x71, 0x6f, + 0x92, 0x6f, 0xa5, 0x6f, 0x76, 0x82, 0x6a, 0x0e, 0x73, 0x30, 0x00, 0x30, 0xd0, 0x2f, + 0xd2, 0x6f, 0xd1, 0x7f, 0xb4, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, + 0x02, 0x0a, 0xc2, 0x6f, 0xc0, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, + 0x42, 0x0a, 0xc0, 0x6f, 0x08, 0x17, 0x41, 0x18, 0x89, 0x16, 0xe1, 0x18, 0xd0, 0x18, + 0xa1, 0x7f, 0x27, 0x25, 0x16, 0x25, 0x98, 0x2e, 0x79, 0xc0, 0x8b, 0x54, 0x90, 0x7f, + 0xb3, 0x30, 0x82, 0x40, 0x80, 0x90, 0x0d, 0x2f, 0x7d, 0x52, 0x92, 0x6f, 0x98, 0x2e, + 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x0e, 0x06, 0x2f, 0x8b, 0x50, 0x14, 0x30, 0x42, 0x6f, + 0x51, 0x6f, 0x14, 0x42, 0x12, 0x42, 0x01, 0x42, 0x00, 0x2e, 0x31, 0x6f, 0x98, 0x2e, + 0x74, 0xc0, 0x41, 0x6f, 0x80, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x82, 0x6f, 0x10, 0x04, + 0x43, 0x52, 0x01, 0x0f, 0x05, 0x2e, 0xcb, 0x00, 0x00, 0x30, 0x04, 0x30, 0x21, 0x2f, + 0x51, 0x6f, 0x43, 0x58, 0x8c, 0x0e, 0x04, 0x30, 0x1c, 0x2f, 0x85, 0x88, 0x41, 0x6f, + 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, 0x16, 0x2f, 0x84, 0x88, 0x00, 0x2e, 0x04, 0x41, + 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x0f, 0x2f, 0x82, 0x88, 0x31, 0x6f, 0x04, 0x41, + 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x08, 0x2f, 0x83, 0x88, 0x00, 0x2e, 0x04, 0x41, + 0x8c, 0x0f, 0x04, 0x30, 0x02, 0x2f, 0x21, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x00, 0x91, + 0x14, 0x2f, 0x03, 0x2e, 0xa1, 0x01, 0x41, 0x90, 0x0e, 0x2f, 0x03, 0x2e, 0xad, 0x01, + 0x14, 0x30, 0x4c, 0x28, 0x23, 0x2e, 0xad, 0x01, 0x46, 0xa0, 0x06, 0x2f, 0x81, 0x84, + 0x8d, 0x52, 0x48, 0x82, 0x82, 0x40, 0x21, 0x2e, 0xa1, 0x01, 0x42, 0x42, 0x5c, 0x2c, + 0x02, 0x30, 0x05, 0x2e, 0xaa, 0x01, 0x80, 0xb2, 0x02, 0x30, 0x55, 0x2f, 0x03, 0x2e, + 0xa9, 0x01, 0x92, 0x6f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x0f, + 0x00, 0x30, 0x02, 0x30, 0x4a, 0x2f, 0xa2, 0x6f, 0x87, 0x52, 0x91, 0x00, 0x85, 0x52, + 0x51, 0x0e, 0x02, 0x2f, 0x00, 0x2e, 0x43, 0x2c, 0x02, 0x30, 0xc2, 0x6f, 0x7f, 0x52, + 0x91, 0x0e, 0x02, 0x30, 0x3c, 0x2f, 0x51, 0x6f, 0x81, 0x54, 0x98, 0x2e, 0xfe, 0xc9, + 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x32, 0x6f, 0xc0, 0x7f, + 0xb3, 0x30, 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x42, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, + 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x28, 0x83, 0x52, 0x98, 0x2e, + 0xfe, 0xc9, 0xc2, 0x6f, 0x90, 0x0f, 0x00, 0x30, 0x02, 0x30, 0x1d, 0x2f, 0x05, 0x2e, + 0xa1, 0x01, 0x80, 0xb2, 0x12, 0x30, 0x0f, 0x2f, 0x42, 0x6f, 0x03, 0x2e, 0xab, 0x01, + 0x91, 0x0e, 0x02, 0x30, 0x12, 0x2f, 0x52, 0x6f, 0x03, 0x2e, 0xac, 0x01, 0x91, 0x0f, + 0x02, 0x30, 0x0c, 0x2f, 0x21, 0x2e, 0xaa, 0x01, 0x0a, 0x2c, 0x12, 0x30, 0x03, 0x2e, + 0xcb, 0x00, 0x8d, 0x58, 0x08, 0x89, 0x41, 0x40, 0x11, 0x43, 0x00, 0x43, 0x25, 0x2e, + 0xa1, 0x01, 0xd4, 0x6f, 0x8f, 0x52, 0x00, 0x43, 0x3a, 0x89, 0x00, 0x2e, 0x10, 0x43, + 0x10, 0x43, 0x61, 0x0e, 0xfb, 0x2f, 0x03, 0x2e, 0xa0, 0x01, 0x11, 0x1a, 0x02, 0x2f, + 0x02, 0x25, 0x21, 0x2e, 0xa0, 0x01, 0xeb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x91, 0x52, + 0x10, 0x30, 0x02, 0x30, 0x95, 0x56, 0x52, 0x42, 0x4b, 0x0e, 0xfc, 0x2f, 0x8d, 0x54, + 0x88, 0x82, 0x93, 0x56, 0x80, 0x42, 0x53, 0x42, 0x40, 0x42, 0x42, 0x86, 0x83, 0x54, + 0xc0, 0x2e, 0xc2, 0x42, 0x00, 0x2e, 0xa3, 0x52, 0x00, 0x51, 0x52, 0x40, 0x47, 0x40, + 0x1a, 0x25, 0x01, 0x2e, 0x97, 0x00, 0x8f, 0xbe, 0x72, 0x86, 0xfb, 0x7f, 0x0b, 0x30, + 0x7c, 0xbf, 0xa5, 0x50, 0x10, 0x08, 0xdf, 0xba, 0x70, 0x88, 0xf8, 0xbf, 0xcb, 0x42, + 0xd3, 0x7f, 0x6c, 0xbb, 0xfc, 0xbb, 0xc5, 0x0a, 0x90, 0x7f, 0x1b, 0x7f, 0x0b, 0x43, + 0xc0, 0xb2, 0xe5, 0x7f, 0xb7, 0x7f, 0xa6, 0x7f, 0xc4, 0x7f, 0x90, 0x2e, 0x1c, 0xb7, + 0x07, 0x2e, 0xd2, 0x00, 0xc0, 0xb2, 0x0b, 0x2f, 0x97, 0x52, 0x01, 0x2e, 0xcd, 0x00, + 0x82, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x0b, 0x30, 0x37, 0x2e, 0xd2, 0x00, 0x82, 0x6f, + 0x90, 0x6f, 0x1a, 0x25, 0x00, 0xb2, 0x8b, 0x7f, 0x14, 0x2f, 0xa6, 0xbd, 0x25, 0xbd, + 0xb6, 0xb9, 0x2f, 0xb9, 0x80, 0xb2, 0xd4, 0xb0, 0x0c, 0x2f, 0x99, 0x54, 0x9b, 0x56, + 0x0b, 0x30, 0x0b, 0x2e, 0xb1, 0x00, 0xa1, 0x58, 0x9b, 0x42, 0xdb, 0x42, 0x6c, 0x09, + 0x2b, 0x2e, 0xb1, 0x00, 0x8b, 0x42, 0xcb, 0x42, 0x86, 0x7f, 0x73, 0x84, 0xa7, 0x56, + 0xc3, 0x08, 0x39, 0x52, 0x05, 0x50, 0x72, 0x7f, 0x63, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, + 0xe1, 0x6f, 0x62, 0x6f, 0xd1, 0x0a, 0x01, 0x2e, 0xcd, 0x00, 0xd5, 0x6f, 0xc4, 0x6f, + 0x72, 0x6f, 0x97, 0x52, 0x9d, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0x23, 0x6f, 0x90, 0x6f, + 0x99, 0x52, 0xc0, 0xb2, 0x04, 0xbd, 0x54, 0x40, 0xaf, 0xb9, 0x45, 0x40, 0xe1, 0x7f, + 0x02, 0x30, 0x06, 0x2f, 0xc0, 0xb2, 0x02, 0x30, 0x03, 0x2f, 0x9b, 0x5c, 0x12, 0x30, + 0x94, 0x43, 0x85, 0x43, 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x06, 0x6f, + 0x26, 0x01, 0x16, 0x6f, 0x6e, 0x03, 0x45, 0x42, 0xc0, 0x90, 0x29, 0x2e, 0xce, 0x00, + 0x9b, 0x52, 0x14, 0x2f, 0x9b, 0x5c, 0x00, 0x2e, 0x93, 0x41, 0x86, 0x41, 0xe3, 0x04, + 0xae, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x86, 0x6f, 0x73, 0x0f, + 0x07, 0x2f, 0x83, 0x6f, 0xc0, 0xb2, 0x04, 0x2f, 0x54, 0x42, 0x45, 0x42, 0x12, 0x30, + 0x04, 0x2c, 0x11, 0x30, 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, 0x02, 0xbc, 0x0f, 0xb8, + 0xd2, 0x7f, 0x00, 0xb2, 0x0a, 0x2f, 0x01, 0x2e, 0xfc, 0x00, 0x05, 0x2e, 0xc7, 0x01, + 0x10, 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xc7, 0x01, 0x03, 0x2d, 0x02, 0x2c, 0x01, 0x30, + 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xd1, 0x6f, 0xa0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xe2, 0x6f, 0x9f, 0x52, 0x01, 0x2e, 0xce, 0x00, 0x82, 0x40, 0x50, 0x42, + 0x0c, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, 0x2e, 0xd2, 0x00, 0x01, 0x30, 0xb0, 0x6f, + 0x98, 0x2e, 0x95, 0xcf, 0xa0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, + 0xfb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x83, 0x86, 0x01, 0x30, 0x00, 0x30, 0x94, 0x40, + 0x24, 0x18, 0x06, 0x00, 0x53, 0x0e, 0x4f, 0x02, 0xf9, 0x2f, 0xb8, 0x2e, 0xa9, 0x52, + 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, + 0x0f, 0xb8, 0xab, 0x52, 0x53, 0x3c, 0x52, 0x40, 0x40, 0x40, 0x4b, 0x00, 0x82, 0x16, + 0x26, 0xb9, 0x01, 0xb8, 0x41, 0x40, 0x10, 0x08, 0x97, 0xb8, 0x01, 0x08, 0xc0, 0x2e, + 0x11, 0x30, 0x01, 0x08, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, 0x0b, + 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x9f, 0x50, + 0x10, 0x50, 0xad, 0x52, 0x05, 0x2e, 0xd3, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, + 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xa5, 0xb7, 0x98, 0x2e, 0x87, 0xcf, + 0x01, 0x2e, 0xd9, 0x00, 0x00, 0xb2, 0xfb, 0x6f, 0x0b, 0x2f, 0x01, 0x2e, 0x69, 0xf7, + 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd9, 0x00, 0x21, 0x2e, + 0x69, 0xf7, 0x80, 0x2e, 0x7a, 0xb7, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xc0, 0xf8, + 0x03, 0x2e, 0xfc, 0xf5, 0x15, 0x54, 0xaf, 0x56, 0x82, 0x08, 0x0b, 0x2e, 0x69, 0xf7, + 0xcb, 0x0a, 0xb1, 0x58, 0x80, 0x90, 0xdd, 0xbe, 0x4c, 0x08, 0x5f, 0xb9, 0x59, 0x22, + 0x80, 0x90, 0x07, 0x2f, 0x03, 0x34, 0xc3, 0x08, 0xf2, 0x3a, 0x0a, 0x08, 0x02, 0x35, + 0xc0, 0x90, 0x4a, 0x0a, 0x48, 0x22, 0xc0, 0x2e, 0x23, 0x2e, 0xfc, 0xf5, 0x10, 0x50, + 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x10, 0x30, 0xfb, 0x6f, + 0xf0, 0x5f, 0x21, 0x2e, 0xcc, 0x00, 0x21, 0x2e, 0xca, 0x00, 0xb8, 0x2e, 0x03, 0x2e, + 0xd3, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, + 0x23, 0x2e, 0xd3, 0x00, 0x03, 0xbc, 0x21, 0x2e, 0xd5, 0x00, 0x03, 0x2e, 0xd5, 0x00, + 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x05, 0x2f, 0x05, 0x2e, + 0xd8, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, + 0xd9, 0x00, 0x11, 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, + 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, + 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x7c, 0x00, + 0xfb, 0x7f, 0x98, 0x2e, 0xc3, 0xb7, 0x40, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0xfb, 0x6f, + 0xf0, 0x5f, 0x03, 0x25, 0x80, 0x2e, 0xaf, 0xb7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, + 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, 0x00, 0x30, 0x41, 0x04, + 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, + 0x0d, 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0xb3, 0x58, 0x02, 0x2f, 0x90, 0xa1, + 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, + 0x00, 0x30, 0xb8, 0x2e, 0xb5, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, 0xb6, 0x0d, 0x17, + 0xc6, 0xbd, 0x56, 0xbc, 0xb7, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, + 0x05, 0x30, 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, + 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, + 0x80, 0x90, 0x02, 0x2f, 0x2d, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, 0x04, 0x2f, + 0xbf, 0x90, 0x06, 0x2f, 0xb7, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, + 0xb7, 0x52, 0x2d, 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, + 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0xbf, 0x56, 0xb9, 0x54, 0xd0, 0x40, 0xc4, 0x40, + 0x0b, 0x2e, 0xfd, 0xf3, 0xbf, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, 0x30, + 0xc1, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, + 0x73, 0x30, 0x0d, 0x2e, 0xd8, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, + 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, 0x2e, 0x77, 0xf7, 0xbd, 0x56, + 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0xbb, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, + 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, + 0x43, 0x40, 0xd4, 0x3f, 0xdc, 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, + 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, 0xfd, 0xf3, 0x4a, 0x0a, + 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, + 0x1a, 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0xc3, 0x52, 0x41, 0x84, + 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, + 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, 0x40, 0x8e, 0x17, + 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, + 0xb8, 0x2e, 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, + 0x0f, 0x0e, 0xcb, 0x58, 0x32, 0x87, 0xc4, 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0xc5, 0x5a, + 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, 0xb6, 0x7f, + 0x94, 0x7f, 0x17, 0x30, 0xc7, 0x52, 0xc9, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, + 0x42, 0x7f, 0x00, 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, + 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, + 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, 0x08, + 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, + 0xa1, 0xb4, 0x52, 0x41, 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, + 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, 0x6f, 0x75, 0x6f, 0x83, 0x40, + 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, + 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, + 0x31, 0x6f, 0x0b, 0x30, 0x04, 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, + 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, 0xc3, 0x7f, 0x03, 0x2f, + 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, + 0x98, 0x2e, 0x79, 0xca, 0x55, 0x6f, 0xb7, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, + 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, + 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, 0x30, 0x42, 0x6f, + 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, + 0x00, 0x2e, 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0xd1, 0x56, 0x63, 0x0e, + 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, 0x2f, 0x09, 0x2e, 0xd8, 0x00, 0x01, 0xb3, + 0x21, 0x2f, 0xcb, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, 0x00, 0x2e, + 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, + 0xd0, 0x7f, 0xcb, 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, + 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, + 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, 0x2d, + 0xcd, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, + 0xb5, 0x7f, 0xe4, 0x7f, 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, + 0x04, 0x32, 0xcf, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x15, 0x54, 0x09, 0x2e, 0x77, 0xf7, + 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, + 0x01, 0x2e, 0xd4, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, + 0x02, 0xb2, 0x42, 0x2f, 0x03, 0x90, 0x56, 0x2f, 0xd7, 0x52, 0x79, 0x80, 0x42, 0x40, + 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, 0xd9, 0x54, 0xd7, 0x50, + 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, + 0xe3, 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, + 0x41, 0x40, 0x15, 0x54, 0x4a, 0x0e, 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, + 0x21, 0x2e, 0x85, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, 0x0c, 0x98, 0x2e, + 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, + 0xce, 0xb7, 0xdd, 0x52, 0xd3, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0xdb, 0x52, + 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, 0x30, 0x27, 0x2e, 0xd7, 0x00, 0x37, 0x2e, + 0xd4, 0x00, 0x21, 0x2e, 0xd6, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, 0x30, 0x30, + 0x21, 0x2e, 0xd4, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0xd4, 0x00, + 0x21, 0x2e, 0x7b, 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0xd5, 0x50, + 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0xd4, 0x00, 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, + 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, 0x86, + 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xb5, 0x56, 0xa5, 0x6f, + 0xab, 0x08, 0x91, 0x6f, 0x4b, 0x08, 0xdf, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, + 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0xcb, 0x52, 0xe1, 0x5e, 0x56, 0x42, + 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, + 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, + 0x90, 0x5f, 0xd1, 0x50, 0x03, 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, + 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, 0x08, 0xb6, 0x89, 0x16, + 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, + 0x01, 0x2e, 0xd4, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, + 0x23, 0x2e, 0xd6, 0x00, 0x21, 0x2e, 0xd7, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0xd7, 0x00, + 0x03, 0x2e, 0xd6, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, 0x0e, 0xb8, 0x2e, + 0xe3, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, + 0x01, 0x00, 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0xe3, 0x54, + 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0xe5, 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, + 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, 0x40, 0x42, + 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, + 0xdd, 0x52, 0x00, 0x30, 0x40, 0x42, 0x7c, 0x86, 0xb9, 0x52, 0x09, 0x2e, 0x70, 0x0f, + 0xbf, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, + 0x21, 0x2e, 0xd7, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, 0x82, + 0x05, 0x2e, 0x7d, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x89, 0x00, 0x27, 0xbd, + 0x2f, 0xb9, 0x80, 0x90, 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, + 0x71, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, 0xf7, 0xbd, 0x56, 0x47, 0xbe, + 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0xe7, 0x54, 0x50, 0x42, 0x4a, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0xe5, 0x50, 0xfb, 0x7f, + 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, + 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, 0x14, 0x30, 0xb1, 0x6f, + 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, + 0xeb, 0x2f, 0x03, 0x2e, 0x86, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, + 0x86, 0x0f, 0x02, 0x2c, 0x00, 0x30, 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, + 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, 0x33, 0x22, 0x30, + 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x47, 0x58, 0xc2, 0x6f, + 0x94, 0x09, 0xeb, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xe9, 0x5a, + 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, + 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, 0xf5, 0x50, + 0x31, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, 0x42, 0x11, 0x30, 0x02, 0x80, + 0x23, 0x33, 0x01, 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0xd3, 0x00, + 0x23, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, + 0x08, 0x0a, 0x1a, 0x25, 0x7b, 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, 0x2e, + 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, 0xe3, 0x6f, 0x01, 0x2e, + 0x80, 0x03, 0x51, 0x30, 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, + 0x98, 0x2e, 0xa5, 0xb7, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, + 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, + 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, + 0x5d, 0xc0, 0xed, 0x50, 0x98, 0x2e, 0x44, 0xcb, 0xef, 0x50, 0x98, 0x2e, 0x46, 0xc3, + 0xf1, 0x50, 0x98, 0x2e, 0x53, 0xc7, 0x35, 0x50, 0x98, 0x2e, 0x64, 0xcf, 0x10, 0x30, + 0x98, 0x2e, 0xdc, 0x03, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0xab, 0x33, + 0x0b, 0x42, 0x37, 0x80, 0x01, 0x30, 0x01, 0x42, 0xf3, 0x37, 0xf7, 0x52, 0xfb, 0x50, + 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, 0x8b, 0x31, 0x09, 0x2e, 0x5e, 0xf7, 0xf9, 0x54, + 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, 0x84, 0x0b, 0x40, + 0x33, 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x58, 0xb7, + 0xd1, 0x6f, 0x80, 0x30, 0x40, 0x42, 0x03, 0x30, 0xe0, 0x6f, 0xf3, 0x54, 0x04, 0x30, + 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, + 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0xc1, 0x4a, 0x00, 0x00, 0x6d, 0x57, + 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, + 0xe5, 0xff, 0xff, 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, + 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1 }; } \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index e071db55a..16088994d 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -1,34 +1,33 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Tailsy13 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include #include +#include +#include -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 -struct ICM42688 -{ - static constexpr uint8_t Address = 0x68; - static constexpr auto Name = "ICM-42688"; - static constexpr auto Type = ImuID::ICM42688; - - static constexpr float GyrTs=1.0/500.0; - static constexpr float AccTs=1.0/100.0; - - static constexpr float MagTs=1.0/100; - - static constexpr float GyroSensitivity = 32.8f; - static constexpr float AccelSensitivity = 4096.0f; - - I2CImpl i2c; - SlimeVR::Logging::Logger &logger; - ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x75; - static constexpr uint8_t value = 0x47; - }; - static constexpr uint8_t TempData = 0x1d; - - struct DeviceConfig { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t valueSwReset = 1; - }; - struct IntfConfig0 { - 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 - }; - struct FifoConfig0 { - static constexpr uint8_t reg = 0x16; - static constexpr uint8_t value = (0b01 << 6); //stream to FIFO mode - }; - struct FifoConfig1 { - 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 GyroConfig { - static constexpr uint8_t reg = 0x4f; - static constexpr uint8_t value = (0b001 << 5) | 0b1111; //1000dps, odr=500Hz - }; - struct AccelConfig { - 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 - //GYRO_CONFIG1 - //GYRO_ACCEL_CONFIG0 - //ACCEL_CONFIG1 - - static constexpr uint8_t FifoCount = 0x2e; - static constexpr uint8_t FifoData = 0x30; - }; - - #pragma pack(push, 1) - struct FifoEntryAligned { - union { - struct { - int16_t accel[3]; - int16_t gyro[3]; - uint8_t temp; - uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned - } part; - uint8_t raw[15]; - }; - }; - #pragma pack(pop) - - static constexpr size_t FullFifoEntrySize = 16; - - bool initialize() - { - // perform initialization step - i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); - delay(20); - - i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value); - i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); - i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); - i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); - i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); - i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value); - delay(1); - - return true; - } - - float getDirectTemp() const - { - const auto value = static_cast(i2c.readReg16(Regs::TempData)); - float result = ((float)value / 132.48f) + 25.0f; - return result; - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); - - std::array read_buffer; // max 8 readings - const auto bytes_to_read = std::min(static_cast(read_buffer.size()), - static_cast(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); - for (auto i=0u; i(i2c.readReg16(Regs::TempData)); + float result = ((float)value / 132.48f) + 25.0f; + return result; + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(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 \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 2615cfb12..382103b9f 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -1,99 +1,107 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Gorbit99 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include #include +#include +#include -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { template -struct LSM6DSOutputHandler -{ - LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger) - {} - - I2CImpl i2c; - SlimeVR::Logging::Logger &logger; - - template - float getDirectTemp() const - { - const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); - float result = ((float)value / 256.0f) + 25.0f; - - return result; - } - - #pragma pack(push, 1) - struct FifoEntryAligned { - union { - int16_t xyz[3]; - uint8_t raw[6]; - }; - }; - #pragma pack(pop) - - static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; - - template - void bulkRead(AccelCall &processAccelSample, GyroCall &processGyroSample, float GyrTs, float AccTs) { - constexpr auto FIFO_SAMPLES_MASK = 0x3ff; - constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800; - - const auto fifo_status = i2c.readReg16(Regs::FifoStatus); - const auto available_axes = fifo_status & FIFO_SAMPLES_MASK; - const auto fifo_bytes = available_axes * FullFifoEntrySize; - if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) { - // FIFO overrun is expected to happen during startup and calibration - logger.error("FIFO OVERRUN! This occuring during normal usage is an issue."); - } - - std::array read_buffer; // max 8 readings - const auto bytes_to_read = std::min(static_cast(read_buffer.size()), - static_cast(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); - for (auto i=0u; 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; - } - } - } - - +struct LSM6DSOutputHandler { + LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + + template + float getDirectTemp() const { + const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); + float result = ((float)value / 256.0f) + 25.0f; + + return result; + } + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + int16_t xyz[3]; + uint8_t raw[6]; + }; + }; +#pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + template + void bulkRead( + AccelCall& processAccelSample, + GyroCall& processGyroSample, + float GyrTs, + float AccTs + ) { + constexpr auto FIFO_SAMPLES_MASK = 0x3ff; + constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800; + + const auto fifo_status = i2c.readReg16(Regs::FifoStatus); + const auto available_axes = fifo_status & FIFO_SAMPLES_MASK; + const auto fifo_bytes = available_axes * FullFifoEntrySize; + if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) { + // FIFO overrun is expected to happen during startup and calibration + logger.error("FIFO OVERRUN! This occuring during normal usage is an issue." + ); + } + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(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 \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 785a5e325..75f036e4a 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -1,139 +1,152 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Tailsy13 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include #include +#include +#include -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 -struct LSM6DS3TRC -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DS3TR-C"; - static constexpr auto Type = ImuID::LSM6DS3TRC; - - static constexpr float Freq = 416; - - static constexpr float GyrTs=1.0/Freq; - static constexpr float AccTs=1.0/Freq; - static constexpr float MagTs=1.0/Freq; - - static constexpr float GyroSensitivity = 28.571428571f; - static constexpr float AccelSensitivity = 4098.360655738f; - - I2CImpl i2c; - SlimeVR::Logging::Logger logger; - LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x0f; - static constexpr uint8_t value = 0x6a; - }; - static constexpr uint8_t OutTemp = 0x20; - struct Ctrl1XL { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4); //8g, 416Hz - }; - struct Ctrl2G { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t value = (0b10 << 2) | (0b0110 << 4); //1000dps, 416Hz - }; - struct Ctrl3C { - static constexpr uint8_t reg = 0x12; - static constexpr uint8_t valueSwReset = 1; - static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 - }; - struct FifoCtrl3 { - static constexpr uint8_t reg = 0x08; - static constexpr uint8_t value = 0b001 | (0b001 << 3); //accel no decimation, gyro no decimation - }; - 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 FifoData = 0x3e; - }; - - bool initialize() - { - // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); - delay(20); - i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); - i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value); - i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); - return true; - } - - float getDirectTemp() const - { - const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); - float result = ((float)value / 256.0f) + 25.0f; - - return result; - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - const auto read_result = i2c.readReg16(Regs::FifoStatus); - if (read_result & 0x4000) { // overrun! - // disable and re-enable fifo to clear it - logger.debug("Fifo overrun, resetting..."); - i2c.writeReg(Regs::FifoCtrl5::reg, 0); - i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); - return; - } - const auto unread_entries = read_result & 0x7ff; - constexpr auto single_measurement_words = 6; - constexpr auto single_measurement_bytes = sizeof(uint16_t) * single_measurement_words; - - std::array read_buffer; // max 10 packages of 6 16bit values of data form fifo - const auto bytes_to_read = std::min(static_cast(read_buffer.size()), static_cast(unread_entries)) \ - * sizeof(uint16_t) / single_measurement_bytes * single_measurement_bytes; - - i2c.readBytes(Regs::FifoData, bytes_to_read, reinterpret_cast(read_buffer.data())); - for (uint16_t i=0; i(&read_buffer[i]), GyrTs); - processAccelSample(reinterpret_cast(&read_buffer[i+3]), AccTs); - } - } - - +struct LSM6DS3TRC { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DS3TR-C"; + static constexpr auto Type = ImuID::LSM6DS3TRC; + + static constexpr float Freq = 416; + + static constexpr float GyrTs = 1.0 / Freq; + static constexpr float AccTs = 1.0 / Freq; + static constexpr float MagTs = 1.0 / Freq; + + static constexpr float GyroSensitivity = 28.571428571f; + static constexpr float AccelSensitivity = 4098.360655738f; + + I2CImpl i2c; + SlimeVR::Logging::Logger logger; + LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6a; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4); // 8g, 416Hz + }; + struct Ctrl2G { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value + = (0b10 << 2) | (0b0110 << 4); // 1000dps, 416Hz + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); // BDU = 1, IF_INC = + // 1 + }; + struct FifoCtrl3 { + static constexpr uint8_t reg = 0x08; + static constexpr uint8_t value + = 0b001 | (0b001 << 3); // accel no decimation, gyro no decimation + }; + 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 FifoData = 0x3e; + }; + + bool initialize() { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value); + i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + return true; + } + + float getDirectTemp() const { + const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); + float result = ((float)value / 256.0f) + 25.0f; + + return result; + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + const auto read_result = i2c.readReg16(Regs::FifoStatus); + if (read_result & 0x4000) { // overrun! + // disable and re-enable fifo to clear it + logger.debug("Fifo overrun, resetting..."); + i2c.writeReg(Regs::FifoCtrl5::reg, 0); + i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + return; + } + const auto unread_entries = read_result & 0x7ff; + constexpr auto single_measurement_words = 6; + constexpr auto single_measurement_bytes + = sizeof(uint16_t) * single_measurement_words; + + std::array + read_buffer; // max 10 packages of 6 16bit values of data form fifo + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(unread_entries) + ) + * sizeof(uint16_t) / single_measurement_bytes + * single_measurement_bytes; + + i2c.readBytes( + Regs::FifoData, + bytes_to_read, + reinterpret_cast(read_buffer.data()) + ); + for (uint16_t i = 0; i < bytes_to_read / sizeof(uint16_t); + i += single_measurement_words) { + processGyroSample(reinterpret_cast(&read_buffer[i]), GyrTs); + processAccelSample( + reinterpret_cast(&read_buffer[i + 3]), + AccTs + ); + } + } }; -} // namespace \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 0dc4870f8..7c26c85bc 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -1,120 +1,121 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Gorbit99 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include #include +#include +#include #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 -struct LSM6DSO : LSM6DSOutputHandler -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DSO"; - static constexpr auto Type = ImuID::LSM6DSO; - - static constexpr float GyrFreq = 416; - static constexpr float AccFreq = 104; - static constexpr float MagFreq = 120; - - static constexpr float GyrTs=1.0/GyrFreq; - static constexpr float AccTs=1.0/AccFreq; - static constexpr float MagTs=1.0/MagFreq; - - static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.244f; - - using LSM6DSOutputHandler::i2c; - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x0f; - static constexpr uint8_t value = 0x6c; - }; - static constexpr uint8_t OutTemp = 0x20; - struct Ctrl1XL { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS - }; - struct Ctrl2GY { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS - }; - struct Ctrl3C { - static constexpr uint8_t reg = 0x12; - static constexpr uint8_t valueSwReset = 1; - static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 - }; - struct FifoCtrl3BDR { - static constexpr uint8_t reg = 0x09; - static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz - }; - 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 FifoData = 0x78; - }; - - LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : LSM6DSOutputHandler(i2c, logger) { - } - - bool initialize() - { - // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); - delay(20); - i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); - i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); - return true; - } - - float getDirectTemp() const - { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); - } - +struct LSM6DSO : LSM6DSOutputHandler { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSO"; + static constexpr auto Type = ImuID::LSM6DSO; + + static constexpr float GyrFreq = 416; + static constexpr float AccFreq = 104; + static constexpr float MagFreq = 120; + + static constexpr float GyrTs = 1.0 / GyrFreq; + static constexpr float AccTs = 1.0 / AccFreq; + static constexpr float MagTs = 1.0 / MagFreq; + + static constexpr float GyroSensitivity = 1000 / 35.0f; + static constexpr float AccelSensitivity = 1000 / 0.244f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6c; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS + }; + struct Ctrl2GY { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b01101000); // GY at 416 Hz, 1000dps FS + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); // BDU = 1, IF_INC = + // 1 + }; + struct FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + static constexpr uint8_t value + = (0b0110) | (0b0110 << 4); // gyro and accel batched at 417Hz + }; + 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 FifoData = 0x78; + }; + + LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(i2c, logger) {} + + bool initialize() { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + float getDirectTemp() const { + return LSM6DSOutputHandler::template getDirectTemp(); + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + LSM6DSOutputHandler::template bulkRead( + processAccelSample, + processGyroSample, + GyrTs, + AccTs + ); + } }; -} // namespace \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index ebef63ca2..248679807 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -1,120 +1,121 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Gorbit99 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include #include +#include +#include #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 -struct LSM6DSR : LSM6DSOutputHandler -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DSR"; - static constexpr auto Type = ImuID::LSM6DSR; - - static constexpr float GyrFreq = 416; - static constexpr float AccFreq = 104; - static constexpr float MagFreq = 120; - - static constexpr float GyrTs=1.0/GyrFreq; - static constexpr float AccTs=1.0/AccFreq; - static constexpr float MagTs=1.0/MagFreq; - - static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.244f; - - using LSM6DSOutputHandler::i2c; - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x0f; - static constexpr uint8_t value = 0x6b; - }; - static constexpr uint8_t OutTemp = 0x20; - struct Ctrl1XL { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS - }; - struct Ctrl2GY { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS - }; - struct Ctrl3C { - static constexpr uint8_t reg = 0x12; - static constexpr uint8_t valueSwReset = 1; - static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 - }; - struct FifoCtrl3BDR { - static constexpr uint8_t reg = 0x09; - static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz - }; - 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 FifoData = 0x78; - }; - - LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : LSM6DSOutputHandler(i2c, logger) { - } - - bool initialize() - { - // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); - delay(20); - i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); - i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); - return true; - } - - float getDirectTemp() const - { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); - } - +struct LSM6DSR : LSM6DSOutputHandler { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSR"; + static constexpr auto Type = ImuID::LSM6DSR; + + static constexpr float GyrFreq = 416; + static constexpr float AccFreq = 104; + static constexpr float MagFreq = 120; + + static constexpr float GyrTs = 1.0 / GyrFreq; + static constexpr float AccTs = 1.0 / AccFreq; + static constexpr float MagTs = 1.0 / MagFreq; + + static constexpr float GyroSensitivity = 1000 / 35.0f; + static constexpr float AccelSensitivity = 1000 / 0.244f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6b; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS + }; + struct Ctrl2GY { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b01101000); // GY at 416 Hz, 1000dps FS + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); // BDU = 1, IF_INC = + // 1 + }; + struct FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + static constexpr uint8_t value + = (0b0110) | (0b0110 << 4); // gyro and accel batched at 417Hz + }; + 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 FifoData = 0x78; + }; + + LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(i2c, logger) {} + + bool initialize() { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + float getDirectTemp() const { + return LSM6DSOutputHandler::template getDirectTemp(); + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + LSM6DSOutputHandler::template bulkRead( + processAccelSample, + processGyroSample, + GyrTs, + AccTs + ); + } }; -} // namespace \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index ef073ecf6..f0aa11ee7 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -1,135 +1,136 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Gorbit99 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include #include +#include +#include #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 -struct LSM6DSV : LSM6DSOutputHandler -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DSV"; - static constexpr auto Type = ImuID::LSM6DSV; - - static constexpr float GyrFreq = 480; - static constexpr float AccFreq = 120; - static constexpr float MagFreq = 120; - - static constexpr float GyrTs=1.0/GyrFreq; - static constexpr float AccTs=1.0/AccFreq; - static constexpr float MagTs=1.0/MagFreq; - - static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.244f; - - using LSM6DSOutputHandler::i2c; - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x0f; - static constexpr uint8_t value = 0x70; - }; - static constexpr uint8_t OutTemp = 0x20; - struct HAODRCFG { - static constexpr uint8_t reg = 0x62; - static constexpr uint8_t value = (0b00); //1st ODR table - }; - struct Ctrl1XLODR { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b0010110); //120Hz, HAODR - }; - struct Ctrl2GODR { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t value = (0b0011000); //480Hz, HAODR - }; - struct Ctrl3C { - static constexpr uint8_t reg = 0x12; - static constexpr uint8_t valueSwReset = 1; - static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 - }; - struct Ctrl6GFS { - static constexpr uint8_t reg = 0x15; - static constexpr uint8_t value = (0b0011); //1000dps - }; - struct Ctrl8XLFS { - static constexpr uint8_t reg = 0x17; - static constexpr uint8_t value = (0b10); //8g - }; - struct FifoCtrl3BDR { - static constexpr uint8_t reg = 0x09; - static constexpr uint8_t value = (0b1000) | (0b1000 << 4); //gyro and accel batched at 480Hz - }; - 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 FifoData = 0x78; - }; - - LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : LSM6DSOutputHandler(i2c, logger) { - } - - bool initialize() - { - // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); - delay(20); - i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value); - i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value); - i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value); - i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); - return true; - } - - float getDirectTemp() const - { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); - } - +struct LSM6DSV : LSM6DSOutputHandler { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSV"; + static constexpr auto Type = ImuID::LSM6DSV; + + static constexpr float GyrFreq = 480; + static constexpr float AccFreq = 120; + static constexpr float MagFreq = 120; + + static constexpr float GyrTs = 1.0 / GyrFreq; + static constexpr float AccTs = 1.0 / AccFreq; + static constexpr float MagTs = 1.0 / MagFreq; + + static constexpr float GyroSensitivity = 1000 / 35.0f; + static constexpr float AccelSensitivity = 1000 / 0.244f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x70; + }; + static constexpr uint8_t OutTemp = 0x20; + struct HAODRCFG { + static constexpr uint8_t reg = 0x62; + static constexpr uint8_t value = (0b00); // 1st ODR table + }; + struct Ctrl1XLODR { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b0010110); // 120Hz, HAODR + }; + struct Ctrl2GODR { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b0011000); // 480Hz, HAODR + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); // BDU = 1, IF_INC = + // 1 + }; + struct Ctrl6GFS { + static constexpr uint8_t reg = 0x15; + static constexpr uint8_t value = (0b0011); // 1000dps + }; + struct Ctrl8XLFS { + static constexpr uint8_t reg = 0x17; + static constexpr uint8_t value = (0b10); // 8g + }; + struct FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + static constexpr uint8_t value + = (0b1000) | (0b1000 << 4); // gyro and accel batched at 480Hz + }; + 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 FifoData = 0x78; + }; + + LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(i2c, logger) {} + + bool initialize() { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value); + i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value); + i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value); + i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + float getDirectTemp() const { + return LSM6DSOutputHandler::template getDirectTemp(); + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + LSM6DSOutputHandler::template bulkRead( + processAccelSample, + processGyroSample, + GyrTs, + AccTs + ); + } }; -} // namespace \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 6546784c8..910d85a92 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -1,185 +1,210 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 furrycoding & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 furrycoding & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include -#include -#include - #include +#include +#include +#include -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 -struct MPU6050 -{ - struct FifoSample { - uint8_t accel_x_h, accel_x_l; - uint8_t accel_y_h, accel_y_l; - uint8_t accel_z_h, accel_z_l; - - // We don't need temperature in FIFO - // uint8_t temp_h, temp_l; - - uint8_t gyro_x_h, gyro_x_l; - uint8_t gyro_y_h, gyro_y_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)) - - static constexpr uint8_t Address = 0x68; - static constexpr auto Name = "MPU-6050"; - static constexpr auto Type = ImuID::MPU6050; - - static constexpr float Freq = 250; - - static constexpr float GyrTs = 1.0 / Freq; - static constexpr float AccTs = 1.0 / Freq; - static constexpr float MagTs = 1.0 / Freq; - - static constexpr float GyroSensitivity = 32.8f; - static constexpr float AccelSensitivity = 4096.0f; - - I2CImpl i2c; - SlimeVR::Logging::Logger &logger; - MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x75; - static constexpr uint8_t value = 0x68; - }; - - struct UserCtrl { - static constexpr uint8_t reg = 0x6A; - static constexpr uint8_t fifoResetValue = (1 << MPU6050_USERCTRL_FIFO_EN_BIT) | (1 << MPU6050_USERCTRL_FIFO_RESET_BIT); - }; - - struct GyroConfig { - static constexpr uint8_t reg = 0x1b; - static constexpr uint8_t value = 0b10 << 3; // 1000dps - }; - - struct AccelConfig { - static constexpr uint8_t reg = 0x1c; - static constexpr uint8_t value = 0b10 << 3; // 8g - }; - - static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H; - - static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS; - - static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH; - static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W; - }; - - inline uint16_t byteSwap(uint16_t value) const { - // Swap bytes because MPU is big-endian - return (value >> 8) | (value << 8); - } - - void resetFIFO() { - i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue); - } - - bool initialize() - { - // Reset - i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1: reset with 100ms delay (also disables sleep) - delay(100); - i2c.writeReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x07); // full SIGNAL_PATH_RESET: with another 100ms delay - delay(100); - - // 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)) - - i2c.writeReg(MPU6050_RA_FIFO_EN, 0x78); // 0111 1000 FIFO_EN: All gyro axes + Accel - - resetFIFO(); - - return true; - } - - float getDirectTemp() const - { - auto value = byteSwap(i2c.readReg16(Regs::OutTemp)); - float result = (static_cast(value) / 340.0f) + 36.53f; - return result; - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - const auto status = i2c.readReg(Regs::IntStatus); - - if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { - // Overflows make it so we lose track of which packet is which - // This necessitates a reset - logger.debug("Fifo overrun, resetting..."); - resetFIFO(); - return; - } - - std::array readBuffer; // max 10 packages of 12byte values (sample) of data form fifo - auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount)); - - auto readBytes = min(static_cast(byteCount), readBuffer.size()) / sizeof(FifoSample) * sizeof(FifoSample); - if (!readBytes) { - return; - } - - i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data()); - for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) { - const FifoSample *sample = reinterpret_cast(&readBuffer[i]); - - int16_t xyz[3]; - - 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); - } - } - - +struct MPU6050 { + struct FifoSample { + uint8_t accel_x_h, accel_x_l; + uint8_t accel_y_h, accel_y_l; + uint8_t accel_z_h, accel_z_l; + + // We don't need temperature in FIFO + // uint8_t temp_h, temp_l; + + uint8_t gyro_x_h, gyro_x_l; + uint8_t gyro_y_h, gyro_y_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)) + + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "MPU-6050"; + static constexpr auto Type = ImuID::MPU6050; + + static constexpr float Freq = 250; + + static constexpr float GyrTs = 1.0 / Freq; + static constexpr float AccTs = 1.0 / Freq; + static constexpr float MagTs = 1.0 / Freq; + + static constexpr float GyroSensitivity = 32.8f; + static constexpr float AccelSensitivity = 4096.0f; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x75; + static constexpr uint8_t value = 0x68; + }; + + struct UserCtrl { + static constexpr uint8_t reg = 0x6A; + static constexpr uint8_t fifoResetValue + = (1 << MPU6050_USERCTRL_FIFO_EN_BIT) + | (1 << MPU6050_USERCTRL_FIFO_RESET_BIT); + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value = 0b10 << 3; // 1000dps + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value = 0b10 << 3; // 8g + }; + + static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H; + + static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS; + + static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH; + static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W; + }; + + inline uint16_t byteSwap(uint16_t value) const { + // Swap bytes because MPU is big-endian + return (value >> 8) | (value << 8); + } + + void resetFIFO() { + i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue); + } + + bool initialize() { + // Reset + i2c.writeReg( + MPU6050_RA_PWR_MGMT_1, + 0x80 + ); // PWR_MGMT_1: reset with 100ms delay (also disables sleep) + delay(100); + i2c.writeReg( + MPU6050_RA_SIGNAL_PATH_RESET, + 0x07 + ); // full SIGNAL_PATH_RESET: with another 100ms delay + delay(100); + + // 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)) + + i2c.writeReg( + MPU6050_RA_FIFO_EN, + 0x78 + ); // 0111 1000 FIFO_EN: All gyro axes + Accel + + resetFIFO(); + + return true; + } + + float getDirectTemp() const { + auto value = byteSwap(i2c.readReg16(Regs::OutTemp)); + float result = (static_cast(value) / 340.0f) + 36.53f; + return result; + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + const auto status = i2c.readReg(Regs::IntStatus); + + if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { + // Overflows make it so we lose track of which packet is which + // This necessitates a reset + logger.debug("Fifo overrun, resetting..."); + resetFIFO(); + return; + } + + std::array + readBuffer; // max 10 packages of 12byte values (sample) of data form fifo + auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount)); + + auto readBytes = min(static_cast(byteCount), readBuffer.size()) + / sizeof(FifoSample) * sizeof(FifoSample); + if (!readBytes) { + return; + } + + i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data()); + for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) { + const FifoSample* sample = reinterpret_cast(&readBuffer[i]); + + int16_t xyz[3]; + + 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 diff --git a/src/sensors/softfusion/i2cimpl.h b/src/sensors/softfusion/i2cimpl.h index 7bc892301..80ac421de 100644 --- a/src/sensors/softfusion/i2cimpl.h +++ b/src/sensors/softfusion/i2cimpl.h @@ -1,72 +1,80 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Tailsy13 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once #include -#include "I2Cdev.h" - - -namespace SlimeVR::Sensors::SoftFusion -{ - -struct I2CImpl -{ - static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2; - - I2CImpl(uint8_t devAddr) - : m_devAddr(devAddr) {} - - uint8_t readReg(uint8_t regAddr) const { - uint8_t buffer = 0; - I2Cdev::readByte(m_devAddr, regAddr, &buffer); - return buffer; - } - uint16_t readReg16(uint8_t regAddr) const { - uint16_t buffer = 0; - I2Cdev::readBytes(m_devAddr, regAddr, sizeof(buffer), reinterpret_cast(&buffer)); - return buffer; - } - - void writeReg(uint8_t regAddr, uint8_t value) const { - I2Cdev::writeByte(m_devAddr, regAddr, value); - } - - void writeReg16(uint8_t regAddr, uint16_t value) const { - I2Cdev::writeBytes(m_devAddr, regAddr, sizeof(value), reinterpret_cast(&value)); - } - - void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { - I2Cdev::readBytes(m_devAddr, regAddr, size, buffer); - } - - void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { - I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer); - } +#include "I2Cdev.h" - private: - uint8_t m_devAddr; +namespace SlimeVR::Sensors::SoftFusion { + +struct I2CImpl { + static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2; + + I2CImpl(uint8_t devAddr) + : m_devAddr(devAddr) {} + + uint8_t readReg(uint8_t regAddr) const { + uint8_t buffer = 0; + I2Cdev::readByte(m_devAddr, regAddr, &buffer); + return buffer; + } + + uint16_t readReg16(uint8_t regAddr) const { + uint16_t buffer = 0; + I2Cdev::readBytes( + m_devAddr, + regAddr, + sizeof(buffer), + reinterpret_cast(&buffer) + ); + return buffer; + } + + void writeReg(uint8_t regAddr, uint8_t value) const { + I2Cdev::writeByte(m_devAddr, regAddr, value); + } + + void writeReg16(uint8_t regAddr, uint16_t value) const { + I2Cdev::writeBytes( + m_devAddr, + regAddr, + sizeof(value), + reinterpret_cast(&value) + ); + } + + void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { + I2Cdev::readBytes(m_devAddr, regAddr, size, buffer); + } + + void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { + I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer); + } + +private: + uint8_t m_devAddr; }; -} +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index a9dacbe4e..cb1709b86 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -1,533 +1,619 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Tailsy13 & SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #pragma once -#include "../sensor.h" #include "../SensorFusionRestDetect.h" - +#include "../sensor.h" #include "GlobalVars.h" -namespace SlimeVR::Sensors -{ - -template typename T, typename I2CImpl> -class SoftFusionSensor : public Sensor -{ - using imu = T; - using RawVectorT = std::array; - static constexpr auto UpsideDownCalibrationInit = true; - static constexpr auto GyroCalibDelaySeconds = 5; - static constexpr auto GyroCalibSeconds = 5; - static constexpr auto SampleRateCalibDelaySeconds = 1; - static constexpr auto SampleRateCalibSeconds = 5; - - static constexpr auto AccelCalibDelaySeconds = 3; - static constexpr auto AccelCalibRestSeconds = 3; - - static constexpr double GScale = ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0); - static constexpr double AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity; - - static constexpr bool HasMotionlessCalib = requires(imu& i){ typename imu::MotionlessCalibrationData; }; - static constexpr size_t MotionlessCalibDataSize() { - if constexpr(HasMotionlessCalib) { - return sizeof(typename imu::MotionlessCalibrationData); - } - else { - return 0; - } - } - - bool detected() const - { - const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg); - if (imu::Regs::WhoAmI::value != value) { - m_Logger.error("Sensor not detected, expected reg 0x%02x = 0x%02x but got 0x%02x", - imu::Regs::WhoAmI::reg, imu::Regs::WhoAmI::value, value); - return false; - } - - return true; - } - - - void sendTempIfNeeded() - { - uint32_t now = micros(); - constexpr float maxSendRateHz = 2.0f; - constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; - uint32_t elapsed = now - m_lastTemperaturePacketSent; - if (elapsed >= sendInterval) { - const float temperature = m_sensor.getDirectTemp(); - m_lastTemperaturePacketSent = now - (elapsed - sendInterval); - networkConnection.sendTemperature(sensorId, temperature); - } - } - - void recalcFusion() - { - m_fusion = SensorFusionRestDetect(m_calibration.G_Ts, m_calibration.A_Ts, m_calibration.M_Ts); - } - - void processAccelSample(const int16_t xyz[3], const sensor_real_t timeDelta) - { - sensor_real_t accelData[] = { - static_cast(xyz[0]), - static_cast(xyz[1]), - static_cast(xyz[2]) }; - - float tmp[3]; - for (uint8_t i = 0; i < 3; i++) - tmp[i] = (accelData[i] - m_calibration.A_B[i]); - - accelData[0] = (m_calibration.A_Ainv[0][0] * tmp[0] + m_calibration.A_Ainv[0][1] * tmp[1] + m_calibration.A_Ainv[0][2] * tmp[2]) * AScale; - accelData[1] = (m_calibration.A_Ainv[1][0] * tmp[0] + m_calibration.A_Ainv[1][1] * tmp[1] + m_calibration.A_Ainv[1][2] * tmp[2]) * AScale; - accelData[2] = (m_calibration.A_Ainv[2][0] * tmp[0] + m_calibration.A_Ainv[2][1] * tmp[1] + m_calibration.A_Ainv[2][2] * tmp[2]) * AScale; - - m_fusion.updateAcc(accelData, m_calibration.A_Ts); - } - - void processGyroSample(const int16_t xyz[3], const sensor_real_t timeDelta) - { - const sensor_real_t scaledData[] = { - static_cast(GScale * (static_cast(xyz[0]) - m_calibration.G_off[0])), - static_cast(GScale * (static_cast(xyz[1]) - m_calibration.G_off[1])), - static_cast(GScale * (static_cast(xyz[2]) - m_calibration.G_off[2]))}; - m_fusion.updateGyro(scaledData, m_calibration.G_Ts); - } - - void eatSamplesForSeconds(const uint32_t seconds) { - const auto targetDelay = millis() + 1000 * seconds; - auto lastSecondsRemaining = seconds; - while (millis() < targetDelay) - { - #ifdef ESP8266 - ESP.wdtFeed(); - #endif - auto currentSecondsRemaining = (targetDelay - millis()) / 1000; - if (currentSecondsRemaining != lastSecondsRemaining) { - m_Logger.info("%d...", currentSecondsRemaining + 1); - lastSecondsRemaining = currentSecondsRemaining; - } - m_sensor.bulkRead( - [](const int16_t xyz[3], const sensor_real_t timeDelta) { }, - [](const int16_t xyz[3], const sensor_real_t timeDelta) { } - ); - } - } - - std::pair eatSamplesReturnLast(const uint32_t milliseconds) - { - RawVectorT accel = {0}; - RawVectorT gyro = {0}; - const auto targetDelay = millis() + milliseconds; - while (millis() < targetDelay) - { - m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { - accel[0] = xyz[0]; - accel[1] = xyz[1]; - accel[2] = xyz[2]; - }, - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { - gyro[0] = xyz[0]; - gyro[1] = xyz[1]; - gyro[2] = xyz[2]; - } - ); - yield(); - } - return std::make_pair(accel, gyro); - } +namespace SlimeVR::Sensors { + +template