github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-05 17:51:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Enforce clang-format on the repo (#355)

Browse Source 
* Enforce clang-format on the repo

* improve CI config

* fix build error

* undo src format

* apply format by hand

* fix last one

* fix build error

* wat
This commit is contained in:
Uriel
2024-11-08 18:23:18 +01:00
committed by GitHub
parent 50fa801653
commit 0b882db74f
81 changed files with 10129 additions and 8571 deletions
Download Patch File Download Diff File Expand all files Collapse all files

200
.clang-tidy Normal file
View File

@@ -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/).)*$'

20
.github/workflows/actions.yml vendored
View File

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

2
src/GlobalVars.h
View File

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

367
src/LEDManager.cpp
View File

@@ -1,212 +1,183 @@
/*
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 "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();
}
void LEDManager::on()
{
#if ENABLE_LEDS
digitalWrite(m_Pin, LED__ON);
#endif
}
void LEDManager::off()
{
#if ENABLE_LEDS
digitalWrite(m_Pin, LED__OFF);
#endif
}
void LEDManager::blink(unsigned long time)
{
on();
delay(time);
off();
}
void LEDManager::pattern(unsigned long timeon, unsigned long timeoff, int times)
{
for (int i = 0; i < times; i++)
{
blink(timeon);
delay(timeoff);
}
}
void LEDManager::update()
{
unsigned long time = millis();
unsigned long diff = time - m_LastUpdate;
// Don't tick the LEDManager *too* often
if (diff < 10)
{
return;
}
m_LastUpdate = time;
unsigned int length = 0;
unsigned int count = 0;
if (statusManager.hasStatus(Status::LOW_BATTERY))
{
count = LOW_BATTERY_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = LOW_BATTERY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LOW_BATTERY_INTERVAL;
break;
}
}
else if (statusManager.hasStatus(Status::IMU_ERROR))
{
count = IMU_ERROR_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = IMU_ERROR_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = IMU_ERROR_INTERVAL;
break;
}
}
else if (statusManager.hasStatus(Status::WIFI_CONNECTING))
{
count = WIFI_CONNECTING_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = WIFI_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = WIFI_CONNECTING_INTERVAL;
break;
}
}
else if (statusManager.hasStatus(Status::SERVER_CONNECTING))
{
count = SERVER_CONNECTING_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = SERVER_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = SERVER_CONNECTING_INTERVAL;
break;
}
}
else
{
#if defined(LED_INTERVAL_STANDBY) && LED_INTERVAL_STANDBY > 0
count = 1;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = STANDBUY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LED_INTERVAL_STANDBY;
break;
}
#else
return;
#endif
}
if (m_CurrentStage == OFF || m_Timer + diff >= length)
{
m_Timer = 0;
// Advance stage
switch (m_CurrentStage)
{
case OFF:
on();
m_CurrentStage = ON;
m_CurrentCount = 0;
break;
case ON:
off();
m_CurrentCount++;
if (m_CurrentCount >= count)
{
m_CurrentCount = 0;
m_CurrentStage = INTERVAL;
}
else
{
m_CurrentStage = GAP;
}
break;
case GAP:
case INTERVAL:
on();
m_CurrentStage = ON;
break;
}
}
else
{
m_Timer += diff;
}
}
// Do the initial pull of the state
update();
}
void LEDManager::on() {
#if ENABLE_LEDS
digitalWrite(m_Pin, LED__ON);
#endif
}
void LEDManager::off() {
#if ENABLE_LEDS
digitalWrite(m_Pin, LED__OFF);
#endif
}
void LEDManager::blink(unsigned long time) {
on();
delay(time);
off();
}
void LEDManager::pattern(unsigned long timeon, unsigned long timeoff, int times) {
for (int i = 0; i < times; i++) {
blink(timeon);
delay(timeoff);
}
}
void LEDManager::update() {
unsigned long time = millis();
unsigned long diff = time - m_LastUpdate;
// Don't tick the LEDManager *too* often
if (diff < 10) {
return;
}
m_LastUpdate = time;
unsigned int length = 0;
unsigned int count = 0;
if (statusManager.hasStatus(Status::LOW_BATTERY)) {
count = LOW_BATTERY_COUNT;
switch (m_CurrentStage) {
case ON:
case OFF:
length = LOW_BATTERY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LOW_BATTERY_INTERVAL;
break;
}
} else if (statusManager.hasStatus(Status::IMU_ERROR)) {
count = IMU_ERROR_COUNT;
switch (m_CurrentStage) {
case ON:
case OFF:
length = IMU_ERROR_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = IMU_ERROR_INTERVAL;
break;
}
} else if (statusManager.hasStatus(Status::WIFI_CONNECTING)) {
count = WIFI_CONNECTING_COUNT;
switch (m_CurrentStage) {
case ON:
case OFF:
length = WIFI_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = WIFI_CONNECTING_INTERVAL;
break;
}
} else if (statusManager.hasStatus(Status::SERVER_CONNECTING)) {
count = SERVER_CONNECTING_COUNT;
switch (m_CurrentStage) {
case ON:
case OFF:
length = SERVER_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = SERVER_CONNECTING_INTERVAL;
break;
}
} else {
#if defined(LED_INTERVAL_STANDBY) && LED_INTERVAL_STANDBY > 0
count = 1;
switch (m_CurrentStage) {
case ON:
case OFF:
length = STANDBUY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LED_INTERVAL_STANDBY;
break;
}
#else
return;
#endif
}
if (m_CurrentStage == OFF || m_Timer + diff >= length) {
m_Timer = 0;
// Advance stage
switch (m_CurrentStage) {
case OFF:
on();
m_CurrentStage = ON;
m_CurrentCount = 0;
break;
case ON:
off();
m_CurrentCount++;
if (m_CurrentCount >= count) {
m_CurrentCount = 0;
m_CurrentStage = INTERVAL;
} else {
m_CurrentStage = GAP;
}
break;
case GAP:
case INTERVAL:
on();
m_CurrentStage = ON;
break;
}
} else {
m_Timer += diff;
}
}
} // namespace SlimeVR

116
src/LEDManager.h
View File

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

234
src/batterymonitor.cpp
View File

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

108
src/batterymonitor.h
View File

@@ -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 <Arduino.h>
#include "globals.h"
#include <i2cscan.h>
#include <I2Cdev.h>
#include <i2cscan.h>
#include "globals.h"
#include "logging/Logger.h"
#if ESP8266
#define ADCResolution 1023.0 // ESP8266 has 10bit ADC
#define 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_

36
src/calibration.h
View File

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

649
src/configuration/Configuration.cpp
View File

@@ -1,327 +1,384 @@
/*
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 "Configuration.h"
#include <LittleFS.h>
#include "Configuration.h"
#include "../FSHelper.h"
#include "consts.h"
#include "utils.h"
#include "../FSHelper.h"
#define DIR_CALIBRATIONS "/calibrations"
#define DIR_TEMPERATURE_CALIBRATIONS "/tempcalibrations"
namespace SlimeVR {
namespace Configuration {
void Configuration::setup() {
if (m_Loaded) {
return;
}
namespace Configuration {
void Configuration::setup() {
if (m_Loaded) {
return;
}
bool status = LittleFS.begin();
if (!status) {
this->m_Logger.warn("Could not mount LittleFS, formatting");
bool status = LittleFS.begin();
if (!status) {
this->m_Logger.warn("Could not mount LittleFS, formatting");
status = LittleFS.format();
if (!status) {
this->m_Logger.warn("Could not format LittleFS, aborting");
return;
}
status = LittleFS.format();
if (!status) {
this->m_Logger.warn("Could not format LittleFS, aborting");
return;
}
status = LittleFS.begin();
if (!status) {
this->m_Logger.error("Could not mount LittleFS, aborting");
return;
}
}
status = LittleFS.begin();
if (!status) {
this->m_Logger.error("Could not mount LittleFS, aborting");
return;
}
}
if (LittleFS.exists("/config.bin")) {
m_Logger.trace("Found configuration file");
if (LittleFS.exists("/config.bin")) {
m_Logger.trace("Found configuration file");
auto file = LittleFS.open("/config.bin", "r");
auto file = LittleFS.open("/config.bin", "r");
file.read((uint8_t*)&m_Config.version, sizeof(int32_t));
file.read((uint8_t*)&m_Config.version, sizeof(int32_t));
if (m_Config.version < CURRENT_CONFIGURATION_VERSION) {
m_Logger.debug("Configuration is outdated: v%d < v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION);
if (m_Config.version < CURRENT_CONFIGURATION_VERSION) {
m_Logger.debug(
"Configuration is outdated: v%d < v%d",
m_Config.version,
CURRENT_CONFIGURATION_VERSION
);
if (!runMigrations(m_Config.version)) {
m_Logger.error("Failed to migrate configuration from v%d to v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION);
return;
}
} else {
m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
}
if (!runMigrations(m_Config.version)) {
m_Logger.error(
"Failed to migrate configuration from v%d to v%d",
m_Config.version,
CURRENT_CONFIGURATION_VERSION
);
return;
}
} else {
m_Logger.info("Found up-to-date configuration v%d", m_Config.version);
}
file.seek(0);
file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
file.close();
} else {
m_Logger.info("No configuration file found, creating new one");
m_Config.version = CURRENT_CONFIGURATION_VERSION;
save();
}
file.seek(0);
file.read((uint8_t*)&m_Config, sizeof(DeviceConfig));
file.close();
} else {
m_Logger.info("No configuration file found, creating new one");
m_Config.version = CURRENT_CONFIGURATION_VERSION;
save();
}
loadSensors();
loadSensors();
m_Loaded = true;
m_Loaded = true;
m_Logger.info("Loaded configuration");
m_Logger.info("Loaded configuration");
#ifdef DEBUG_CONFIGURATION
print();
print();
#endif
}
void Configuration::save() {
for (size_t i = 0; i < m_Sensors.size(); i++) {
SensorConfig config = m_Sensors[i];
if (config.type == SensorConfigType::NONE) {
continue;
}
char path[17];
sprintf(path, DIR_CALIBRATIONS"/%d", i);
m_Logger.trace("Saving sensor config data for %d", i);
File file = LittleFS.open(path, "w");
file.write((uint8_t*)&config, sizeof(SensorConfig));
file.close();
}
{
File file = LittleFS.open("/config.bin", "w");
file.write((uint8_t*)&m_Config, sizeof(DeviceConfig));
file.close();
}
m_Logger.debug("Saved configuration");
}
void Configuration::reset() {
LittleFS.format();
m_Sensors.clear();
m_Config.version = 1;
save();
m_Logger.debug("Reset configuration");
}
int32_t Configuration::getVersion() const {
return m_Config.version;
}
size_t Configuration::getSensorCount() const {
return m_Sensors.size();
}
SensorConfig Configuration::getSensor(size_t sensorID) const {
if (sensorID >= m_Sensors.size()) {
return {};
}
return m_Sensors.at(sensorID);
}
void Configuration::setSensor(size_t sensorID, const SensorConfig& config) {
size_t currentSensors = m_Sensors.size();
if (sensorID >= currentSensors) {
m_Sensors.resize(sensorID + 1);
}
m_Sensors[sensorID] = config;
}
void Configuration::loadSensors() {
SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
SensorConfig sensorConfig;
f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig));
uint8_t sensorId = strtoul(f.name(), nullptr, 10);
m_Logger.debug(
"Found sensor calibration for %s at index %d",
calibrationConfigTypeToString(sensorConfig.type),
sensorId
);
setSensor(sensorId, sensorConfig);
});
}
bool Configuration::loadTemperatureCalibration(
uint8_t sensorId,
GyroTemperatureCalibrationConfig& config
) {
if (!SlimeVR::Utils::ensureDirectory(DIR_TEMPERATURE_CALIBRATIONS)) {
return false;
}
char path[32];
sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
if (!LittleFS.exists(path)) {
return false;
}
auto f = SlimeVR::Utils::openFile(path, "r");
if (f.isDirectory()) {
return false;
}
if (f.size() != sizeof(GyroTemperatureCalibrationConfig)) {
m_Logger.debug(
"Found incompatible sensor temperature calibration (size mismatch) "
"sensorId:%d, skipping",
sensorId
);
return false;
}
SensorConfigType storedConfigType;
f.read((uint8_t*)&storedConfigType, sizeof(SensorConfigType));
if (storedConfigType != config.type) {
m_Logger.debug(
"Found incompatible sensor temperature calibration (expected %s, "
"found %s) sensorId:%d, skipping",
calibrationConfigTypeToString(config.type),
calibrationConfigTypeToString(storedConfigType),
sensorId
);
return false;
}
f.seek(0);
f.read((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
m_Logger.debug(
"Found sensor temperature calibration for %s sensorId:%d",
calibrationConfigTypeToString(config.type),
sensorId
);
return true;
}
bool Configuration::saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config) {
if (config.type == SensorConfigType::NONE) {
return false;
}
char path[32];
sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId);
m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId);
File file = LittleFS.open(path, "w");
file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
file.close();
m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId);
return true;
}
bool Configuration::runMigrations(int32_t version) {
return true;
}
void Configuration::print() {
m_Logger.info("Configuration:");
m_Logger.info(" Version: %d", m_Config.version);
m_Logger.info(" %d Sensors:", m_Sensors.size());
for (size_t i = 0; i < m_Sensors.size(); i++) {
const SensorConfig& c = m_Sensors[i];
m_Logger.info(" - [%3d] %s", i, calibrationConfigTypeToString(c.type));
switch (c.type) {
case SensorConfigType::NONE:
break;
case SensorConfigType::BMI160:
m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B));
m_Logger.info(" A_Ainv :");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_Ainv[i]));
}
m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.G_off));
m_Logger.info(" Temperature: %f", c.data.bmi160.temperature);
break;
case SensorConfigType::SFUSION:
m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B));
m_Logger.info(" A_Ainv :");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_Ainv[i]));
}
m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.G_off));
m_Logger.info(" Temperature: %f", c.data.sfusion.temperature);
break;
case SensorConfigType::ICM20948:
m_Logger.info(" G: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.G));
m_Logger.info(" A: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.A));
m_Logger.info(" C: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.C));
break;
case SensorConfigType::MPU9250:
m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B));
m_Logger.info(" A_Ainv:");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_Ainv[i]));
}
m_Logger.info(" M_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B));
m_Logger.info(" M_Ainv:");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_Ainv[i]));
}
m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off));
break;
case SensorConfigType::MPU6050:
m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B));
m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off));
break;
case SensorConfigType::BNO0XX:
m_Logger.info(" magEnabled: %d", c.data.bno0XX.magEnabled);
break;
}
}
}
}
}
void Configuration::save() {
for (size_t i = 0; i < m_Sensors.size(); i++) {
SensorConfig config = m_Sensors[i];
if (config.type == SensorConfigType::NONE) {
continue;
}
char path[17];
sprintf(path, DIR_CALIBRATIONS "/%d", i);
m_Logger.trace("Saving sensor config data for %d", i);
File file = LittleFS.open(path, "w");
file.write((uint8_t*)&config, sizeof(SensorConfig));
file.close();
}
{
File file = LittleFS.open("/config.bin", "w");
file.write((uint8_t*)&m_Config, sizeof(DeviceConfig));
file.close();
}
m_Logger.debug("Saved configuration");
}
void Configuration::reset() {
LittleFS.format();
m_Sensors.clear();
m_Config.version = 1;
save();
m_Logger.debug("Reset configuration");
}
int32_t Configuration::getVersion() const { return m_Config.version; }
size_t Configuration::getSensorCount() const { return m_Sensors.size(); }
SensorConfig Configuration::getSensor(size_t sensorID) const {
if (sensorID >= m_Sensors.size()) {
return {};
}
return m_Sensors.at(sensorID);
}
void Configuration::setSensor(size_t sensorID, const SensorConfig& config) {
size_t currentSensors = m_Sensors.size();
if (sensorID >= currentSensors) {
m_Sensors.resize(sensorID + 1);
}
m_Sensors[sensorID] = config;
}
void Configuration::loadSensors() {
SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
SensorConfig sensorConfig;
f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig));
uint8_t sensorId = strtoul(f.name(), nullptr, 10);
m_Logger.debug(
"Found sensor calibration for %s at index %d",
calibrationConfigTypeToString(sensorConfig.type),
sensorId
);
setSensor(sensorId, sensorConfig);
});
}
bool Configuration::loadTemperatureCalibration(
uint8_t sensorId,
GyroTemperatureCalibrationConfig& config
) {
if (!SlimeVR::Utils::ensureDirectory(DIR_TEMPERATURE_CALIBRATIONS)) {
return false;
}
char path[32];
sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
if (!LittleFS.exists(path)) {
return false;
}
auto f = SlimeVR::Utils::openFile(path, "r");
if (f.isDirectory()) {
return false;
}
if (f.size() != sizeof(GyroTemperatureCalibrationConfig)) {
m_Logger.debug(
"Found incompatible sensor temperature calibration (size mismatch) "
"sensorId:%d, skipping",
sensorId
);
return false;
}
SensorConfigType storedConfigType;
f.read((uint8_t*)&storedConfigType, sizeof(SensorConfigType));
if (storedConfigType != config.type) {
m_Logger.debug(
"Found incompatible sensor temperature calibration (expected %s, "
"found %s) sensorId:%d, skipping",
calibrationConfigTypeToString(config.type),
calibrationConfigTypeToString(storedConfigType),
sensorId
);
return false;
}
f.seek(0);
f.read((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
m_Logger.debug(
"Found sensor temperature calibration for %s sensorId:%d",
calibrationConfigTypeToString(config.type),
sensorId
);
return true;
}
bool Configuration::saveTemperatureCalibration(
uint8_t sensorId,
const GyroTemperatureCalibrationConfig& config
) {
if (config.type == SensorConfigType::NONE) {
return false;
}
char path[32];
sprintf(path, DIR_TEMPERATURE_CALIBRATIONS "/%d", sensorId);
m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId);
File file = LittleFS.open(path, "w");
file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig));
file.close();
m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId);
return true;
}
bool Configuration::runMigrations(int32_t version) { return true; }
void Configuration::print() {
m_Logger.info("Configuration:");
m_Logger.info(" Version: %d", m_Config.version);
m_Logger.info(" %d Sensors:", m_Sensors.size());
for (size_t i = 0; i < m_Sensors.size(); i++) {
const SensorConfig& c = m_Sensors[i];
m_Logger.info(" - [%3d] %s", i, calibrationConfigTypeToString(c.type));
switch (c.type) {
case SensorConfigType::NONE:
break;
case SensorConfigType::BMI160:
m_Logger.info(
" A_B : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B)
);
m_Logger.info(" A_Ainv :");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(
" %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.bmi160.A_Ainv[i])
);
}
m_Logger.info(
" G_off : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.bmi160.G_off)
);
m_Logger.info(" Temperature: %f", c.data.bmi160.temperature);
break;
case SensorConfigType::SFUSION:
m_Logger.info(
" A_B : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B)
);
m_Logger.info(" A_Ainv :");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(
" %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.sfusion.A_Ainv[i])
);
}
m_Logger.info(
" G_off : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.sfusion.G_off)
);
m_Logger.info(
" Temperature: %f",
c.data.sfusion.temperature
);
break;
case SensorConfigType::ICM20948:
m_Logger.info(
" G: %d, %d, %d",
UNPACK_VECTOR_ARRAY(c.data.icm20948.G)
);
m_Logger.info(
" A: %d, %d, %d",
UNPACK_VECTOR_ARRAY(c.data.icm20948.A)
);
m_Logger.info(
" C: %d, %d, %d",
UNPACK_VECTOR_ARRAY(c.data.icm20948.C)
);
break;
case SensorConfigType::MPU9250:
m_Logger.info(
" A_B : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B)
);
m_Logger.info(" A_Ainv:");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(
" %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_Ainv[i])
);
}
m_Logger.info(
" M_B : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B)
);
m_Logger.info(" M_Ainv:");
for (uint8_t i = 0; i < 3; i++) {
m_Logger.info(
" %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_Ainv[i])
);
}
m_Logger.info(
" G_off : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off)
);
break;
case SensorConfigType::MPU6050:
m_Logger.info(
" A_B : %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)
);
m_Logger.info(
" G_off: %f, %f, %f",
UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)
);
break;
case SensorConfigType::BNO0XX:
m_Logger.info(" magEnabled: %d", c.data.bno0XX.magEnabled);
break;
}
}
}
} // namespace Configuration
} // namespace SlimeVR

88
src/configuration/Configuration.h
View File

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

125
src/consts.h
View File

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

39
src/credentials.h
View File

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

73
src/debug.h
View File

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

293
src/defines.h
View File

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

49
src/defines_bmi160.h
View File

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

80
src/defines_sensitivity.h
View File

@@ -1,24 +1,24 @@
/*
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 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

73
src/globals.h
View File

@@ -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 <Arduino.h>
#include "consts.h"
#include "debug.h"
#include "defines.h"
@@ -42,29 +43,29 @@
#define BATTERY_MONITOR BAT_INTERNAL
#endif
// If LED_PIN is not defined in "defines.h" take the default pin from "pins_arduino.h" framework.
// If 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
// 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
#define ENABLE_LEDS true
#endif
#else
// LED_PIN is not defined
#if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0)
#define LED_PIN LED_BUILTIN
#define ENABLE_LEDS true
#else
#define LED_PIN LED_OFF
#define ENABLE_LEDS false
#endif
#endif
#if !defined(LED_INVERTED)
// default is inverted for SlimeVR / ESP-12E
#define LED_INVERTED true
// 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_

46
src/logging/Level.cpp
View File

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

29
src/logging/Level.h
View File

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

146
src/logging/Logger.cpp
View File

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

161
src/logging/Logger.h
View File

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

183
src/main.cpp
View File

@@ -1,35 +1,36 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "Wire.h"
#include "ota.h"
#include "GlobalVars.h"
#include "globals.h"
#include "credentials.h"
#include <i2cscan.h>
#include "serial/serialcommands.h"
#include "GlobalVars.h"
#include "Wire.h"
#include "batterymonitor.h"
#include "credentials.h"
#include "globals.h"
#include "logging/Logger.h"
#include "ota.h"
#include "serial/serialcommands.h"
Timer<> globalTimer;
SlimeVR::Logging::Logger logger("SlimeVR");
@@ -48,99 +49,101 @@ unsigned long lastStatePrint = 0;
bool secondImuActive = false;
BatteryMonitor battery;
void setup()
{
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<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
// using `static_cast` here seems to be better, because there are 2 similar function
// signatures
Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
#ifdef ESP8266
Wire.setClockStretchLimit(150000L); // Default stretch limit 150mS
Wire.setClockStretchLimit(150000L); // Default stretch limit 150mS
#endif
#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
}

389
src/motionprocessing/GyroTemperatureCalibrator.cpp
View File

@@ -1,211 +1,244 @@
/*
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.
*/
#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();
}
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;
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++;
}
}
bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
if (currentTempAlreadyCalibrated) return;
const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(temperature);
if (state.temperatureCurrentIdx != idx) {
state.temperatureCurrentIdx = idx;
resetCurrentTemperatureState();
}
if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) {
return;
}
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();
}
bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f;
if (currentTempAlreadyCalibrated) {
return;
}
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);
if (state.temperatureCurrentIdx != idx) {
state.temperatureCurrentIdx = idx;
resetCurrentTemperatureState();
}
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();
}
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;
}
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 float constrainedTemperature = constrain(
temperature,
config.minTemperatureRange,
config.maxTemperatureRange
);
const int16_t idx =
TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature);
const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature);
if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) return false;
if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) {
return false;
}
bool isCurrentTempCalibrated = config.samples[idx].t != 0.0f;
if (isCurrentTempCalibrated) {
GOxyz[0] = config.samples[idx].x;
GOxyz[1] = config.samples[idx].y;
GOxyz[2] = config.samples[idx].z;
return true;
}
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;
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;
}

306
src/motionprocessing/GyroTemperatureCalibrator.h
View File

@@ -1,24 +1,24 @@
/*
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 GYRO_TEMPERATURE_CALIBRATOR_H
@@ -26,11 +26,11 @@
#include <Arduino.h>
#include <stdint.h>
#include "debug.h"
#include "../logging/Logger.h"
#include "../configuration/SensorConfig.h"
#include "OnlinePolyfit.h"
#include "../configuration/SensorConfig.h"
#include "../logging/Logger.h"
#include "OnlinePolyfit.h"
#include "debug.h"
// Degrees C
// default: 15.0f
@@ -54,166 +54,186 @@
#define TEMP_CALIBRATION_SECONDS_PER_STEP 0.2f
#if IMU == IMU_ICM20948
// 16 bit 333 lsb/K, ~0.00508 degrees per bit
// 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;
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)
{ };
GyroTemperatureCalibrationState()
: temperatureCurrentIdx(-1)
, numSamples(0)
, tSum(0.0f)
, xSum(0)
, ySum(0)
, zSum(0){};
};
struct GyroTemperatureOffsetSample {
float t;
float x;
float y;
float z;
float t;
float x;
float y;
float z;
GyroTemperatureOffsetSample(): t(0.0f), x(0), y(0), z(0)
{ }
GyroTemperatureOffsetSample()
: t(0.0f)
, x(0)
, y(0)
, z(0) {}
};
struct GyroTemperatureCalibrationConfig {
SlimeVR::Configuration::SensorConfigType type;
SlimeVR::Configuration::SensorConfigType type;
float sensitivityLSB;
float 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;
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)
{ }
GyroTemperatureCalibrationConfig(
SlimeVR::Configuration::SensorConfigType _type,
float _sensitivityLSB
)
: type(_type)
, sensitivityLSB(_sensitivityLSB)
, minTemperatureRange(1000)
, maxTemperatureRange(-1000) {}
bool hasData() {
return minTemperatureRange != 1000;
}
bool hasData() { return minTemperatureRange != 1000; }
bool fullyCalibrated() {
return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs;
}
bool fullyCalibrated() {
return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs;
}
float getCalibrationDonePercent() {
return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f;
}
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 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;
}
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;
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;
// 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);
}
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 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;
}
void reset() {
config.reset();
configSaved = false;
configSaveFailed = false;
}
bool isCalibrating() {
return calibrationRunning;
}
bool isCalibrating() { return calibrationRunning; }
private:
GyroTemperatureCalibrationState state;
uint32_t samplesPerStep;
SlimeVR::Logging::Logger m_Logger;
GyroTemperatureCalibrationState state;
uint32_t samplesPerStep;
SlimeVR::Logging::Logger m_Logger;
float lastApproximatedTemperature = 0.0f;
float lastApproximatedOffsets[3];
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;
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();
void resetCurrentTemperatureState();
};
#endif

207
src/motionprocessing/OnlinePolyfit.h
View File

@@ -1,24 +1,24 @@
/*
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.
*/
#include <Arduino.h>
@@ -28,98 +28,103 @@
template <uint32_t degree, uint32_t dimensions, uint64_t forgettingFactorNumSamples>
class OnlineVectorPolyfit {
public:
constexpr static int32_t numDimensions = dimensions;
constexpr static int32_t numCoefficients = degree + 1;
constexpr static double forgettingFactor = std::exp(-1.0/forgettingFactorNumSamples);
OnlineVectorPolyfit() {
reset();
}
constexpr static int32_t numDimensions = dimensions;
constexpr static int32_t numCoefficients = degree + 1;
constexpr static double forgettingFactor
= std::exp(-1.0 / forgettingFactorNumSamples);
void reset() {
std::fill(Rb[0], Rb[0] + rows * cols, 0.0);
std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f);
}
OnlineVectorPolyfit() { reset(); }
// 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
void reset() {
std::fill(Rb[0], Rb[0] + rows * cols, 0.0);
std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f);
}
// https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf
// Recursive least squares update using QR decomposition by Givens transformations
void update(double xValue, const double yValues[numDimensions]) {
double xin[cols];
xin[0] = 1;
for (int32_t i = 1; i < cols - numDimensions; i++) {
xin[i] = xin[i - 1] * xValue;
}
for (int32_t i = 0; i < numDimensions; i++) {
xin[cols - numDimensions + i] = yValues[i];
}
for (int32_t y = 0; y < rows; y++) {
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;
}
}
}
// 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
// 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;
}
// https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf
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;
}
for (int32_t y = 0; y < rows; y++) {
double c = 1, s = 0;
if (xin[y] != 0.0) {
Rb[y][y] *= forgettingFactor;
const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]);
c = Rb[y][y] * (1.0 / norm);
s = xin[y] * (1.0 / norm);
Rb[y][y] = norm;
}
for (int32_t x = y + 1; x < cols; x++) {
Rb[y][x] *= forgettingFactor;
const double xout = (c * xin[x] - s * Rb[y][x]);
Rb[y][x] = (s * xin[x] + c * Rb[y][x]);
xin[x] = xout;
}
}
}
// Back solves upper triangular system
// Returns float[numDimensions][numCoefficients] coefficients from lowest to highest
// power
const auto computeCoefficients() {
// https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution
for (int32_t d = 0; d < numDimensions; d++) {
for (int32_t y = rows - 1; y >= 0; y--) {
const int32_t bColumn = cols - numDimensions + d;
coeffs[d][y] = Rb[y][bColumn];
if (Rb[y][y] == 0.0) {
continue;
}
for (int32_t x = y + 1; x < rows; x++) {
coeffs[d][y] -= coeffs[d][x] * Rb[y][x];
}
coeffs[d][y] /= Rb[y][y];
}
}
return coeffs;
}
float predict(int32_t d, float x) {
if (d >= numDimensions) {
return 0.0;
}
// https://en.wikipedia.org/wiki/Horner%27s_method
float y = coeffs[d][numCoefficients - 1];
for (int32_t i = numCoefficients - 2; i >= 0; i--) {
y = y * x + coeffs[d][i];
}
return y;
}
std::pair<float, float> tangentAt(float x) {
float intercept = coeffs[0];
float slope = coeffs[1];
for (uint32_t i = 2; i < degree + 1; i++) {
intercept -= coeffs[i] * (i - 1) * pow(x, i);
slope += coeffs[i] * i * pow(x, i - 1);
}
return std::make_pair(slope, intercept);
}
std::pair<float, float> tangentAt(float x) {
float intercept = coeffs[0];
float slope = coeffs[1];
for (uint32_t i = 2; i < degree + 1; i++) {
intercept -= coeffs[i] * (i - 1) * pow(x, i);
slope += coeffs[i] * i * pow(x, i - 1);
}
return std::make_pair(slope, intercept);
}
private:
constexpr static int32_t rows = numCoefficients;
constexpr static int32_t cols = numCoefficients + 3;
double Rb[rows][cols];
float coeffs[numDimensions][rows];
constexpr static int32_t rows = numCoefficients;
constexpr static int32_t cols = numCoefficients + 3;
double Rb[rows][cols];
float coeffs[numDimensions][rows];
};
#endif

415
src/motionprocessing/RestDetection.h
View File

@@ -11,252 +11,281 @@
// #define REST_DETECTION_DISABLE_LPF
#include <Arduino.h>
#include <vqf.h>
#include <basicvqf.h>
#include <vqf.h>
#include "types.h"
#define NaN std::numeric_limits<sensor_real_t>::quiet_NaN()
struct RestDetectionParams {
sensor_real_t biasClip;
sensor_real_t 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 &params, 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]);
}
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;
}
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);
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;
}
// 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);
}
}
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]);
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;
}
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];
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);
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]);
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;
}
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]);
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;
}
}
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];
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);
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]);
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;
}
}
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;
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);
}
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
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;
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;
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

6
src/motionprocessing/types.h
View File

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

46
src/network/connection.cpp
View File

@@ -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<std::unique_ptr<Sensor>> & sensors) {
void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors) {
if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
return;
}
@@ -591,7 +589,7 @@ void Connection::searchForServer() {
m_Connected = true;
m_FeatureFlagsRequestAttempts = 0;
m_ServerFeatures = ServerFeatures { };
m_ServerFeatures = ServerFeatures{};
statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
ledManager.off();
@@ -621,7 +619,11 @@ void Connection::searchForServer() {
void Connection::reset() {
m_Connected = false;
std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
std::fill(
m_AckedSensorState,
m_AckedSensorState + MAX_IMU_COUNT,
SensorStatus::SENSOR_OFFLINE
);
m_UDP.begin(m_ServerPort);
@@ -629,7 +631,7 @@ void Connection::reset() {
}
void Connection::update() {
auto & sensors = sensorManager.getSensors();
auto& sensors = sensorManager.getSensors();
updateSensorState(sensors);
maybeRequestFeatureFlags();
@@ -643,7 +645,11 @@ void Connection::update() {
statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
m_Connected = false;
std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
std::fill(
m_AckedSensorState,
m_AckedSensorState + MAX_IMU_COUNT,
SensorStatus::SENSOR_OFFLINE
);
m_Logger.warn("Connection to server timed out");
return;
@@ -720,18 +726,19 @@ void Connection::update() {
m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength);
if (!hadFlags) {
#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
if (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;
}
}

10
src/network/connection.h
View File

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

22
src/network/featureflags.h
View File

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

3
src/network/packets.h
View File

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

420
src/network/wifihandler.cpp
View File

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

68
src/network/wifihandler.h
View File

@@ -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 <ESP8266WiFi.h>
#include <ESP8266WiFi.h>
#else
#include <WiFi.h>
#include <WiFi.h>
#endif
namespace WiFiNetwork {
bool isConnected();
void setUp();
void upkeep();
void setWiFiCredentials(const char * SSID, const char * pass);
IPAddress getAddress();
uint8_t getWiFiState();
}
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_

58
src/network/wifiprovisioning.cpp
View File

@@ -1,28 +1,29 @@
/*
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.
*/
#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(); }

48
src/network/wifiprovisioning.h
View File

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

71
src/sensors/EmptySensor.h
View File

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

60
src/sensors/ErroneousSensor.cpp
View File

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

71
src/sensors/ErroneousSensor.h
View File

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

388
src/sensors/SensorFusion.cpp
View File

@@ -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);
}
namespace SlimeVR {
namespace Sensors {
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;
}
}
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

200
src/sensors/SensorFusion.h
View File

@@ -14,13 +14,13 @@
#define SENSOR_FUSION_VQF 4
#if SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY
#define SENSOR_FUSION_TYPE_STRING "mahony"
#define SENSOR_FUSION_TYPE_STRING "mahony"
#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK
#define SENSOR_FUSION_TYPE_STRING "madgwick"
#define SENSOR_FUSION_TYPE_STRING "madgwick"
#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF
#define SENSOR_FUSION_TYPE_STRING "bvqf"
#define SENSOR_FUSION_TYPE_STRING "bvqf"
#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF
#define SENSOR_FUSION_TYPE_STRING "vqf"
#define SENSOR_FUSION_TYPE_STRING "vqf"
#endif
#define SENSOR_USE_MAHONY (SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY)
@@ -28,106 +28,122 @@
#define SENSOR_USE_BASICVQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF)
#define SENSOR_USE_VQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF)
#include <basicvqf.h>
#include <vqf.h>
#include "../motionprocessing/types.h"
#include "mahony.h"
#include "madgwick.h"
#include <vqf.h>
#include <basicvqf.h>
#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
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
{}
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);
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();
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]);
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;
protected:
sensor_real_t gyrTs;
sensor_real_t accTs;
sensor_real_t magTs;
#if SENSOR_USE_MAHONY
Mahony<sensor_real_t> mahony;
#elif SENSOR_USE_MADGWICK
Madgwick<sensor_real_t> madgwick;
#elif SENSOR_USE_BASICVQF
BasicVQF basicvqf;
#elif SENSOR_USE_VQF
SensorVQFParams vqfParams {};
VQF vqf;
#endif
#if SENSOR_USE_MAHONY
Mahony<sensor_real_t> mahony;
#elif SENSOR_USE_MADGWICK
Madgwick<sensor_real_t> madgwick;
#elif SENSOR_USE_BASICVQF
BasicVQF basicvqf;
#elif SENSOR_USE_VQF
SensorVQFParams vqfParams{};
VQF vqf;
#endif
// A also used for linear acceleration extraction
sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
// 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
#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 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
};
}
}
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_SENSORFUSION_H
#endif // SLIMEVR_SENSORFUSION_H

207
src/sensors/SensorFusionDMP.cpp
View File

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

75
src/sensors/SensorFusionDMP.h
View File

@@ -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]);
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();
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<sensor_real_t> dmpmag;
protected:
DMPMag<sensor_real_t> 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;
sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f};
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
};
}
}
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;
#endif // SLIMEVR_SENSORFUSIONDMP_H
bool gravityReady = false;
sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f};
bool linaccelReady = false;
sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f};
#if ESP32
sensor_real_t linAccel_guard; // Temporary patch for some weird ESP32 bug
#endif
};
} // namespace Sensors
} // namespace SlimeVR
#endif // SLIMEVR_SENSORFUSIONDMP_H

63
src/sensors/SensorFusionRestDetect.cpp
View File

@@ -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);
}
void SensorFusionRestDetect::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat)
{
if (deltat < 0) deltat = gyrTs;
restDetection.updateGyr(Gxyz);
SensorFusion::updateGyro(Gxyz, deltat);
}
#endif
bool SensorFusionRestDetect::getRestDetected()
{
#if !SENSOR_FUSION_WITH_RESTDETECT
return restDetection.getRestDetected();
#elif SENSOR_USE_VQF
return vqf.getRestDetected();
#endif
}
}
namespace SlimeVR {
namespace Sensors {
#if !SENSOR_FUSION_WITH_RESTDETECT
void SensorFusionRestDetect::updateAcc(
const sensor_real_t Axyz[3],
sensor_real_t deltat
) {
if (deltat < 0) {
deltat = accTs;
}
restDetection.updateAcc(deltat, Axyz);
SensorFusion::updateAcc(Axyz, deltat);
}
void SensorFusionRestDetect::updateGyro(
const sensor_real_t Gxyz[3],
sensor_real_t deltat
) {
if (deltat < 0) {
deltat = gyrTs;
}
restDetection.updateGyr(Gxyz);
SensorFusion::updateGyro(Gxyz, deltat);
}
#endif
bool SensorFusionRestDetect::getRestDetected() {
#if !SENSOR_FUSION_WITH_RESTDETECT
return restDetection.getRestDetected();
#elif SENSOR_USE_VQF
return vqf.getRestDetected();
#endif
}
} // namespace Sensors
} // namespace SlimeVR

80
src/sensors/SensorFusionRestDetect.h
View File

@@ -1,55 +1,51 @@
#ifndef SLIMEVR_SENSORFUSIONRESTDETECT_H_
#define SLIMEVR_SENSORFUSIONRESTDETECT_H_
#include "../motionprocessing/RestDetection.h"
#include "SensorFusion.h"
#include "../motionprocessing/RestDetection.h"
#if SENSOR_USE_VQF
#define SENSOR_FUSION_WITH_RESTDETECT 1
#define SENSOR_FUSION_WITH_RESTDETECT 1
#else
#define SENSOR_FUSION_WITH_RESTDETECT 0
#define SENSOR_FUSION_WITH_RESTDETECT 0
#endif
namespace SlimeVR
{
namespace 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_

333
src/sensors/SensorManager.cpp
View File

@@ -1,192 +1,199 @@
/*
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 "SensorManager.h"
#include "bmi160sensor.h"
#include "bno055sensor.h"
#include "bno080sensor.h"
#include "mpu9250sensor.h"
#include "mpu6050sensor.h"
#include "bmi160sensor.h"
#include "icm20948sensor.h"
#include "mpu6050sensor.h"
#include "mpu9250sensor.h"
#include "sensoraddresses.h"
#include "softfusion/softfusionsensor.h"
#include "softfusion/drivers/lsm6ds3trc.h"
#include "softfusion/drivers/icm42688.h"
#include "softfusion/drivers/bmi270.h"
#include "softfusion/drivers/lsm6dsv.h"
#include "softfusion/drivers/icm42688.h"
#include "softfusion/drivers/lsm6ds3trc.h"
#include "softfusion/drivers/lsm6dso.h"
#include "softfusion/drivers/lsm6dsr.h"
#include "softfusion/drivers/lsm6dsv.h"
#include "softfusion/drivers/mpu6050.h"
#include "softfusion/i2cimpl.h"
#include "softfusion/softfusionsensor.h"
#if ESP32
#include "driver/i2c.h"
#include "driver/i2c.h"
#endif
namespace SlimeVR
{
namespace Sensors
{
using SoftFusionLSM6DS3TRC = SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl>;
using SoftFusionICM42688 = SoftFusionSensor<SoftFusion::Drivers::ICM42688, SoftFusion::I2CImpl>;
using SoftFusionBMI270 = SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSV = SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
namespace SlimeVR {
namespace Sensors {
using SoftFusionLSM6DS3TRC
= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl>;
using SoftFusionICM42688
= SoftFusionSensor<SoftFusion::Drivers::ICM42688, SoftFusion::I2CImpl>;
using SoftFusionBMI270
= SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSV
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSO
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSR
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
using SoftFusionMPU6050
= SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
// TODO Make it more generic in the future and move another place (abstract sensor interface)
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);
// 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<int>(sdaPin), static_cast<int>(sclPin), I2C_SPEED);
Wire.setTimeOut(150);
}
#else
Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin));
#endif
if (running) {
i2c_set_pin(I2C_NUM_0, sdaPin, sclPin, false, false, I2C_MODE_MASTER);
} else {
Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin), I2C_SPEED);
Wire.setTimeOut(150);
}
#else
Wire.begin(static_cast<int>(sdaPin), static_cast<int>(sclPin));
#endif
activeSCL = sclPin;
activeSDA = sdaPin;
}
}
activeSCL = sclPin;
activeSDA = sdaPin;
}
}
void SensorManager::setup()
{
running = false;
activeSCL = PIN_IMU_SCL;
activeSDA = PIN_IMU_SDA;
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<ImuType>(sensorID, __VA_ARGS__); \
if (sensor->isWorking()) { \
m_Logger.info("Sensor %d configured", sensorID+1);\
activeSensorCount++; \
} \
m_Sensors.push_back(std::move(sensor)); \
sensorID++; \
}
// Apply descriptor list and expand to entrys
IMU_DESC_LIST;
uint8_t sensorID = 0;
uint8_t activeSensorCount = 0;
#define IMU_DESC_ENTRY(ImuType, ...) \
{ \
auto sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__); \
if (sensor->isWorking()) { \
m_Logger.info("Sensor %d configured", sensorID + 1); \
activeSensorCount++; \
} \
m_Sensors.push_back(std::move(sensor)); \
sensorID++; \
}
// Apply descriptor list and expand to 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

196
src/sensors/SensorManager.h
View File

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

96
src/sensors/axisremap.h
View File

@@ -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<typename T>
template <typename T>
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<typename T>
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 <typename T>
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

1817
src/sensors/bmi160sensor.cpp
View File

File diff suppressed because it is too large Load Diff

346
src/sensors/bmi160sensor.h
View File

@@ -1,52 +1,51 @@
/*
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 SENSORS_BMI160SENSOR_H
#define SENSORS_BMI160SENSOR_H
#include "sensor.h"
#include "sensors/axisremap.h"
#include "magneto1.4.h"
#include <BMI160.h>
#include "SensorFusionRestDetect.h"
#include "../motionprocessing/types.h"
#include "../motionprocessing/GyroTemperatureCalibrator.h"
#include "../motionprocessing/RestDetection.h"
#include "../motionprocessing/types.h"
#include "SensorFusionRestDetect.h"
#include "magneto1.4.h"
#include "sensor.h"
#include "sensors/axisremap.h"
#if BMI160_USE_VQF
#if USE_6_AXIS
#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 double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB
= (16.4f * (1 << BMI160_GYRO_RANGE));
constexpr std::pair<uint8_t, float> BMI160_ACCEL_SENSITIVITY_LSB_MAP[] = {
{BMI160_ACCEL_RANGE_2G, 16384.0f},
{BMI160_ACCEL_RANGE_4G, 8192.0f},
{BMI160_ACCEL_RANGE_8G, 4096.0f},
{BMI160_ACCEL_RANGE_16G, 2048.0f}
};
constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB = BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second;
constexpr double BMI160_ASCALE = CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB;
constexpr std::pair<uint8_t, float> BMI160_ACCEL_SENSITIVITY_LSB_MAP[]
= {{BMI160_ACCEL_RANGE_2G, 16384.0f},
{BMI160_ACCEL_RANGE_4G, 8192.0f},
{BMI160_ACCEL_RANGE_8G, 4096.0f},
{BMI160_ACCEL_RANGE_16G, 2048.0f}};
constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB
= BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second;
constexpr double BMI160_ASCALE
= CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB;
// Scale conversion steps: LSB/°/s -> °/s -> step/°/s -> step/rad/s
constexpr double BMI160_GSCALE = ((32768. / BMI160_GYRO_TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0);
constexpr 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;
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);
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 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 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]);
void applyAccelCalibrationAndScale(sensor_real_t Axyz[3]);
void applyMagCalibrationAndScale(sensor_real_t Mxyz[3]);
bool hasGyroCalibration();
bool hasAccelCalibration();
bool hasMagCalibration();
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 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
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);
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);
bool getTemperature(float* out);
private:
BMI160 imu {};
int axisRemap;
private:
BMI160 imu{};
int axisRemap;
SlimeVR::Sensors::SensorFusionRestDetect sfusion;
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;
// 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
// 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;
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};
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 gscaleX = BMI160_GSCALE;
double gscaleY = BMI160_GSCALE;
double gscaleZ = BMI160_GSCALE;
double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0};
double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0};
bool isGyroCalibrated = false;
bool isAccelCalibrated = false;
bool isMagCalibrated = false;
bool isGyroCalibrated = false;
bool isAccelCalibrated = false;
bool isMagCalibrated = false;
SlimeVR::Configuration::BMI160SensorConfig m_Config = {};
SlimeVR::Configuration::BMI160SensorConfig m_Config = {};
};
#endif

100
src/sensors/bno055sensor.cpp
View File

@@ -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) {}

78
src/sensors/bno055sensor.h
View File

@@ -1,49 +1,63 @@
/*
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 SENSORS_BNO055SENSOR_H
#define SENSORS_BNO055SENSOR_H
#include "sensor.h"
#include <Adafruit_BNO055.h>
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

424
src/sensors/bno080sensor.cpp
View File

@@ -1,231 +1,286 @@
/*
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 "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;
}
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
);
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);
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;
}
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 (!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 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 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();
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);
}
networkConnection.sendInspectionRawIMUData(
sensorId,
rX,
rY,
rZ,
rA,
aX,
aY,
aZ,
aA,
mX,
mY,
mZ,
mA
);
}
#endif
lastReset = 0;
lastData = millis();
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);
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 {
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
);
if (imu.hasNewQuat()) // New quaternion if context
{
Quat nRotation;
imu.getQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, magneticAccuracyEstimate, calibrationAccuracy);
setFusedRotation(nRotation);
setFusedRotation(nRotation);
// Leave new quaternion if context open, it's closed later
} // Closing new quaternion if context
}
// Leave new quaternion if context open, it's closed later
} // Closing new quaternion if context
}
// Continuation of the new quaternion if context, used for both 6 and 9 axis
// 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
{
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);
}
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);
}
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
}

175
src/sensors/bno080sensor.h
View File

@@ -1,91 +1,146 @@
/*
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 SENSORS_BNO080SENSOR_H
#define SENSORS_BNO080SENSOR_H
#include "sensor.h"
#include <BNO080.h>
#include "sensor.h"
#define FLAG_SENSOR_BNO0XX_MAG_ENABLED 1
class BNO080Sensor : public Sensor
{
class BNO080Sensor : public Sensor {
public:
static constexpr auto TypeID = ImuID::BNO080;
static constexpr uint8_t Address = 0x4a;
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();
}
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;
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{};
BNO080 imu{};
uint8_t m_IntPin;
uint8_t m_IntPin;
uint8_t tap;
unsigned long lastData = 0;
uint8_t lastReset = 0;
BNO080Error lastError{};
SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {};
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;
// 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

1495
src/sensors/icm20948sensor.cpp
View File

File diff suppressed because it is too large Load Diff

133
src/sensors/icm20948sensor.h
View File

@@ -1,85 +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_ICM20948SENSOR_H_
#define SLIMEVR_ICM20948SENSOR_H_
#include <ICM_20948.h>
#include "sensor.h"
#include "SensorFusionDMP.h"
#include "sensor.h"
class ICM20948Sensor : public Sensor
{
class ICM20948Sensor : public Sensor {
public:
static constexpr auto TypeID = ImuID::ICM20948;
static constexpr 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_

309
src/sensors/mpu6050sensor.cpp
View File

@@ -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.
*/
#include "globals.h"
@@ -29,174 +29,207 @@
#include "MPU6050_6Axis_MotionApps20.h"
#endif
#include "mpu6050sensor.h"
#include <i2cscan.h>
#include "calibration.h"
#include "GlobalVars.h"
#include "calibration.h"
#include "mpu6050sensor.h"
#define ACCEL_SENSITIVITY_2G 16384.0f
// Accel scale conversion steps: LSB/G -> G -> m/s^2
constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY;
constexpr float ASCALE_2G
= ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY;
void MPU6050Sensor::motionSetup()
{
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;
}
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);
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;
// 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;
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");
}
}
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
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
imu.CalibrateGyro(6);
imu.CalibrateAccel(6);
imu.PrintActiveOffsets();
#endif // IMU_MPU6050_RUNTIME_CALIBRATION
ledManager.pattern(50, 50, 5);
ledManager.pattern(50, 50, 5);
// turn on the DMP, now that it's ready
m_Logger.debug("Enabling DMP...");
imu.setDMPEnabled(true);
// turn on the DMP, now that it's ready
m_Logger.debug("Enabling DMP...");
imu.setDMPEnabled(true);
// TODO: Add interrupt support
// mpuIntStatus = imu.getIntStatus();
// 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;
// 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();
// 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);
}
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);
{
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);
}
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);
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);
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;
}
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();
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
m_Logger.info("Calibration finished");
#endif // !IMU_MPU6050_RUNTIME_CALIBRATION
ledManager.off();
ledManager.off();
}

96
src/sensors/mpu6050sensor.h
View File

@@ -1,62 +1,78 @@
/*
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 SENSORS_MPU6050SENSOR_H
#define SENSORS_MPU6050SENSOR_H
#include "sensor.h"
#include <MPU6050.h>
#include "SensorFusionDMP.h"
#include "sensor.h"
class MPU6050Sensor : public Sensor
{
class MPU6050Sensor : public Sensor {
public:
static constexpr auto TypeID = ImuID::MPU6050;
static constexpr 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
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;
SlimeVR::Sensors::SensorFusionDMP sfusion;
#ifndef IMU_MPU6050_RUNTIME_CALIBRATION
SlimeVR::Configuration::MPU6050SensorConfig m_Config = {};
SlimeVR::Configuration::MPU6050SensorConfig m_Config = {};
#endif
};

737
src/sensors/mpu9250sensor.cpp
View File

@@ -1,421 +1,491 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain, S.J. Remington & SlimeVR contributors
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.
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 <i2cscan.h>
#include "GlobalVars.h"
#include "calibration.h"
#include "globals.h"
#include "helper_3dmath.h"
#include <i2cscan.h>
#include "calibration.h"
#include "magneto1.4.h"
#include "GlobalVars.h"
#if MPU_USE_DMPMAG
#include "dmpmag.h"
#endif
//#if defined(_MAHONY_H_) || defined(_MADGWICK_H_)
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;
}
// 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);
m_Logger.info(
"Connected to MPU9250 (reported device ID 0x%02x) at address 0x%02x",
imu.getDeviceID(),
addr
);
int16_t ax,ay,az;
int16_t ax, ay, az;
// turn on while flip back to calibrate. then, flip again after 5 seconds.
// 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();
// 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);
}
}
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;
// 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;
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");
}
}
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);
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);
// turn on the DMP, now that it's ready
m_Logger.debug("Enabling DMP...");
imu.setDMPEnabled(true);
// TODO: Add interrupt support
// mpuIntStatus = imu.getIntStatus();
// 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;
// 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);
}
// 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);
{
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);
}
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;
// 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);
sfusion.updateQuaternion(rawQuat);
int16_t temp[3];
imu.getMagnetometer(&temp[0], &temp[1], &temp[2]);
parseMagData(temp);
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.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);
}
sfusion.updateAcc(Axyz);
}
#endif
setFusedRotation(sfusion.getQuaternionQuat());
#if SEND_ACCELERATION
#else
union fifo_sample_raw buf;
uint16_t remaining_samples;
// TODO: would it be faster to read multiple samples at once
while (getNextSample(&buf, &remaining_samples)) {
parseAccelData(buf.sample.accel);
parseGyroData(buf.sample.gyro);
parseMagData(buf.sample.mag);
// TODO: monitor magnetometer status
// buf.sample.mag_status;
// TODO: monitor interrupts
// imu.getIntStatus();
// TODO: monitor remaining_samples to ensure that the number is going down, not
// up. remaining_samples
sfusion.update9D(Axyz, Gxyz, Mxyz);
}
#endif
setFusedRotation(sfusion.getQuaternionQuat());
#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;
// 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);
// 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...");
ledManager.off();
delay(250);
}
m_Logger.debug("Calculating calibration data...");
float M_BAinv[4][3];
magneto->current_calibration(M_BAinv);
delete magneto;
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("}");
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;
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);
// 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;
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)) { ; }
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;
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];
// 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);
// 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();
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);
// 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...");
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 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;
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("}");
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
// 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];
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]);
}
// 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
}
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];
// 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
#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]);
}
// 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;
}
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
#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);
// 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;
}
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;
}
if (remaining_count) {
*remaining_count = (count / sensor_data_len) - 1;
}
imu.getFIFOBytes(buffer->raw, sensor_data_len);
swapFifoData(buffer);
return true;
imu.getFIFOBytes(buffer->raw, sensor_data_len);
swapFifoData(buffer);
return true;
}

166
src/sensors/mpu9250sensor.h
View File

@@ -1,37 +1,38 @@
/*
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 SENSORS_MPU9250SENSOR_H
#define SENSORS_MPU9250SENSOR_H
#include "sensor.h"
#include "logging/Logger.h"
#include <MPU9250_6Axis_MotionApps_V6_12.h>
#include "logging/Logger.h"
#include "sensor.h"
#define MPU9250_DEFAULT_ODR_HZ 8000.0f
#define MPU9250_SAMPLE_DIV 39
constexpr float MPU9250_ODR_TS = ( 1.0f / MPU9250_DEFAULT_ODR_HZ) * (1+MPU9250_SAMPLE_DIV);
constexpr float MPU9250_ODR_TS
= (1.0f / MPU9250_DEFAULT_ODR_HZ) * (1 + MPU9250_SAMPLE_DIV);
#define MPU_USE_DMPMAG 1
@@ -41,71 +42,88 @@ constexpr float MPU9250_ODR_TS = ( 1.0f / MPU9250_DEFAULT_ODR_HZ) * (1+MPU9250_S
#include "SensorFusion.h"
#endif
class MPU9250Sensor : public Sensor
{
class MPU9250Sensor : public Sensor {
public:
static constexpr auto TypeID = ImuID::MPU9250;
static constexpr uint8_t Address = 0x68;
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();
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)
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
#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};
// 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 = {};
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]);
// 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;
// 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;
};
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;
};
// 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);
// 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

177
src/sensors/sensor.cpp
View File

@@ -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 <i2cscan.h>
#include "GlobalVars.h"
#include "calibration.h"
SensorStatus Sensor::getSensorState() {
return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR;
return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR;
}
void Sensor::setAcceleration(Vector3 a) {
acceleration = a;
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";
}

68
src/sensors/sensor.h
View File

@@ -27,6 +27,7 @@
#include <Arduino.h>
#include <quat.h>
#include <vector3.h>
#include "configuration/Configuration.h"
#include "globals.h"
#include "logging/Logger.h"
@@ -47,13 +48,24 @@ enum class MagnetometerStatus : uint8_t {
MAG_ENABLED = 2,
};
class Sensor
{
class Sensor {
public:
Sensor(const char *sensorName, ImuID type, uint8_t id, uint8_t address, float rotation, uint8_t sclpin=0, uint8_t sdapin=0)
: 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 @@ public:
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 @@ private:
void printTemperatureCalibrationUnsupported();
};
const char * getIMUNameByType(ImuID imuType);
const char* getIMUNameByType(ImuID imuType);
#endif // SLIMEVR_SENSOR_H_
#endif // SLIMEVR_SENSOR_H_

4
src/sensors/sensoraddresses.h
View File

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

821
src/sensors/softfusion/drivers/bmi270.h
View File

@@ -1,36 +1,37 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
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:
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstdint>
#include <array>
#include <algorithm>
#include <array>
#include <cstdint>
#include <cstring>
#include <limits>
#include "bmi270fw.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers
{
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 16g
// and gyroscope range at 1000dps
@@ -38,382 +39,406 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers
// Timestamps reading are not used
template <typename I2CImpl>
struct BMI270
{
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<uint8_t*>(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<int8_t>(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<uint8_t, 3> 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<int16_t>(i2c.readReg16(Regs::TempData));
return static_cast<float>(value) * TempStep + 23.0f;
}
using FifoBuffer = std::array<uint8_t, I2CImpl::MaxTransactionLength>;
FifoBuffer read_buffer;
template<typename T>
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 <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(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<uint8_t>(i, read_buffer);
if ((header & Fifo::ModeMask) == Fifo::SkipFrame && (i - bytes_to_read) >= 1) {
getFromFifo<uint8_t>(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<uint16_t>(i, read_buffer);
gyro[1] = getFromFifo<uint16_t>(i, read_buffer);
gyro[2] = getFromFifo<uint16_t>(i, read_buffer);
using ShortLimit = std::numeric_limits<int16_t>;
// apply zx factor, todo: this awful line should be simplified and validated
gyro[0] = std::clamp(static_cast<int32_t>(gyro[0]) - static_cast<int16_t>((static_cast<int32_t>(zxFactor) * gyro[2]) / 512),
static_cast<int32_t>(ShortLimit::min()), static_cast<int32_t>(ShortLimit::max()));
processGyroSample(gyro, GyrTs);
}
if (header & Fifo::AccelDataBit) {
int16_t accel[3];
accel[0] = getFromFifo<uint16_t>(i, read_buffer);
accel[1] = getFromFifo<uint16_t>(i, read_buffer);
accel[2] = getFromFifo<uint16_t>(i, read_buffer);
processAccelSample(accel, AccTs);
}
}
}
}
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<uint8_t*>(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<int8_t>(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<uint8_t, 3> 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<int16_t>(i2c.readReg16(Regs::TempData));
return static_cast<float>(value) * TempStep + 23.0f;
}
using FifoBuffer = std::array<uint8_t, I2CImpl::MaxTransactionLength>;
FifoBuffer read_buffer;
template <typename T>
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 <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
const auto bytes_to_read = std::min(
static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(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<uint8_t>(i, read_buffer);
if ((header & Fifo::ModeMask) == Fifo::SkipFrame
&& (i - bytes_to_read) >= 1) {
getFromFifo<uint8_t>(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<uint16_t>(i, read_buffer);
gyro[1] = getFromFifo<uint16_t>(i, read_buffer);
gyro[2] = getFromFifo<uint16_t>(i, read_buffer);
using ShortLimit = std::numeric_limits<int16_t>;
// apply zx factor, todo: this awful line should be simplified and
// validated
gyro[0] = std::clamp(
static_cast<int32_t>(gyro[0])
- static_cast<int16_t>(
(static_cast<int32_t>(zxFactor) * gyro[2]) / 512
),
static_cast<int32_t>(ShortLimit::min()),
static_cast<int32_t>(ShortLimit::max())
);
processGyroSample(gyro, GyrTs);
}
if (header & Fifo::AccelDataBit) {
int16_t accel[3];
accel[0] = getFromFifo<uint16_t>(i, read_buffer);
accel[1] = getFromFifo<uint16_t>(i, read_buffer);
accel[2] = getFromFifo<uint16_t>(i, read_buffer);
processAccelSample(accel, AccTs);
}
}
}
}
};
} // namespace
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

1091
src/sensors/softfusion/drivers/bmi270fw.h
View File

File diff suppressed because it is too large Load Diff

265
src/sensors/softfusion/drivers/icm42688.h
View File

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

160
src/sensors/softfusion/drivers/lsm6ds-common.h
View File

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

231
src/sensors/softfusion/drivers/lsm6ds3trc.h
View File

@@ -1,139 +1,152 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Tailsy13 & SlimeVR Contributors
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:
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstdint>
#include <array>
#include <algorithm>
#include <array>
#include <cstdint>
namespace SlimeVR::Sensors::SoftFusion::Drivers
{
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 416Hz
template <typename I2CImpl>
struct LSM6DS3TRC
{
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DS3TR-C";
static constexpr auto Type = ImuID::LSM6DS3TRC;
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 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 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;
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) {}
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
};
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;
};
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;
}
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<int16_t>(i2c.readReg16(Regs::OutTemp));
float result = ((float)value / 256.0f) + 25.0f;
float getDirectTemp() const {
const auto value = static_cast<int16_t>(i2c.readReg16(Regs::OutTemp));
float result = ((float)value / 256.0f) + 25.0f;
return result;
}
return result;
}
template <typename AccelCall, typename GyroCall>
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<int16_t, 60> read_buffer; // max 10 packages of 6 16bit values of data form fifo
const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()), static_cast<size_t>(unread_entries)) \
* sizeof(uint16_t) / single_measurement_bytes * single_measurement_bytes;
i2c.readBytes(Regs::FifoData, bytes_to_read, reinterpret_cast<uint8_t *>(read_buffer.data()));
for (uint16_t i=0; i<bytes_to_read/sizeof(uint16_t); i+=single_measurement_words) {
processGyroSample(reinterpret_cast<const int16_t *>(&read_buffer[i]), GyrTs);
processAccelSample(reinterpret_cast<const int16_t *>(&read_buffer[i+3]), AccTs);
}
}
template <typename AccelCall, typename GyroCall>
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<int16_t, 60>
read_buffer; // max 10 packages of 6 16bit values of data form fifo
const auto bytes_to_read = std::min(
static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(unread_entries)
)
* sizeof(uint16_t) / single_measurement_bytes
* single_measurement_bytes;
i2c.readBytes(
Regs::FifoData,
bytes_to_read,
reinterpret_cast<uint8_t*>(read_buffer.data())
);
for (uint16_t i = 0; i < bytes_to_read / sizeof(uint16_t);
i += single_measurement_words) {
processGyroSample(reinterpret_cast<const int16_t*>(&read_buffer[i]), GyrTs);
processAccelSample(
reinterpret_cast<const int16_t*>(&read_buffer[i + 3]),
AccTs
);
}
}
};
} // namespace
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

181
src/sensors/softfusion/drivers/lsm6dso.h
View File

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

181
src/sensors/softfusion/drivers/lsm6dsr.h
View File

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

211
src/sensors/softfusion/drivers/lsm6dsv.h
View File

@@ -1,135 +1,136 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
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:
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstdint>
#include <array>
#include <algorithm>
#include <array>
#include <cstdint>
#include "lsm6ds-common.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers
{
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// and gyroscope range at 1000dps
// Gyroscope ODR = 480Hz, accel ODR = 120Hz
template <typename I2CImpl>
struct LSM6DSV : LSM6DSOutputHandler<I2CImpl>
{
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DSV";
static constexpr auto Type = ImuID::LSM6DSV;
struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
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 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 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;
static constexpr float GyroSensitivity = 1000 / 35.0f;
static constexpr float AccelSensitivity = 1000 / 0.244f;
using LSM6DSOutputHandler<I2CImpl>::i2c;
using LSM6DSOutputHandler<I2CImpl>::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
};
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;
};
static constexpr uint8_t FifoStatus = 0x1b;
static constexpr uint8_t FifoData = 0x78;
};
LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {
}
LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: LSM6DSOutputHandler<I2CImpl>(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;
}
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<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(processAccelSample, processGyroSample, GyrTs, AccTs);
}
float getDirectTemp() const {
return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(
processAccelSample,
processGyroSample,
GyrTs,
AccTs
);
}
};
} // namespace
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

297
src/sensors/softfusion/drivers/mpu6050.h
View File

@@ -1,185 +1,210 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 furrycoding & SlimeVR Contributors
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:
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstdint>
#include <array>
#include <algorithm>
#include <MPU6050.h>
#include <algorithm>
#include <array>
#include <cstdint>
namespace SlimeVR::Sensors::SoftFusion::Drivers
{
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// and gyroscope range at 1000dps
// Gyroscope ODR = accel ODR = 250Hz
template <typename I2CImpl>
struct MPU6050
{
struct 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;
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;
// 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))
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 uint8_t Address = 0x68;
static constexpr auto Name = "MPU-6050";
static constexpr auto Type = ImuID::MPU6050;
static constexpr float Freq = 250;
static constexpr float Freq = 250;
static constexpr float GyrTs = 1.0 / Freq;
static constexpr float AccTs = 1.0 / Freq;
static constexpr float MagTs = 1.0 / Freq;
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;
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) {}
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 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 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 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
};
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 OutTemp = MPU6050_RA_TEMP_OUT_H;
static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS;
static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS;
static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH;
static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W;
};
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);
}
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);
}
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))
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);
i2c.writeReg(MPU6050_RA_FIFO_EN, 0x78); // 0111 1000 FIFO_EN: All gyro axes + Accel
// Configure
i2c.writeReg(
MPU6050_RA_PWR_MGMT_1,
0x01
); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
i2c.writeReg(
MPU6050_RA_USER_CTRL,
0x00
); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP
i2c.writeReg(
MPU6050_RA_INT_ENABLE,
0x10
); // 0001 0000 INT_ENABLE: only FIFO overflow interrupt
i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
i2c.writeReg(
MPU6050_RA_CONFIG,
0x02
); // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro
// output rate to 1KHz)
i2c.writeReg(
MPU6050_RA_SMPLRT_DIV,
0x03
); // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate
// = Gyroscope Output Rate / (1 + SMPLRT_DIV))
resetFIFO();
i2c.writeReg(
MPU6050_RA_FIFO_EN,
0x78
); // 0111 1000 FIFO_EN: All gyro axes + Accel
return true;
}
resetFIFO();
float getDirectTemp() const
{
auto value = byteSwap(i2c.readReg16(Regs::OutTemp));
float result = (static_cast<int16_t>(value) / 340.0f) + 36.53f;
return result;
}
return true;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
const auto status = i2c.readReg(Regs::IntStatus);
float getDirectTemp() const {
auto value = byteSwap(i2c.readReg16(Regs::OutTemp));
float result = (static_cast<int16_t>(value) / 340.0f) + 36.53f;
return result;
}
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;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
const auto status = i2c.readReg(Regs::IntStatus);
std::array<uint8_t, 12 * 10> readBuffer; // max 10 packages of 12byte values (sample) of data form fifo
auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount));
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;
}
auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size()) / sizeof(FifoSample) * sizeof(FifoSample);
if (!readBytes) {
return;
}
std::array<uint8_t, 12 * 10>
readBuffer; // max 10 packages of 12byte values (sample) of data form fifo
auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount));
i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data());
for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) {
const FifoSample *sample = reinterpret_cast<FifoSample *>(&readBuffer[i]);
int16_t xyz[3];
auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size())
/ sizeof(FifoSample) * sizeof(FifoSample);
if (!readBytes) {
return;
}
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);
i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data());
for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) {
const FifoSample* sample = reinterpret_cast<FifoSample*>(&readBuffer[i]);
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);
}
}
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

108
src/sensors/softfusion/i2cimpl.h
View File

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

1004
src/sensors/softfusion/softfusionsensor.h
View File

File diff suppressed because it is too large Load Diff

650
src/serial/serialcommands.cpp
View File

@@ -1,342 +1,376 @@
/*
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 "serialcommands.h"
#include "logging/Logger.h"
#include <CmdCallback.hpp>
#include "base64.hpp"
#include "GlobalVars.h"
#include "base64.hpp"
#include "batterymonitor.h"
#include "logging/Logger.h"
#include "utils.h"
#if ESP32
#include "nvs_flash.h"
#include "nvs_flash.h"
#endif
namespace SerialCommands {
SlimeVR::Logging::Logger logger("SerialCommands");
SlimeVR::Logging::Logger logger("SerialCommands");
CmdCallback<6> cmdCallbacks;
CmdParser cmdParser;
CmdBuffer<256> cmdBuffer;
CmdCallback<6> cmdCallbacks;
CmdParser cmdParser;
CmdBuffer<256> cmdBuffer;
bool lengthCheck (const char* const text, unsigned int length, const char* const cmd, const char* const name)
{
size_t l = text != nullptr ? strlen(text) : 0;
if ((l > length)) {
logger.error("%s ERROR: %s is longer than %d bytes / Characters", cmd, name, length);
return false;
}
return true;
bool lengthCheck(
const char* const text,
unsigned int length,
const char* const cmd,
const char* const name
) {
size_t l = text != nullptr ? strlen(text) : 0;
if ((l > length)) {
logger.error(
"%s ERROR: %s is longer than %d bytes / Characters",
cmd,
name,
length
);
return false;
}
return true;
}
unsigned int decode_base64_length_null(const char* const b64char, unsigned int* b64ssidlength)
{
if (b64char==NULL) {
return 0;
}
*b64ssidlength = (unsigned int)strlen(b64char);
return decode_base64_length((unsigned char *)b64char, *b64ssidlength);
unsigned int
decode_base64_length_null(const char* const b64char, unsigned int* b64ssidlength) {
if (b64char == NULL) {
return 0;
}
*b64ssidlength = (unsigned int)strlen(b64char);
return decode_base64_length((unsigned char*)b64char, *b64ssidlength);
}
void cmdSet(CmdParser * parser) {
if(parser->getParamCount() != 1) {
if (parser->equalCmdParam(1, "WIFI")) {
if(parser->getParamCount() < 3) {
logger.error("CMD SET WIFI ERROR: Too few arguments");
logger.info("Syntax: SET WIFI \"<SSID>\" \"<PASSWORD>\"");
} else {
const char *sc_ssid = parser->getCmdParam(2);
const char *sc_pw = parser->getCmdParam(3);
if (!lengthCheck(sc_ssid, 32, "CMD SET WIFI", "SSID") &&
!lengthCheck(sc_pw, 64, "CMD SET WIFI", "Password")) {
return;
}
WiFiNetwork::setWiFiCredentials(sc_ssid, sc_pw);
logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting");
}
} else if (parser->equalCmdParam(1, "BWIFI")) {
if(parser->getParamCount() < 3) {
logger.error("CMD SET BWIFI ERROR: Too few arguments");
logger.info("Syntax: SET BWIFI <B64SSID> <B64PASSWORD>");
} else {
const char * b64ssid = parser->getCmdParam(2);
const char * b64pass = parser->getCmdParam(3);
unsigned int b64ssidlength = 0;
unsigned int b64passlength = 0;
unsigned int ssidlength = decode_base64_length_null(b64ssid, &b64ssidlength);
unsigned int passlength = decode_base64_length_null(b64pass, &b64passlength);
// alloc the strings and set them to 0 (null terminating)
char ssid[ssidlength+1];
memset(ssid, 0, ssidlength+1);
char pass[passlength+1];
memset(pass, 0, passlength+1);
// make a pointer to pass
char *ppass = pass;
decode_base64((const unsigned char *)b64ssid, b64ssidlength, (unsigned char*)ssid);
if (!lengthCheck(ssid, 32, "CMD SET BWIFI", "SSID")) {
return;
}
if ((b64pass!=NULL) && (b64passlength > 0)) {
decode_base64((const unsigned char *)b64pass, b64passlength, (unsigned char*)pass);
if (!lengthCheck(pass, 64, "CMD SET BWIFI", "Password")) {
return;
}
} else {
// set the pointer for pass to null for no password
ppass = NULL;
}
WiFiNetwork::setWiFiCredentials(ssid, ppass);
logger.info("CMD SET BWIFI OK: New wifi credentials set, reconnecting");
}
void cmdSet(CmdParser* parser) {
if (parser->getParamCount() != 1) {
if (parser->equalCmdParam(1, "WIFI")) {
if (parser->getParamCount() < 3) {
logger.error("CMD SET WIFI ERROR: Too few arguments");
logger.info("Syntax: SET WIFI \"<SSID>\" \"<PASSWORD>\"");
} else {
logger.error("CMD SET ERROR: Unrecognized variable to set");
const char* sc_ssid = parser->getCmdParam(2);
const char* sc_pw = parser->getCmdParam(3);
if (!lengthCheck(sc_ssid, 32, "CMD SET WIFI", "SSID")
&& !lengthCheck(sc_pw, 64, "CMD SET WIFI", "Password")) {
return;
}
WiFiNetwork::setWiFiCredentials(sc_ssid, sc_pw);
logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting");
}
} else if (parser->equalCmdParam(1, "BWIFI")) {
if (parser->getParamCount() < 3) {
logger.error("CMD SET BWIFI ERROR: Too few arguments");
logger.info("Syntax: SET BWIFI <B64SSID> <B64PASSWORD>");
} else {
const char* b64ssid = parser->getCmdParam(2);
const char* b64pass = parser->getCmdParam(3);
unsigned int b64ssidlength = 0;
unsigned int b64passlength = 0;
unsigned int ssidlength
= decode_base64_length_null(b64ssid, &b64ssidlength);
unsigned int passlength
= decode_base64_length_null(b64pass, &b64passlength);
// alloc the strings and set them to 0 (null terminating)
char ssid[ssidlength + 1];
memset(ssid, 0, ssidlength + 1);
char pass[passlength + 1];
memset(pass, 0, passlength + 1);
// make a pointer to pass
char* ppass = pass;
decode_base64(
(const unsigned char*)b64ssid,
b64ssidlength,
(unsigned char*)ssid
);
if (!lengthCheck(ssid, 32, "CMD SET BWIFI", "SSID")) {
return;
}
if ((b64pass != NULL) && (b64passlength > 0)) {
decode_base64(
(const unsigned char*)b64pass,
b64passlength,
(unsigned char*)pass
);
if (!lengthCheck(pass, 64, "CMD SET BWIFI", "Password")) {
return;
}
} else {
// set the pointer for pass to null for no password
ppass = NULL;
}
WiFiNetwork::setWiFiCredentials(ssid, ppass);
logger.info("CMD SET BWIFI OK: New wifi credentials set, reconnecting");
}
} else {
logger.error("CMD SET ERROR: No variable to set");
logger.error("CMD SET ERROR: Unrecognized variable to set");
}
} else {
logger.error("CMD SET ERROR: No variable to set");
}
}
void printState() {
logger.info(
"SlimeVR Tracker, board: %d, hardware: %d, protocol: %d, firmware: %s, "
"address: %s, mac: %s, status: %d, wifi state: %d",
BOARD,
HARDWARE_MCU,
PROTOCOL_VERSION,
FIRMWARE_VERSION,
WiFiNetwork::getAddress().toString().c_str(),
WiFi.macAddress().c_str(),
statusManager.getStatus(),
WiFiNetwork::getWiFiState()
);
for (auto& sensor : sensorManager.getSensors()) {
logger.info(
"Sensor[%d]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
sensor->getSensorId(),
getIMUNameByType(sensor->getSensorType()),
UNPACK_QUATERNION(sensor->getFusedRotation()),
sensor->isWorking() ? "true" : "false",
sensor->getHadData() ? "true" : "false"
);
}
logger.info(
"Battery voltage: %.3f, level: %.1f%%",
battery.getVoltage(),
battery.getLevel() * 100
);
}
void cmdGet(CmdParser* parser) {
if (parser->getParamCount() < 2) {
return;
}
if (parser->equalCmdParam(1, "INFO")) {
printState();
// We don't want to print this on every timed state output
logger.info("Git commit: %s", GIT_REV);
}
if (parser->equalCmdParam(1, "CONFIG")) {
String str
= "BOARD=%d\n"
"IMU=%d\n"
"SECOND_IMU=%d\n"
"IMU_ROTATION=%f\n"
"SECOND_IMU_ROTATION=%f\n"
"BATTERY_MONITOR=%d\n"
"BATTERY_SHIELD_RESISTANCE=%d\n"
"BATTERY_SHIELD_R1=%d\n"
"BATTERY_SHIELD_R2=%d\n"
"PIN_IMU_SDA=%d\n"
"PIN_IMU_SCL=%d\n"
"PIN_IMU_INT=%d\n"
"PIN_IMU_INT_2=%d\n"
"PIN_BATTERY_LEVEL=%d\n"
"LED_PIN=%d\n"
"LED_INVERTED=%d\n";
Serial.printf(
str.c_str(),
BOARD,
static_cast<int>(sensorManager.getSensorType(0)),
static_cast<int>(sensorManager.getSensorType(1)),
IMU_ROTATION,
SECOND_IMU_ROTATION,
BATTERY_MONITOR,
BATTERY_SHIELD_RESISTANCE,
BATTERY_SHIELD_R1,
BATTERY_SHIELD_R2,
PIN_IMU_SDA,
PIN_IMU_SCL,
PIN_IMU_INT,
PIN_IMU_INT_2,
PIN_BATTERY_LEVEL,
LED_PIN,
LED_INVERTED
);
}
if (parser->equalCmdParam(1, "TEST")) {
logger.info(
"[TEST] Board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, "
"mac: %s, status: %d, wifi state: %d",
BOARD,
HARDWARE_MCU,
PROTOCOL_VERSION,
FIRMWARE_VERSION,
WiFiNetwork::getAddress().toString().c_str(),
WiFi.macAddress().c_str(),
statusManager.getStatus(),
WiFiNetwork::getWiFiState()
);
auto& sensor0 = sensorManager.getSensors()[0];
sensor0->motionLoop();
logger.info(
"[TEST] Sensor[0]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
getIMUNameByType(sensor0->getSensorType()),
UNPACK_QUATERNION(sensor0->getFusedRotation()),
sensor0->isWorking() ? "true" : "false",
sensor0->getHadData() ? "true" : "false"
);
if (!sensor0->getHadData()) {
logger.error("[TEST] Sensor[0] didn't send any data yet!");
} else {
logger.info("[TEST] Sensor[0] sent some data, looks working.");
}
}
void printState() {
logger.info(
"SlimeVR Tracker, board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
BOARD,
HARDWARE_MCU,
PROTOCOL_VERSION,
FIRMWARE_VERSION,
WiFiNetwork::getAddress().toString().c_str(),
WiFi.macAddress().c_str(),
statusManager.getStatus(),
WiFiNetwork::getWiFiState()
);
for (auto &sensor : sensorManager.getSensors()) {
logger.info(
"Sensor[%d]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
sensor->getSensorId(),
getIMUNameByType(sensor->getSensorType()),
UNPACK_QUATERNION(sensor->getFusedRotation()),
sensor->isWorking() ? "true" : "false",
sensor->getHadData() ? "true" : "false"
);
}
logger.info(
"Battery voltage: %.3f, level: %.1f%%",
battery.getVoltage(),
battery.getLevel() * 100
);
}
if (parser->equalCmdParam(1, "WIFISCAN")) {
logger.info("[WSCAN] Scanning for WiFi networks...");
void cmdGet(CmdParser * parser) {
if (parser->getParamCount() < 2) {
return;
}
// Scan would fail if connecting, stop connecting before scan
if (WiFi.status() != WL_CONNECTED) {
WiFi.disconnect();
}
if (WiFiNetwork::isProvisioning()) {
WiFiNetwork::stopProvisioning();
}
if (parser->equalCmdParam(1, "INFO")) {
printState();
WiFi.scanNetworks();
// We don't want to print this on every timed state output
logger.info("Git commit: %s", GIT_REV);
}
if (parser->equalCmdParam(1, "CONFIG")) {
String str =
"BOARD=%d\n"
"IMU=%d\n"
"SECOND_IMU=%d\n"
"IMU_ROTATION=%f\n"
"SECOND_IMU_ROTATION=%f\n"
"BATTERY_MONITOR=%d\n"
"BATTERY_SHIELD_RESISTANCE=%d\n"
"BATTERY_SHIELD_R1=%d\n"
"BATTERY_SHIELD_R2=%d\n"
"PIN_IMU_SDA=%d\n"
"PIN_IMU_SCL=%d\n"
"PIN_IMU_INT=%d\n"
"PIN_IMU_INT_2=%d\n"
"PIN_BATTERY_LEVEL=%d\n"
"LED_PIN=%d\n"
"LED_INVERTED=%d\n";
Serial.printf(
str.c_str(),
BOARD,
static_cast<int>(sensorManager.getSensorType(0)),
static_cast<int>(sensorManager.getSensorType(1)),
IMU_ROTATION,
SECOND_IMU_ROTATION,
BATTERY_MONITOR,
BATTERY_SHIELD_RESISTANCE,
BATTERY_SHIELD_R1,
BATTERY_SHIELD_R2,
PIN_IMU_SDA,
PIN_IMU_SCL,
PIN_IMU_INT,
PIN_IMU_INT_2,
PIN_BATTERY_LEVEL,
LED_PIN,
LED_INVERTED
);
}
if (parser->equalCmdParam(1, "TEST")) {
logger.info(
"[TEST] Board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
BOARD,
HARDWARE_MCU,
PROTOCOL_VERSION,
FIRMWARE_VERSION,
WiFiNetwork::getAddress().toString().c_str(),
WiFi.macAddress().c_str(),
statusManager.getStatus(),
WiFiNetwork::getWiFiState()
);
auto& sensor0 = sensorManager.getSensors()[0];
sensor0->motionLoop();
logger.info(
"[TEST] Sensor[0]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
getIMUNameByType(sensor0->getSensorType()),
UNPACK_QUATERNION(sensor0->getFusedRotation()),
sensor0->isWorking() ? "true" : "false",
sensor0->getHadData() ? "true" : "false"
);
if(!sensor0->getHadData()) {
logger.error("[TEST] Sensor[0] didn't send any data yet!");
} else {
logger.info("[TEST] Sensor[0] sent some data, looks working.");
}
}
if (parser->equalCmdParam(1, "WIFISCAN")) {
logger.info("[WSCAN] Scanning for WiFi networks...");
// Scan would fail if connecting, stop connecting before scan
if (WiFi.status() != WL_CONNECTED) {
WiFi.disconnect();
}
if (WiFiNetwork::isProvisioning()) {
WiFiNetwork::stopProvisioning();
int scanRes = WiFi.scanComplete();
if (scanRes >= 0) {
logger.info("[WSCAN] Found %d networks:", scanRes);
for (int i = 0; i < scanRes; i++) {
logger.info(
"[WSCAN] %d:\t%02d\t%s\t(%d)\t%s",
i,
WiFi.SSID(i).length(),
WiFi.SSID(i).c_str(),
WiFi.RSSI(i),
((WiFi.encryptionType(i) == 0) ? "OPEN" : "PASSWD")
);
}
WiFi.scanDelete();
} else {
logger.info("[WSCAN] Scan failed!");
}
WiFi.scanNetworks();
int scanRes = WiFi.scanComplete();
if (scanRes >= 0) {
logger.info("[WSCAN] Found %d networks:", scanRes);
for (int i = 0; i < scanRes; i++) {
logger.info("[WSCAN] %d:\t%02d\t%s\t(%d)\t%s",
i, WiFi.SSID(i).length(), WiFi.SSID(i).c_str(), WiFi.RSSI(i),
((WiFi.encryptionType(i) == 0) ? "OPEN" : "PASSWD")
);
}
WiFi.scanDelete();
} else {
logger.info("[WSCAN] Scan failed!");
}
// Restore conencting state
if (WiFi.status() != WL_CONNECTED) {
WiFi.begin();
}
}
}
void cmdReboot(CmdParser * parser) {
logger.info("REBOOT");
ESP.restart();
}
void cmdFactoryReset(CmdParser * parser) {
logger.info("FACTORY RESET");
configuration.reset();
WiFi.disconnect(true); // Clear WiFi credentials
#if ESP8266
ESP.eraseConfig(); // Clear ESP config
#elif ESP32
nvs_flash_erase();
#else
#warning SERIAL COMMAND FACTORY RESET NOT SUPPORTED
logger.info("FACTORY RESET NOT SUPPORTED");
return;
#endif
#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
#warning FACTORY RESET does not clear your hardcoded WiFi credentials!
logger.warn("FACTORY RESET does not clear your hardcoded WiFi credentials!");
#endif
delay(3000);
ESP.restart();
}
void cmdTemperatureCalibration(CmdParser* parser) {
if (parser->getParamCount() > 1) {
if (parser->equalCmdParam(1, "PRINT")) {
for (auto &sensor : sensorManager.getSensors()) {
sensor->printTemperatureCalibrationState();
}
return;
} else if (parser->equalCmdParam(1, "DEBUG")) {
for (auto &sensor : sensorManager.getSensors()) {
sensor->printDebugTemperatureCalibrationState();
}
return;
} else if (parser->equalCmdParam(1, "RESET")) {
for (auto &sensor : sensorManager.getSensors()) {
sensor->resetTemperatureCalibrationState();
}
return;
} else if (parser->equalCmdParam(1, "SAVE")) {
for (auto &sensor : sensorManager.getSensors()) {
sensor->saveTemperatureCalibration();
}
return;
}
}
logger.info("Usage:");
logger.info(" TCAL PRINT: print current temperature calibration config");
logger.info(" TCAL DEBUG: print debug values for the current temperature calibration profile");
logger.info(" TCAL RESET: reset current temperature calibration in RAM (does not delete already saved)");
logger.info(" TCAL SAVE: save current temperature calibration to persistent flash");
logger.info("Note:");
logger.info(" Temperature calibration config saves automatically when calibration percent is at 100%");
}
void setUp() {
cmdCallbacks.addCmd("SET", &cmdSet);
cmdCallbacks.addCmd("GET", &cmdGet);
cmdCallbacks.addCmd("FRST", &cmdFactoryReset);
cmdCallbacks.addCmd("REBOOT", &cmdReboot);
cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration);
}
void update() {
cmdCallbacks.updateCmdProcessing(&cmdParser, &cmdBuffer, &Serial);
}
// Restore conencting state
if (WiFi.status() != WL_CONNECTED) {
WiFi.begin();
}
}
}
void cmdReboot(CmdParser* parser) {
logger.info("REBOOT");
ESP.restart();
}
void cmdFactoryReset(CmdParser* parser) {
logger.info("FACTORY RESET");
configuration.reset();
WiFi.disconnect(true); // Clear WiFi credentials
#if ESP8266
ESP.eraseConfig(); // Clear ESP config
#elif ESP32
nvs_flash_erase();
#else
#warning SERIAL COMMAND FACTORY RESET NOT SUPPORTED
logger.info("FACTORY RESET NOT SUPPORTED");
return;
#endif
#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
#warning FACTORY RESET does not clear your hardcoded WiFi credentials!
logger.warn("FACTORY RESET does not clear your hardcoded WiFi credentials!");
#endif
delay(3000);
ESP.restart();
}
void cmdTemperatureCalibration(CmdParser* parser) {
if (parser->getParamCount() > 1) {
if (parser->equalCmdParam(1, "PRINT")) {
for (auto& sensor : sensorManager.getSensors()) {
sensor->printTemperatureCalibrationState();
}
return;
} else if (parser->equalCmdParam(1, "DEBUG")) {
for (auto& sensor : sensorManager.getSensors()) {
sensor->printDebugTemperatureCalibrationState();
}
return;
} else if (parser->equalCmdParam(1, "RESET")) {
for (auto& sensor : sensorManager.getSensors()) {
sensor->resetTemperatureCalibrationState();
}
return;
} else if (parser->equalCmdParam(1, "SAVE")) {
for (auto& sensor : sensorManager.getSensors()) {
sensor->saveTemperatureCalibration();
}
return;
}
}
logger.info("Usage:");
logger.info(" TCAL PRINT: print current temperature calibration config");
logger.info(
" TCAL DEBUG: print debug values for the current temperature calibration "
"profile"
);
logger.info(
" TCAL RESET: reset current temperature calibration in RAM (does not delete "
"already saved)"
);
logger.info(" TCAL SAVE: save current temperature calibration to persistent flash"
);
logger.info("Note:");
logger.info(
" Temperature calibration config saves automatically when calibration percent "
"is at 100%"
);
}
void setUp() {
cmdCallbacks.addCmd("SET", &cmdSet);
cmdCallbacks.addCmd("GET", &cmdGet);
cmdCallbacks.addCmd("FRST", &cmdFactoryReset);
cmdCallbacks.addCmd("REBOOT", &cmdReboot);
cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration);
}
void update() { cmdCallbacks.updateCmdProcessing(&cmdParser, &cmdBuffer, &Serial); }
} // namespace SerialCommands

44
src/serial/serialcommands.h
View File

@@ -1,33 +1,33 @@
/*
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_SERIALCOMMANDS_H_
#define SLIMEVR_SERIALCOMMANDS_H_
namespace SerialCommands {
void setUp();
void update();
void printState();
}
void setUp();
void update();
void printState();
} // namespace SerialCommands
#endif // SLIMEVR_SERIALCOMMANDS_H_
#endif // SLIMEVR_SERIALCOMMANDS_H_

42
src/status/Status.cpp
View File

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

27
src/status/Status.h
View File

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

51
src/status/StatusManager.cpp
View File

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

31
src/status/StatusManager.h
View File

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

34
src/utils.h
View File

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

191
test/i2cscan.cpp
View File

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