├── .gitignore ├── lib ├── .clang-format ├── magneto │ ├── madgwick.h │ ├── dmpmag.h │ ├── dmpmag.hpp │ ├── mahony.h │ ├── magneto1.4.h │ └── mymathlib_matrix.h ├── ICM20948 │ ├── AK09916_ENUMERATIONS.h │ ├── ISSUE_TEMPLATE.md │ ├── library.properties │ ├── AK09916_REGISTERS.h │ ├── README.md │ ├── License.md │ └── keywords.txt ├── i2cscan │ └── i2cscan.h ├── magnetometers │ ├── qmc5883l.h │ └── hmc5883l.h ├── README ├── ota │ ├── ota.h │ └── ota.cpp ├── math │ └── helper_3dmath.cpp └── ICM42688 │ └── MMC5983MA.h ├── .github ├── FUNDING.yml └── workflows │ ├── release.yml │ └── actions.yml ├── src ├── motionprocessing │ ├── types.h │ └── OnlinePolyfit.h ├── status │ ├── Status.h │ ├── StatusManager.h │ ├── Status.cpp │ └── StatusManager.cpp ├── logging │ ├── Level.cpp │ ├── Level.h │ ├── Logger.cpp │ └── Logger.h ├── sensors │ ├── SensorFusionRestDetect.cpp │ ├── ErroneousSensor.cpp │ ├── SensorFusionDMP.h │ ├── sensoraddresses.h │ ├── SensorFusionRestDetect.h │ ├── bno055sensor.h │ ├── EmptySensor.h │ ├── ErroneousSensor.h │ ├── bno080sensor.h │ ├── icm42688sensor.h │ ├── mpu6050sensor.h │ ├── SensorManager.h │ ├── icm20948sensor.h │ ├── bno055sensor.cpp │ ├── SensorFusionDMP.cpp │ ├── sensor.cpp │ ├── axisremap.h │ ├── sensor.h │ ├── mpu9250sensor.h │ └── SensorFusion.h ├── utils.h ├── serial │ └── serialcommands.h ├── configuration │ ├── DeviceConfig.h │ ├── CalibrationConfig.cpp │ ├── Configuration.h │ └── CalibrationConfig.h ├── network │ ├── wifiprovisioning.h │ ├── manager.h │ ├── manager.cpp │ ├── wifihandler.h │ ├── wifiprovisioning.cpp │ ├── packets.h │ ├── featureflags.h │ └── connection.h ├── calibration.h ├── GlobalVars.h ├── credentials.h ├── globals.h ├── defines_sensitivity.h ├── defines_bmi160.h ├── LEDManager.h ├── batterymonitor.h ├── debug.h ├── consts.h ├── main.cpp ├── batterymonitor.cpp └── LEDManager.cpp ├── .editorconfig ├── scripts └── get_git_commit.py ├── test ├── README └── i2cscan.cpp ├── .clang-format ├── LICENSE-MIT ├── platformio-tools.ini ├── include └── README ├── platformio.ini ├── ci └── build.py └── CONTRIBUTING.md /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/* 3 | build/ 4 | venv/ 5 | -------------------------------------------------------------------------------- /lib/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | DisableFormat: true 3 | SortIncludes: Never 4 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | 3 | github: SlimeVR 4 | -------------------------------------------------------------------------------- /src/motionprocessing/types.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTIONPROCESSING_TYPES_H 2 | #define MOTIONPROCESSING_TYPES_H 3 | 4 | #if SENSORS_DOUBLE_PRECISION 5 | typedef double sensor_real_t; 6 | #else 7 | typedef float sensor_real_t; 8 | #define VQF_SINGLE_PRECISION 9 | #endif 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: Releases 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | 8 | jobs: 9 | 10 | build: 11 | runs-on: ubuntu-latest 12 | permissions: 13 | contents: write 14 | steps: 15 | - uses: actions/checkout@v2 16 | - uses: ncipollo/release-action@v1 17 | with: 18 | artifacts: "./build/*.bin" 19 | draft: true 20 | token: ${{ secrets.GITHUB_TOKEN }} -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | # EditorConfig is awesome: https://EditorConfig.org 2 | 3 | # top-most EditorConfig file 4 | root = true 5 | 6 | [*] 7 | indent_style = tab 8 | indent_size = 4 9 | end_of_line = lf 10 | charset = utf-8 11 | trim_trailing_whitespace = true 12 | insert_final_newline = true 13 | 14 | # the files need the trailing withe space at the end of = else it does not work 15 | [{platformio.ini,platformio-tools.ini}] 16 | trim_trailing_whitespace = false 17 | -------------------------------------------------------------------------------- /lib/magneto/madgwick.h: -------------------------------------------------------------------------------- 1 | #ifndef _MADGWICK_H_ 2 | #define _MADGWICK_H_ 3 | 4 | #include "helper_3dmath.h" 5 | 6 | template 7 | class Madgwick { 8 | 9 | static constexpr float madgwickBeta = 0.1f; 10 | 11 | public: 12 | void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat); 13 | void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat); 14 | }; 15 | 16 | #include "madgwick.hpp" 17 | 18 | #endif /* _MADGWICK_H_ */ 19 | -------------------------------------------------------------------------------- /src/status/Status.h: -------------------------------------------------------------------------------- 1 | #ifndef STATUS_STATUS_H 2 | #define STATUS_STATUS_H 3 | 4 | namespace SlimeVR 5 | { 6 | namespace Status 7 | { 8 | enum Status 9 | { 10 | LOADING = 1 << 0, 11 | LOW_BATTERY = 1 << 1, 12 | IMU_ERROR = 1 << 2, 13 | WIFI_CONNECTING = 1 << 3, 14 | SERVER_CONNECTING = 1 << 4 15 | }; 16 | 17 | const char *statusToString(Status status); 18 | } 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /lib/ICM20948/AK09916_ENUMERATIONS.h: -------------------------------------------------------------------------------- 1 | #ifndef _AK09916_ENUMERATIONS_H_ 2 | #define _AK09916_ENUMERATIONS_H_ 3 | 4 | typedef enum 5 | { 6 | AK09916_mode_power_down = 0x00, 7 | AK09916_mode_single = (0x01 << 0), 8 | AK09916_mode_cont_10hz = (0x01 << 1), 9 | AK09916_mode_cont_20hz = (0x02 << 1), 10 | AK09916_mode_cont_50hz = (0x03 << 1), 11 | AK09916_mode_cont_100hz = (0x04 << 1), 12 | AK09916_mode_self_test = (0x01 << 4), 13 | } AK09916_mode_e; 14 | 15 | #endif // _AK09916_ENUMERATIONS_H_ -------------------------------------------------------------------------------- /lib/i2cscan/i2cscan.h: -------------------------------------------------------------------------------- 1 | #ifndef _I2CSCAN_H_ 2 | #define _I2CSCAN_H_ 1 3 | 4 | #include 5 | #include 6 | 7 | namespace I2CSCAN { 8 | void scani2cports(); 9 | bool checkI2C(uint8_t i, uint8_t j); 10 | bool hasDevOnBus(uint8_t addr); 11 | uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound); 12 | int clearBus(uint8_t SDA, uint8_t SCL); 13 | boolean inArray(uint8_t value, uint8_t* arr, size_t arrSize); 14 | } 15 | 16 | #endif // _I2CSCAN_H_ -------------------------------------------------------------------------------- /scripts/get_git_commit.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import os 3 | 4 | revision = "" 5 | 6 | env_rev = os.environ.get("GIT_REV") 7 | if not env_rev is None and env_rev != "": 8 | revision = env_rev 9 | else: 10 | try: 11 | revision = ( 12 | subprocess.check_output(["git", "rev-parse", "HEAD"]) 13 | .strip() 14 | .decode("utf-8") 15 | ) 16 | except Exception: 17 | revision = "NOT_GIT" 18 | 19 | print(f"'-DGIT_REV=\"{revision}\"'") 20 | -------------------------------------------------------------------------------- /lib/magneto/dmpmag.h: -------------------------------------------------------------------------------- 1 | #ifndef _DMPMAG_H_ 2 | #define _DMPMAG_H_ 3 | 4 | #include "quat.h" 5 | 6 | template 7 | class DMPMag { 8 | static constexpr T magCorrRatio = 0.02; 9 | public: 10 | void update(T oqwxyz[4], const T iqwxyz[4], const T Grav[3], const T Mxyz[3]); 11 | private: 12 | Quat getQuatDCM(const T acc[3], const T mag[3]); 13 | Quat getCorrection(const T acc[3], const T mag[3], Quat quat); 14 | 15 | Quat correction{0, 0, 0, 0}; 16 | }; 17 | 18 | #include "dmpmag.hpp" 19 | 20 | #endif /* _DMPMAG_H_ */ 21 | -------------------------------------------------------------------------------- /test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PlatformIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PlatformIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /src/logging/Level.cpp: -------------------------------------------------------------------------------- 1 | #include "Level.h" 2 | 3 | namespace SlimeVR 4 | { 5 | namespace Logging 6 | { 7 | const char *levelToString(Level level) 8 | { 9 | switch (level) 10 | { 11 | case TRACE: 12 | return "TRACE"; 13 | case DEBUG: 14 | return "DEBUG"; 15 | case INFO: 16 | return "INFO"; 17 | case WARN: 18 | return "WARN"; 19 | case ERROR: 20 | return "ERROR"; 21 | case FATAL: 22 | return "FATAL"; 23 | default: 24 | return "UNKNOWN"; 25 | } 26 | } 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /lib/ICM20948/ISSUE_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | ### Subject of the issue 2 | Describe your issue here. 3 | 4 | ### Your workbench 5 | * What platform are you using? 6 | * What version of the device are you using? Is there a firmware version? 7 | * How is the device wired to your platform? 8 | * How is everything being powered? 9 | * Are there any additional details that may help us help you? 10 | 11 | ### Steps to reproduce 12 | Tell us how to reproduce this issue. Please post stripped down example code demonstrating your issue to a gist. 13 | 14 | ### Expected behaviour 15 | Tell us what should happen 16 | 17 | ### Actual behaviour 18 | Tell us what happens instead 19 | -------------------------------------------------------------------------------- /src/logging/Level.h: -------------------------------------------------------------------------------- 1 | #ifndef LOGGING_LEVEL_H 2 | 3 | #define LOG_LEVEL_TRACE 0 4 | #define LOG_LEVEL_DEBUG 1 5 | #define LOG_LEVEL_INFO 2 6 | #define LOG_LEVEL_WARN 3 7 | #define LOG_LEVEL_ERROR 4 8 | #define LOG_LEVEL_FATAL 5 9 | 10 | namespace SlimeVR 11 | { 12 | namespace Logging 13 | { 14 | enum Level 15 | { 16 | TRACE = LOG_LEVEL_TRACE, 17 | DEBUG = LOG_LEVEL_DEBUG, 18 | INFO = LOG_LEVEL_INFO, 19 | WARN = LOG_LEVEL_WARN, 20 | ERROR = LOG_LEVEL_ERROR, 21 | FATAL = LOG_LEVEL_FATAL 22 | }; 23 | 24 | const char *levelToString(Level level); 25 | } 26 | } 27 | 28 | #define LOGGING_LEVEL_H 29 | #endif 30 | -------------------------------------------------------------------------------- /src/status/StatusManager.h: -------------------------------------------------------------------------------- 1 | #ifndef STATUS_STATUSMANAGER_H 2 | #define STATUS_STATUSMANAGER_H 3 | 4 | #include "Status.h" 5 | #include "logging/Logger.h" 6 | 7 | namespace SlimeVR 8 | { 9 | namespace Status 10 | { 11 | class StatusManager 12 | { 13 | public: 14 | void setStatus(Status status, bool value); 15 | bool hasStatus(Status status); 16 | uint32_t getStatus() { 17 | return m_Status; 18 | }; 19 | 20 | private: 21 | uint32_t m_Status; 22 | 23 | Logging::Logger m_Logger = Logging::Logger("StatusManager"); 24 | }; 25 | } 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /.github/workflows/actions.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | pull_request: 6 | 7 | jobs: 8 | build: 9 | runs-on: ubuntu-20.04 10 | 11 | steps: 12 | - uses: actions/checkout@v2 13 | 14 | - name: Set up Python 15 | uses: actions/setup-python@v1 16 | 17 | - name: Install pio and its dependencies 18 | run: | 19 | python -m pip install --upgrade pip 20 | pip install platformio 21 | 22 | - name: Run builds 23 | run: python ./ci/build.py 24 | 25 | - name: Upload binaries 26 | uses: actions/upload-artifact@v2 27 | with: 28 | name: binaries 29 | path: ./build/*.bin 30 | -------------------------------------------------------------------------------- /lib/magnetometers/qmc5883l.h: -------------------------------------------------------------------------------- 1 | #define QMC_DEVADDR 0x0D 2 | #define QMC_RA_DATA 0x00 3 | #define QMC_RA_CONTROL 0x09 4 | #define QMC_RA_RESET 0x0B 5 | 6 | #define QMC_CFG_MODE_STANDBY 0b00 7 | #define QMC_CFG_MODE_CONTINUOUS 0b01 8 | #define QMC_CFG_ODR_10HZ 0b00 << 2 9 | #define QMC_CFG_ODR_50HZ 0b01 << 2 10 | #define QMC_CFG_ODR_100HZ 0b10 << 2 11 | #define QMC_CFG_ODR_200HZ 0b11 << 2 12 | #define QMC_CFG_RNG_2G 0b00 << 4 13 | #define QMC_CFG_RNG_8G 0b01 << 4 14 | #define QMC_CFG_OSR_512 0b00 << 6 15 | #define QMC_CFG_OSR_256 0b01 << 6 16 | #define QMC_CFG_OSR_128 0b10 << 6 17 | #define QMC_CFG_OSR_64 0b11 << 6 18 | -------------------------------------------------------------------------------- /src/status/Status.cpp: -------------------------------------------------------------------------------- 1 | #include "Status.h" 2 | 3 | namespace SlimeVR 4 | { 5 | namespace Status 6 | { 7 | const char *statusToString(Status status) 8 | { 9 | switch (status) 10 | { 11 | case LOADING: 12 | return "LOADING"; 13 | case LOW_BATTERY: 14 | return "LOW_BATTERY"; 15 | case IMU_ERROR: 16 | return "IMU_ERROR"; 17 | case WIFI_CONNECTING: 18 | return "WIFI_CONNECTING"; 19 | case SERVER_CONNECTING: 20 | return "SERVER_CONNECTING"; 21 | default: 22 | return "UNKNOWN"; 23 | } 24 | } 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -4 4 | AlignAfterOpenBracket: BlockIndent 5 | AlignOperands: AlignAfterOperator 6 | AlignTrailingComments: false 7 | AllowAllArgumentsOnNextLine: false 8 | AllowAllConstructorInitializersOnNextLine: false 9 | AllowAllParametersOfDeclarationOnNextLine: false 10 | AllowShortIfStatementsOnASingleLine: Never 11 | AllowShortLoopsOnASingleLine: false 12 | BinPackArguments: false 13 | BinPackParameters: false 14 | BreakBeforeBinaryOperators: All 15 | BreakConstructorInitializers: BeforeComma 16 | BreakInheritanceList: BeforeComma 17 | ColumnLimit: 88 18 | DerivePointerAlignment: false 19 | IndentWidth: 4 20 | InsertBraces: true 21 | InsertTrailingCommas: Wrapped 22 | PackConstructorInitializers: Never 23 | TabWidth: 4 24 | UseTab: ForContinuationAndIndentation 25 | -------------------------------------------------------------------------------- /lib/ICM20948/library.properties: -------------------------------------------------------------------------------- 1 | name=SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library 2 | version=1.2.9 3 | author=SparkFun Electronics 4 | maintainer=SparkFun Electronics 5 | sentence=Use the low-power high-resolution ICM 20948 9 DoF IMU from Invensense with I2C or SPI. Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™). 6 | paragraph=The SparkFun 9DoF IMU Breakout uses the Invensense ICM-20948 -- a system-in-package featuring acceleration full-scales of ±2 / ±4 / ±8 / ±16 (g), rotational full-scales of ±250 / ±500 / ±1000 / ±2000 (°/sec) and a magnetic field full scale of ±4800 µT. The ICM-20948 can be accessed via either I2C (400 kHz) or SPI (7 MHz) 7 | category=Sensors 8 | url=https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary 9 | architectures=* 10 | -------------------------------------------------------------------------------- /src/status/StatusManager.cpp: -------------------------------------------------------------------------------- 1 | #include "StatusManager.h" 2 | 3 | namespace SlimeVR 4 | { 5 | namespace Status 6 | { 7 | void StatusManager::setStatus(Status status, bool value) 8 | { 9 | if (value) 10 | { 11 | if (m_Status & status) 12 | { 13 | return; 14 | } 15 | 16 | m_Logger.trace("Added status %s", statusToString(status)); 17 | 18 | m_Status |= status; 19 | } 20 | else 21 | { 22 | if (!(m_Status & status)) 23 | { 24 | return; 25 | } 26 | 27 | m_Logger.trace("Removed status %s", statusToString(status)); 28 | 29 | m_Status &= ~status; 30 | } 31 | } 32 | 33 | bool StatusManager::hasStatus(Status status) 34 | { 35 | return (m_Status & status) == status; 36 | } 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /src/sensors/SensorFusionRestDetect.cpp: -------------------------------------------------------------------------------- 1 | #include "SensorFusionRestDetect.h" 2 | 3 | namespace SlimeVR 4 | { 5 | namespace Sensors 6 | { 7 | #if !SENSOR_FUSION_WITH_RESTDETECT 8 | void SensorFusionRestDetect::updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat) 9 | { 10 | if (deltat < 0) deltat = accTs; 11 | restDetection.updateAcc(deltat, Axyz); 12 | SensorFusion::updateAcc(Axyz, deltat); 13 | } 14 | 15 | void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat) 16 | { 17 | if (deltat < 0) deltat = gyrTs; 18 | restDetection.updateGyr(deltat, Gxyz); 19 | SensorFusion::updateGyro(Gxyz, deltat); 20 | } 21 | #endif 22 | 23 | bool SensorFusionRestDetect::getRestDetected() 24 | { 25 | #if !SENSOR_FUSION_WITH_RESTDETECT 26 | return restDetection.getRestDetected(); 27 | #elif SENSOR_USE_VQF 28 | return vqf.getRestDetected(); 29 | #endif 30 | } 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /LICENSE-MIT: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Eiren Rain and SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /lib/magnetometers/hmc5883l.h: -------------------------------------------------------------------------------- 1 | #define HMC_DEVADDR 0x1E 2 | #define HMC_RA_CFGA 0x00 3 | #define HMC_RA_CFGB 0x01 4 | #define HMC_RA_MODE 0x02 5 | #define HMC_RA_DATA 0x03 6 | 7 | #define HMC_CFGA_DATA_RATE_0_75 0b000 << 2 8 | #define HMC_CFGA_DATA_RATE_1_5 0b001 << 2 9 | #define HMC_CFGA_DATA_RATE_3 0b010 << 2 10 | #define HMC_CFGA_DATA_RATE_7_5 0b011 << 2 11 | #define HMC_CFGA_DATA_RATE_15 0b100 << 2 12 | #define HMC_CFGA_DATA_RATE_30 0b101 << 2 13 | #define HMC_CFGA_DATA_RATE_75 0b110 << 2 14 | #define HMC_CFGA_AVG_SAMPLES_1 0b00 << 5 15 | #define HMC_CFGA_AVG_SAMPLES_2 0b01 << 5 16 | #define HMC_CFGA_AVG_SAMPLES_4 0b10 << 5 17 | #define HMC_CFGA_AVG_SAMPLES_8 0b11 << 5 18 | #define HMC_CFGA_BIAS_NORMAL 0b00 19 | #define HMC_CFGA_BIAS_POS 0b01 20 | #define HMC_CFGA_BIAS_NEG 0b10 21 | 22 | #define HMC_CFGB_GAIN_0_88 0 23 | #define HMC_CFGB_GAIN_1_30 1 << 5 24 | #define HMC_CFGB_GAIN_1_90 2 << 5 25 | #define HMC_CFGB_GAIN_2_50 3 << 5 26 | #define HMC_CFGB_GAIN_4_00 4 << 5 27 | #define HMC_CFGB_GAIN_4_70 5 << 5 28 | #define HMC_CFGB_GAIN_5_60 6 << 5 29 | #define HMC_CFGB_GAIN_8_10 7 << 5 30 | 31 | #define HMC_MODE_HIGHSPEED 1 << 7 32 | #define HMC_MODE_READ_CONTINUOUS 0b00 33 | #define HMC_MODE_READ_SINGLEMEAS 0b01 34 | -------------------------------------------------------------------------------- /lib/ota/ota.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _OTA_H_ 25 | #define _OTA_H 1 26 | 27 | #include 28 | 29 | namespace OTA { 30 | void otaSetup(const char * const otaPassword); 31 | void otaUpdate(); 32 | } 33 | 34 | #endif // _OTA_H_ -------------------------------------------------------------------------------- /src/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef UTILS_H 25 | #define UTILS_H 26 | 27 | #define UNPACK_VECTOR(V) V.x, V.y, V.z 28 | #define UNPACK_VECTOR_ARRAY(V) V[0], V[1], V[2] 29 | #define UNPACK_QUATERNION(Q) Q.x, Q.y, Q.z, Q.w 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /src/serial/serialcommands.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_SERIALCOMMANDS_H_ 25 | #define SLIMEVR_SERIALCOMMANDS_H_ 26 | 27 | namespace SerialCommands { 28 | void setUp(); 29 | void update(); 30 | void printState(); 31 | } 32 | 33 | #endif // SLIMEVR_SERIALCOMMANDS_H_ 34 | -------------------------------------------------------------------------------- /platformio-tools.ini: -------------------------------------------------------------------------------- 1 | [env] 2 | lib_deps= 3 | https://github.com/SlimeVR/CmdParser.git 4 | https://github.com/SlimeVR/base64_arduino.git 5 | monitor_speed = 115200 6 | framework = arduino 7 | build_flags = 8 | !python scripts/get_git_commit.py 9 | -O2 10 | -std=gnu++17 11 | build_unflags = 12 | -Os 13 | -std=gnu++11 14 | 15 | [env:BOARD_SLIMEVR] 16 | platform = espressif8266 @ 4.2.0 17 | board = esp12e 18 | 19 | [env:BOARD_SLIMEVR_DEV] 20 | platform = espressif8266 @ 4.2.0 21 | board = esp12e 22 | 23 | [env:BOARD_NODEMCU] 24 | platform = espressif8266 @ 4.2.0 25 | board = esp12e 26 | 27 | [env:BOARD_WEMOSD1MINI] 28 | platform = espressif8266 @ 4.2.0 29 | board = esp12e 30 | 31 | [env:BOARD_TTGO_TBASE] 32 | platform = espressif8266 @ 4.2.0 33 | board = esp12e 34 | 35 | [env:BOARD_WEMOSWROOM02] 36 | platform = espressif8266 @ 4.2.0 37 | board = esp12e 38 | 39 | [env:BOARD_WROOM32] 40 | platform = espressif32 @ 6.1.0 41 | board = esp32dev 42 | 43 | [env:BOARD_ESP01] 44 | platform = espressif32 @ 6.1.0 45 | board = esp32dev 46 | 47 | [env:BOARD_LOLIN_C3_MINI] 48 | platform = espressif32 @ 6.1.0 49 | build_flags = 50 | ${env.build_flags} 51 | -DESP32C3 52 | board = lolin_c3_mini 53 | 54 | [env:BOARD_BEETLE32C3] 55 | platform = espressif32 @ 6.1.0 56 | build_flags = 57 | ${env.build_flags} 58 | -DESP32C3 59 | board = dfrobot_beetle_esp32c3 60 | 61 | [env:BOARD_ES32C3DEVKITM1] 62 | platform = espressif32 @ 6.1.0 63 | build_flags = 64 | ${env.build_flags} 65 | -DESP32C3 66 | board = esp32-c3-devkitm-1 67 | -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /src/configuration/DeviceConfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_CONFIGURATION_DEVICECONFIG_H 25 | #define SLIMEVR_CONFIGURATION_DEVICECONFIG_H 26 | 27 | #include "CalibrationConfig.h" 28 | 29 | namespace SlimeVR { 30 | namespace Configuration { 31 | struct DeviceConfig { 32 | int32_t version; 33 | }; 34 | } 35 | } 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /src/network/wifiprovisioning.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_WIFIPROVISIONING_H_ 24 | #define SLIMEVR_WIFIPROVISIONING_H_ 25 | 26 | namespace WiFiNetwork { 27 | void upkeepProvisioning(); 28 | void startProvisioning(); 29 | void stopProvisioning(); 30 | bool isProvisioning(); 31 | void provideNeighbours(); 32 | } 33 | 34 | #endif // SLIMEVR_WIFIPROVISIONING_H_ 35 | -------------------------------------------------------------------------------- /src/calibration.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_CALIBRATION_H_ 24 | #define SLIMEVR_CALIBRATION_H_ 25 | 26 | #define CALIBRATION_TYPE_INTERNAL_GYRO 1 27 | #define CALIBRATION_TYPE_INTERNAL_ACCEL 2 28 | #define CALIBRATION_TYPE_INTERNAL_MAG 3 29 | #define CALIBRATION_TYPE_EXTERNAL_ALL 4 30 | #define CALIBRATION_TYPE_EXTERNAL_GYRO 5 31 | #define CALIBRATION_TYPE_EXTERNAL_ACCEL 6 32 | #define CALIBRATION_TYPE_EXTERNAL_MAG 7 33 | 34 | #endif // SLIMEVR_CALIBRATION_H_ -------------------------------------------------------------------------------- /src/network/manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2023 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_NETWORK_MANAGER_H_ 24 | #define SLIMEVR_NETWORK_MANAGER_H_ 25 | 26 | #include "globals.h" 27 | #include "packets.h" 28 | #include "wifihandler.h" 29 | #include "wifiprovisioning.h" 30 | 31 | namespace SlimeVR { 32 | namespace Network { 33 | 34 | class Manager { 35 | public: 36 | void setup(); 37 | void update(); 38 | 39 | private: 40 | bool m_IsConnected = false; 41 | }; 42 | 43 | } // namespace Network 44 | } // namespace SlimeVR 45 | 46 | #endif // SLIMEVR_NETWORK_MANAGER_H_ 47 | -------------------------------------------------------------------------------- /src/sensors/ErroneousSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "ErroneousSensor.h" 25 | #include "GlobalVars.h" 26 | 27 | namespace SlimeVR 28 | { 29 | namespace Sensors 30 | { 31 | void ErroneousSensor::motionSetup() 32 | { 33 | m_Logger.error("IMU of type %s failed to initialize", getIMUNameByType(m_ExpectedType)); 34 | } 35 | 36 | SensorStatus ErroneousSensor::getSensorState() 37 | { 38 | return SensorStatus::SENSOR_ERROR; 39 | }; 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /src/sensors/SensorFusionDMP.h: -------------------------------------------------------------------------------- 1 | #ifndef SLIMEVR_SENSORFUSIONDMP_H 2 | #define SLIMEVR_SENSORFUSIONDMP_H 3 | 4 | #include "SensorFusion.h" 5 | #include "dmpmag.h" 6 | 7 | namespace SlimeVR 8 | { 9 | namespace Sensors 10 | { 11 | class SensorFusionDMP 12 | { 13 | public: 14 | void updateQuaternion(sensor_real_t nqwxyz[4]); 15 | void updateQuaternion(Quaternion const & nq); 16 | void updateQuaternion(Quat const & nq); 17 | void updateAcc(sensor_real_t Axyz[3]); 18 | void updateMag(sensor_real_t Mxyz[3]); 19 | 20 | bool isUpdated(); 21 | void clearUpdated(); 22 | sensor_real_t const * getQuaternion(); 23 | Quat getQuaternionQuat(); 24 | sensor_real_t const * getGravityVec(); 25 | sensor_real_t const * getLinearAcc(); 26 | void getLinearAcc(sensor_real_t outLinAccel[3]); 27 | Vector3 getLinearAccVec(); 28 | 29 | protected: 30 | DMPMag dmpmag; 31 | 32 | sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f}; 33 | 34 | bool magExist = false; 35 | sensor_real_t bqwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f}; 36 | sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f}; 37 | bool updated = false; 38 | 39 | bool gravityReady = false; 40 | sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f}; 41 | bool linaccelReady = false; 42 | sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f}; 43 | #if ESP32 44 | sensor_real_t linAccel_guard; // Temporary patch for some weird ESP32 bug 45 | #endif 46 | }; 47 | } 48 | } 49 | 50 | #endif // SLIMEVR_SENSORFUSIONDMP_H 51 | -------------------------------------------------------------------------------- /src/sensors/sensoraddresses.h: -------------------------------------------------------------------------------- 1 | //This is useful if you want to use an address shifter 2 | #define DEFAULT_IMU_ADDRESS true 3 | 4 | //We use fixed address values instead of scanning to keep the sensorID consistent and to avoid issues with some breakout board with multiple active ic2 addresses 5 | #if DEFAULT_IMU_ADDRESS 6 | 7 | #if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 8 | #define PRIMARY_IMU_ADDRESS_ONE 0x4A 9 | #define PRIMARY_IMU_ADDRESS_TWO 0x4B 10 | #elif IMU == IMU_BNO055 11 | #define PRIMARY_IMU_ADDRESS_ONE 0x29 12 | #define PRIMARY_IMU_ADDRESS_TWO 0x28 13 | #elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 || IMU == IMU_ICM42688 14 | #define PRIMARY_IMU_ADDRESS_ONE 0x68 15 | #define PRIMARY_IMU_ADDRESS_TWO 0x69 16 | #endif 17 | 18 | #if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 19 | #define SECONDARY_IMU_ADDRESS_ONE 0x4A 20 | #define SECONDARY_IMU_ADDRESS_TWO 0x4B 21 | #elif SECOND_IMU == IMU_BNO055 22 | #define SECONDARY_IMU_ADDRESS_ONE 0x29 23 | #define SECONDARY_IMU_ADDRESS_TWO 0x28 24 | #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 || SECOND_IMU == IMU_ICM42688 25 | #define SECONDARY_IMU_ADDRESS_ONE 0x68 26 | #define SECONDARY_IMU_ADDRESS_TWO 0x69 27 | #endif 28 | 29 | #else 30 | //If not using the default address you can set custom addresses here 31 | #define PRIMARY_IMU_ADDRESS_ONE 0x69 32 | #define PRIMARY_IMU_ADDRESS_TWO 0x68 33 | #define SECONDARY_IMU_ADDRESS_ONE 0x69 34 | #define SECONDARY_IMU_ADDRESS_TWO 0x68 35 | #endif -------------------------------------------------------------------------------- /lib/ICM20948/AK09916_REGISTERS.h: -------------------------------------------------------------------------------- 1 | #ifndef _AK09916_REGISTERS_H_ 2 | #define _AK09916_REGISTERS_H_ 3 | 4 | #include 5 | 6 | typedef enum 7 | { 8 | AK09916_REG_WIA1 = 0x00, 9 | AK09916_REG_WIA2, 10 | AK09916_REG_RSV1, 11 | AK09916_REG_RSV2, // Reserved register. We start reading here when using the DMP. Secret sauce... 12 | // discontinuity - containing another nine reserved registers? Secret sauce... 13 | AK09916_REG_ST1 = 0x10, 14 | AK09916_REG_HXL, 15 | AK09916_REG_HXH, 16 | AK09916_REG_HYL, 17 | AK09916_REG_HYH, 18 | AK09916_REG_HZL, 19 | AK09916_REG_HZH, 20 | // discontinuity 21 | AK09916_REG_ST2 = 0x18, 22 | // discontinuity 23 | AK09916_REG_CNTL2 = 0x31, 24 | AK09916_REG_CNTL3, 25 | } AK09916_Reg_Addr_e; 26 | 27 | typedef struct 28 | { 29 | uint8_t WIA1; 30 | } AK09916_WIA1_Reg_t; 31 | 32 | typedef struct 33 | { 34 | uint8_t WIA2; 35 | } AK09916_WIA2_Reg_t; 36 | 37 | typedef struct 38 | { 39 | uint8_t DRDY : 1; 40 | uint8_t DOR : 1; 41 | uint8_t reserved_0 : 6; 42 | } AK09916_ST1_Reg_t; 43 | 44 | // typedef struct{ 45 | 46 | // }AK09916_HXL_Reg_t; 47 | 48 | // typedef struct{ 49 | 50 | // }AK09916_HXH_Reg_t; 51 | // typedef struct{ 52 | 53 | // }AK09916_HYL_Reg_t; 54 | // typedef struct{ 55 | 56 | // }AK09916_HYH_Reg_t; 57 | // typedef struct{ 58 | 59 | // }AK09916_HZL_Reg_t; 60 | // typedef struct{ 61 | 62 | // }AK09916_HZH_Reg_t; 63 | 64 | typedef struct 65 | { 66 | uint8_t reserved_0 : 3; 67 | uint8_t HOFL : 1; 68 | uint8_t reserved_1 : 4; 69 | } AK09916_ST2_Reg_t; 70 | 71 | typedef struct 72 | { 73 | uint8_t MODE : 5; 74 | uint8_t reserved_0 : 3; 75 | } AK09916_CNTL2_Reg_t; 76 | 77 | typedef struct 78 | { 79 | uint8_t SRST : 1; 80 | uint8_t reserved_0 : 7; 81 | } AK09916_CNTL3_Reg_t; 82 | 83 | #endif // _AK09916_REGISTERS_H_ 84 | -------------------------------------------------------------------------------- /lib/magneto/dmpmag.hpp: -------------------------------------------------------------------------------- 1 | #include "dmpmag.h" 2 | 3 | //Get rotation quaternion from gravity vector and geomagnetic vector by Direction Cosine Matrix 4 | //https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-attitudetran 5 | 6 | template 7 | Quat DMPMag::getQuatDCM(const T acc[3], const T mag[3]) 8 | { 9 | Vector3 Mv(mag[0], mag[1], mag[2]); 10 | Vector3 Dv(acc[0], acc[1], acc[2]); 11 | Dv.normalize(); 12 | Vector3 Rv = Dv.cross(Mv); 13 | Rv.normalize(); 14 | Vector3 Fv = Rv.cross(Dv); 15 | Fv.normalize(); 16 | float q04 = 2*sqrt(1+Fv.x+Rv.y+Dv.z); 17 | return Quat(Rv.z-Dv.y, Dv.x-Fv.z, Fv.y-Rv.x, q04*q04/4).normalized(); 18 | } 19 | 20 | template 21 | Quat DMPMag::getCorrection(const T acc[3], const T mag[3], Quat quat) 22 | { 23 | Quat magQ = getQuatDCM(acc,mag); 24 | //dmp.w=DCM.z 25 | //dmp.x=DCM.y 26 | //dmp.y=-DCM.x 27 | //dmp.z=DCM.w 28 | Quat trans(magQ.x, magQ.y, magQ.w, magQ.z); 29 | Quat result = trans*quat.inverse(); 30 | return result; 31 | } 32 | 33 | template 34 | void DMPMag::update(T oqwxyz[4], const T iqwxyz[4], const T Grav[3], const T Mxyz[3]) 35 | { 36 | // Map DMP axis to sensor axis 37 | Quat quat(-iqwxyz[2], iqwxyz[1], iqwxyz[3], iqwxyz[0]); 38 | if (correction.length_squared() == 0.0f) { 39 | correction = getCorrection(Grav, Mxyz, quat); 40 | } else { 41 | Quat newCorr = getCorrection(Grav, Mxyz, quat); 42 | 43 | if(!__isnanf(newCorr.w)) { 44 | correction = correction.slerp(newCorr, magCorrRatio); 45 | } 46 | } 47 | Quat fusedquat = correction * quat; 48 | oqwxyz[0] = fusedquat.w; 49 | oqwxyz[1] = fusedquat.x; 50 | oqwxyz[2] = fusedquat.y; 51 | oqwxyz[3] = fusedquat.z; 52 | } 53 | -------------------------------------------------------------------------------- /src/network/manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2023 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #include "manager.h" 24 | 25 | #include "GlobalVars.h" 26 | 27 | namespace SlimeVR { 28 | namespace Network { 29 | 30 | void Manager::setup() { ::WiFiNetwork::setUp(); } 31 | 32 | void Manager::update() { 33 | WiFiNetwork::upkeep(); 34 | 35 | auto wasConnected = m_IsConnected; 36 | 37 | m_IsConnected = ::WiFiNetwork::isConnected(); 38 | 39 | if (!m_IsConnected) { 40 | return; 41 | } 42 | 43 | if (!wasConnected) { 44 | // WiFi was reconnected, rediscover the server and reconnect 45 | networkConnection.reset(); 46 | } 47 | 48 | networkConnection.update(); 49 | } 50 | 51 | } // namespace Network 52 | } // namespace SlimeVR 53 | -------------------------------------------------------------------------------- /src/sensors/SensorFusionRestDetect.h: -------------------------------------------------------------------------------- 1 | #ifndef SLIMEVR_SENSORFUSIONRESTDETECT_H_ 2 | #define SLIMEVR_SENSORFUSIONRESTDETECT_H_ 3 | 4 | #include "SensorFusion.h" 5 | 6 | #include "../motionprocessing/RestDetection.h" 7 | 8 | #if SENSOR_USE_VQF 9 | #define SENSOR_FUSION_WITH_RESTDETECT 1 10 | #else 11 | #define SENSOR_FUSION_WITH_RESTDETECT 0 12 | #endif 13 | 14 | namespace SlimeVR 15 | { 16 | namespace Sensors 17 | { 18 | #if !SENSOR_FUSION_WITH_RESTDETECT 19 | struct SensorRestDetectionParams: RestDetectionParams { 20 | SensorRestDetectionParams() : RestDetectionParams() { 21 | restMinTimeMicros = 2.0f * 1e6; 22 | restThGyr = 0.6f; // 400 norm 23 | restThAcc = 0.06f; // 100 norm 24 | } 25 | }; 26 | #endif 27 | 28 | class SensorFusionRestDetect : public SensorFusion 29 | { 30 | public: 31 | SensorFusionRestDetect(float gyrTs, float accTs=-1.0, float magTs=-1.0) 32 | : SensorFusion(gyrTs, accTs, magTs) 33 | #if !SENSOR_FUSION_WITH_RESTDETECT 34 | , restDetection(restDetectionParams, gyrTs, 35 | (accTs<0) ? gyrTs : accTs) 36 | #endif 37 | {} 38 | 39 | bool getRestDetected(); 40 | 41 | #if !SENSOR_FUSION_WITH_RESTDETECT 42 | void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat); 43 | void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat); 44 | #endif 45 | protected: 46 | #if !SENSOR_FUSION_WITH_RESTDETECT 47 | SensorRestDetectionParams restDetectionParams {}; 48 | RestDetection restDetection; 49 | #endif 50 | 51 | }; 52 | } 53 | } 54 | 55 | #endif // SLIMEVR_SENSORFUSIONRESTDETECT_H_ 56 | -------------------------------------------------------------------------------- /src/GlobalVars.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef GLOBALVARS_H 25 | #define GLOBALVARS_H 26 | 27 | #include 28 | 29 | #include "LEDManager.h" 30 | #include "configuration/Configuration.h" 31 | #include "network/connection.h" 32 | #include "network/manager.h" 33 | #include "sensors/SensorManager.h" 34 | #include "status/StatusManager.h" 35 | 36 | extern Timer<> globalTimer; 37 | extern SlimeVR::LEDManager ledManager; 38 | extern SlimeVR::Status::StatusManager statusManager; 39 | extern SlimeVR::Configuration::Configuration configuration; 40 | extern SlimeVR::Sensors::SensorManager sensorManager; 41 | extern SlimeVR::Network::Manager networkManager; 42 | extern SlimeVR::Network::Connection networkConnection; 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /src/sensors/bno055sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SENSORS_BNO055SENSOR_H 25 | #define SENSORS_BNO055SENSOR_H 26 | 27 | #include "sensor.h" 28 | 29 | #include 30 | 31 | class BNO055Sensor : public Sensor 32 | { 33 | public: 34 | BNO055Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) 35 | : Sensor("BNO055Sensor", IMU_BNO055, id, address, rotation, sclPin, sdaPin){}; 36 | ~BNO055Sensor(){}; 37 | void motionSetup() override final; 38 | void motionLoop() override final; 39 | void startCalibration(int calibrationType) override final; 40 | 41 | private: 42 | Adafruit_BNO055 imu; 43 | }; 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/credentials.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_CREDENTIALS_H_ 24 | #define SLIMEVR_CREDENTIALS_H_ 25 | 26 | // The OTA password is public, server should know it for OTA updates, 27 | // and devices don't have any authentication anyway. 28 | // We have password here to prevent random attacks on IOT things 29 | // that might try to hack all esp-based devices and upload malicious 30 | // firmware. We don't have any hardware buttons for the user to confirm 31 | // OTA update, so this is the best way we have. 32 | // OTA is allowed only for the first 60 seconds after device startup. 33 | const char *otaPassword = "SlimeVR-OTA"; // YOUR OTA PASSWORD HERE, LEAVE EMPTY TO DISABLE OTA UPDATES 34 | 35 | #endif // SLIMEVR_CREDENTIALS_H_ 36 | -------------------------------------------------------------------------------- /lib/math/helper_3dmath.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "helper_3dmath.h" 25 | 26 | // Fast inverse square root function (https://en.wikipedia.org/wiki/Fast_inverse_square_root). 27 | // Source: https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/ 28 | float invSqrt(float x) 29 | { 30 | union { float f; uint32_t u; } y = {x}; 31 | y.u = 0x5F1FFF77ul - (y.u >> 1); 32 | return 0.703974056f * y.f * (2.38919526f - x * y.f * y.f); 33 | } 34 | 35 | float vector_dot(float a[3], float b[3]) 36 | { 37 | return (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]); 38 | } 39 | 40 | void vector_normalize(float a[3]) 41 | { 42 | float norm = invSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); 43 | a[0] *= norm; 44 | a[1] *= norm; 45 | a[2] *= norm; 46 | } -------------------------------------------------------------------------------- /src/configuration/CalibrationConfig.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "CalibrationConfig.h" 25 | 26 | namespace SlimeVR { 27 | namespace Configuration { 28 | const char* calibrationConfigTypeToString(CalibrationConfigType type) { 29 | switch (type) { 30 | case NONE: 31 | return "NONE"; 32 | case BMI160: 33 | return "BMI160"; 34 | case MPU6050: 35 | return "MPU6050"; 36 | case MPU9250: 37 | return "MPU9250"; 38 | case ICM20948: 39 | return "ICM20948"; 40 | case ICM42688: 41 | return "ICM42688"; 42 | default: 43 | return "UNKNOWN"; 44 | } 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /src/logging/Logger.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | 3 | namespace SlimeVR 4 | { 5 | namespace Logging 6 | { 7 | void Logger::setTag(const char *tag) 8 | { 9 | m_Tag = (char *)malloc(strlen(tag) + 1); 10 | strcpy(m_Tag, tag); 11 | } 12 | 13 | void Logger::trace(const char *format, ...) 14 | { 15 | va_list args; 16 | va_start(args, format); 17 | log(TRACE, format, args); 18 | va_end(args); 19 | } 20 | 21 | void Logger::debug(const char *format, ...) 22 | { 23 | va_list args; 24 | va_start(args, format); 25 | log(DEBUG, format, args); 26 | va_end(args); 27 | } 28 | 29 | void Logger::info(const char *format, ...) 30 | { 31 | va_list args; 32 | va_start(args, format); 33 | log(INFO, format, args); 34 | va_end(args); 35 | } 36 | 37 | void Logger::warn(const char *format, ...) 38 | { 39 | va_list args; 40 | va_start(args, format); 41 | log(WARN, format, args); 42 | va_end(args); 43 | } 44 | 45 | void Logger::error(const char *format, ...) 46 | { 47 | va_list args; 48 | va_start(args, format); 49 | log(ERROR, format, args); 50 | va_end(args); 51 | } 52 | 53 | void Logger::fatal(const char *format, ...) 54 | { 55 | va_list args; 56 | va_start(args, format); 57 | log(FATAL, format, args); 58 | va_end(args); 59 | } 60 | 61 | void Logger::log(Level level, const char *format, va_list args) 62 | { 63 | if (level < LOG_LEVEL) 64 | { 65 | return; 66 | } 67 | 68 | char buffer[256]; 69 | vsnprintf(buffer, 256, format, args); 70 | 71 | char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2]; 72 | strcpy(buf, m_Prefix); 73 | if (m_Tag != nullptr) 74 | { 75 | strcat(buf, ":"); 76 | strcat(buf, m_Tag); 77 | } 78 | 79 | Serial.printf("[%-5s] [%s] %s\n", levelToString(level), buf, buffer); 80 | } 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /lib/magneto/mahony.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _MAHONY_H_ 25 | #define _MAHONY_H_ 26 | 27 | #include "helper_3dmath.h" 28 | 29 | template 30 | class Mahony { 31 | 32 | // These are the free parameters in the Mahony filter and fusion scheme, 33 | // Kp for proportional feedback, Ki for integral 34 | // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required. 35 | static constexpr float Kp = 10.0f; 36 | static constexpr float Ki = 0.0f; 37 | 38 | public: 39 | void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T mx, T my, T mz, T deltat); 40 | void update(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat); 41 | 42 | private: 43 | T ix = 0.0; 44 | T iy = 0.0; 45 | T iz = 0.0; 46 | }; 47 | 48 | #include "mahony.hpp" 49 | 50 | #endif /* _MAHONY_H_ */ 51 | -------------------------------------------------------------------------------- /src/sensors/EmptySensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SENSORS_EMPTYSENSOR_H 25 | #define SENSORS_EMPTYSENSOR_H 26 | 27 | #include "sensor.h" 28 | 29 | namespace SlimeVR 30 | { 31 | namespace Sensors 32 | { 33 | class EmptySensor : public Sensor 34 | { 35 | public: 36 | EmptySensor(uint8_t id) : Sensor("EmptySensor", 255, id, 0, 0.0){}; 37 | ~EmptySensor(){}; 38 | 39 | void motionSetup() override final{}; 40 | void motionLoop() override final{}; 41 | void sendData() override final{}; 42 | void startCalibration(int calibrationType) override final{}; 43 | SensorStatus getSensorState() override final 44 | { 45 | return SensorStatus::SENSOR_OFFLINE; 46 | }; 47 | }; 48 | } 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /src/network/wifihandler.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_WIFI_H_ 24 | #define SLIMEVR_WIFI_H_ 25 | 26 | #ifdef ESP8266 27 | #include 28 | #else 29 | #include 30 | #endif 31 | 32 | namespace WiFiNetwork { 33 | bool isConnected(); 34 | void setUp(); 35 | void upkeep(); 36 | void setWiFiCredentials(const char * SSID, const char * pass); 37 | IPAddress getAddress(); 38 | uint8_t getWiFiState(); 39 | } 40 | 41 | /** Wifi Reconnection Statuses **/ 42 | typedef enum { 43 | SLIME_WIFI_NOT_SETUP = 0, 44 | SLIME_WIFI_SAVED_ATTEMPT, 45 | SLIME_WIFI_SAVED_G_ATTEMPT, 46 | SLIME_WIFI_HARDCODE_ATTEMPT, 47 | SLIME_WIFI_HARDCODE_G_ATTEMPT, 48 | SLIME_WIFI_SERVER_CRED_ATTEMPT, 49 | SLIME_WIFI_SERVER_CRED_G_ATTEMPT 50 | } wifi_reconnection_statuses; 51 | 52 | #endif // SLIMEVR_WIFI_H_ 53 | -------------------------------------------------------------------------------- /src/sensors/ErroneousSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SENSORS_ERRONEOUSSENSOR_H 25 | #define SENSORS_ERRONEOUSSENSOR_H 26 | 27 | #include "sensor.h" 28 | 29 | namespace SlimeVR 30 | { 31 | namespace Sensors 32 | { 33 | class ErroneousSensor : public Sensor 34 | { 35 | public: 36 | ErroneousSensor(uint8_t id, uint8_t type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){}; 37 | ~ErroneousSensor(){}; 38 | 39 | void motionSetup() override; 40 | void motionLoop() override final{}; 41 | void sendData() override{}; 42 | void startCalibration(int calibrationType) override final{}; 43 | SensorStatus getSensorState() override final; 44 | 45 | private: 46 | uint8_t m_ExpectedType; 47 | }; 48 | } 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /lib/ICM20948/README.md: -------------------------------------------------------------------------------- 1 | # SparkFun ICM-20948 Arduino Library 2 | 3 | This is the SparkFun library for the TDK InvenSense ICM-20948 Inertial Measurement Unit 9-Degree Of Freedom sensor as used on the [SparkFun 9DoF IMU Breakout - ICM-20948 (Qwiic)](https://www.sparkfun.com/products/15335). 4 | 5 | Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™). You can find further details in [DMP.md](./DMP.md). 6 | 7 | ## Contributing 8 | 9 | If you would like to contribute to this library: please do, we truly appreciate it, but please follow [these guidelines](./CONTRIBUTING.md). Thanks! 10 | 11 | ## Repository Contents 12 | 13 | * [**/examples**](./examples) - Example sketches for the library (.ino). Run these from the Arduino IDE. 14 | * [**/src**](./src) - Source files for the library (.cpp, .h). 15 | * [**keywords.txt**](./keywords.txt) - Keywords from this library that will be highlighted in the Arduino IDE. 16 | * [**library.properties**](./library.properties) - General library properties for the Arduino package manager. 17 | * [**CONTRIBUTING.md**](./CONTRIBUTING.md) - Guidelines on how to contribute to this library. 18 | * [**DMP.md**](./DMP.md) - Information about the InvenSense Digital Motion Processor (DMP™) 19 | 20 | ## Documentation 21 | 22 | * **[Hookup Guide](https://learn.sparkfun.com/tutorials/sparkfun-9dof-imu-icm-20948-breakout-hookup-guide)** - Basic hookup guide for the SparkFun 9DoF IMU Breakout. 23 | * **[Installing an Arduino Library Guide](https://learn.sparkfun.com/tutorials/installing-an-arduino-library)** - Basic information on how to install an Arduino library. 24 | 25 | ## Products that use this Library 26 | 27 | * [SparkFun 9DoF IMU Breakout - ICM-20948 (Qwiic) - SEN-15335](https://www.sparkfun.com/products/15335) 28 | * [SparkFun OpenLog Artemis - DEV-16832](https://www.sparkfun.com/products/16832) 29 | * [SparkFun MicroMod Asset Tracker Carrier Board - DEV-17272](https://www.sparkfun.com/products/17272) 30 | 31 | ## License Information 32 | 33 | This product is _**open source**_! 34 | 35 | Please see the included [License.md](./License.md) for more information. 36 | 37 | Distributed as-is; no warranty is given. 38 | 39 | - Your friends at SparkFun. 40 | -------------------------------------------------------------------------------- /lib/ICM42688/MMC5983MA.h: -------------------------------------------------------------------------------- 1 | /* 06/14/2020 Copyright Tlera Corporation 2 | 3 | Created by Kris Winer 4 | 5 | This sketch uses SDA/SCL on pins 21/20 (Ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board. 6 | The MMC5983MA is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution. 7 | 8 | Library may be used freely and without limit with attribution. 9 | 10 | */ 11 | 12 | #ifndef MMC5983MA_h 13 | #define MMC5983MA_h 14 | 15 | //Register map for MMC5983MA' 16 | //http://www.memsic.com/userfiles/files/DataSheets/Magnetic-Sensors-Datasheets/MMC5983MA_Datasheet.pdf 17 | #define MMC5983MA_XOUT_0 0x00 18 | #define MMC5983MA_XOUT_1 0x01 19 | #define MMC5983MA_YOUT_0 0x02 20 | #define MMC5983MA_YOUT_1 0x03 21 | #define MMC5983MA_ZOUT_0 0x04 22 | #define MMC5983MA_ZOUT_1 0x05 23 | #define MMC5983MA_XYZOUT_2 0x06 24 | #define MMC5983MA_TOUT 0x07 25 | #define MMC5983MA_STATUS 0x08 26 | #define MMC5983MA_CONTROL_0 0x09 27 | #define MMC5983MA_CONTROL_1 0x0A 28 | #define MMC5983MA_CONTROL_2 0x0B 29 | #define MMC5983MA_CONTROL_3 0x0C 30 | #define MMC5983MA_PRODUCT_ID 0x2F 31 | 32 | #define MMC5983MA_ADDRESS 0x30 33 | 34 | // Sample rates 35 | #define MODR_ONESHOT 0x00 36 | #define MODR_1Hz 0x01 37 | #define MODR_10Hz 0x02 38 | #define MODR_20Hz 0x03 39 | #define MODR_50Hz 0x04 40 | #define MODR_100Hz 0x05 41 | #define MODR_200Hz 0x06 // BW = 0x01 only 42 | #define MODR_1000Hz 0x07 // BW = 0x11 only 43 | 44 | //Bandwidths 45 | #define MBW_100Hz 0x00 // 8 ms measurement time 46 | #define MBW_200Hz 0x01 // 4 ms 47 | #define MBW_400Hz 0x02 // 2 ms 48 | #define MBW_800Hz 0x03 // 0.5 ms 49 | 50 | // Set/Reset as a function of measurements 51 | #define MSET_1 0x00 // Set/Reset each data measurement 52 | #define MSET_25 0x01 // each 25 data measurements 53 | #define MSET_75 0x02 54 | #define MSET_100 0x03 55 | #define MSET_250 0x04 56 | #define MSET_500 0x05 57 | #define MSET_1000 0x06 58 | #define MSET_2000 0x07 59 | 60 | #define MMC5983MA_mRes (1.0f / 16384.0f) // mag sensitivity if using 18 bit data 61 | #define MMC5983MA_offset 131072.0f // mag range unsigned to signed 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /src/network/wifiprovisioning.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #include "wifiprovisioning.h" 24 | #include "wifihandler.h" 25 | #include "logging/Logger.h" 26 | 27 | // TODO Currently provisioning implemented via SmartConfig 28 | // it sucks. 29 | // TODO: New implementation: https://github.com/SlimeVR/SlimeVR-Tracker-ESP/issues/71 30 | 31 | // TODO: Cleanup with proper classes 32 | SlimeVR::Logging::Logger wifiProvisioningLogger("WiFiProvisioning"); 33 | bool provisioning = false; 34 | 35 | void WiFiNetwork::upkeepProvisioning() { 36 | // Called even when not provisioning to do things like provide neighbours or other upkeep 37 | } 38 | 39 | void WiFiNetwork::startProvisioning() { 40 | if(WiFi.beginSmartConfig()) { 41 | provisioning = true; 42 | wifiProvisioningLogger.info("SmartConfig started"); 43 | } 44 | } 45 | 46 | void WiFiNetwork::stopProvisioning() { 47 | WiFi.stopSmartConfig(); 48 | provisioning = false; 49 | } 50 | 51 | void WiFiNetwork::provideNeighbours() { 52 | // TODO: SmartConfig can't do this, created for future 53 | } 54 | 55 | bool WiFiNetwork::isProvisioning() { 56 | return provisioning && !WiFi.smartConfigDone(); 57 | } 58 | -------------------------------------------------------------------------------- /src/sensors/bno080sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SENSORS_BNO080SENSOR_H 25 | #define SENSORS_BNO080SENSOR_H 26 | 27 | #include "sensor.h" 28 | #include 29 | 30 | class BNO080Sensor : public Sensor 31 | { 32 | public: 33 | BNO080Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) 34 | : Sensor("BNO080Sensor", type, id, address, rotation, sclPin, sdaPin), m_IntPin(intPin) {}; 35 | ~BNO080Sensor(){}; 36 | void motionSetup() override final; 37 | void postSetup() override { 38 | lastData = millis(); 39 | } 40 | 41 | void motionLoop() override final; 42 | void sendData() override final; 43 | void startCalibration(int calibrationType) override final; 44 | SensorStatus getSensorState() override final; 45 | 46 | private: 47 | BNO080 imu{}; 48 | 49 | uint8_t m_IntPin; 50 | 51 | uint8_t tap; 52 | unsigned long lastData = 0; 53 | uint8_t lastReset = 0; 54 | BNO080Error lastError{}; 55 | 56 | // Magnetometer specific members 57 | Quat magQuaternion{}; 58 | uint8_t magCalibrationAccuracy = 0; 59 | float magneticAccuracyEstimate = 999; 60 | bool newMagData = false; 61 | }; 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /src/sensors/icm42688sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SENSORS_ICM42688SENSOR_H 25 | #define SENSORS_ICM42688SENSOR_H 26 | 27 | #include "sensor.h" 28 | #include "logging/Logger.h" 29 | 30 | #include 31 | #include 32 | #include "I2Cdev.h" 33 | 34 | #include "SensorFusion.h" 35 | 36 | class ICM42688Sensor : public Sensor 37 | { 38 | public: 39 | ICM42688Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) 40 | : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation, sclPin, sdaPin), 41 | sfusion(0.001f, 0.01f, 0.01f){}; 42 | ~ICM42688Sensor(){}; 43 | void motionSetup() override final; 44 | void motionLoop() override final; 45 | void startCalibration(int calibrationType) override final; 46 | 47 | private: 48 | uint8_t addr_mag = 0x30; 49 | bool magExists = false; 50 | 51 | // raw data and scaled as vector 52 | float Axyz[3]{}; 53 | float Gxyz[3]{}; 54 | float Mxyz[3]{}; 55 | 56 | SlimeVR::Sensors::SensorFusion sfusion; 57 | 58 | SlimeVR::Configuration::ICM42688CalibrationConfig m_Calibration; 59 | 60 | void accel_read(); 61 | void gyro_read(); 62 | void mag_read(); 63 | 64 | void parseAccelData(); 65 | void parseGyroData(); 66 | void parseMagData(); 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /lib/magneto/magneto1.4.h: -------------------------------------------------------------------------------- 1 | #ifndef __MAGENTO_1_4__ 2 | #define __MAGENTO_1_4__ 3 | 4 | /* Copyright (C) 2013 www.sailboatinstruments.blogspot.com 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | 25 | // In https://github.com/jremington/MPU-9250-AHRS 26 | // magneto 1.4 magnetometer/accelerometer calibration code 27 | // from http://sailboatinstruments.blogspot.com/2011/08/improved-magnetometer-calibration.html 28 | 29 | /// @brief Accelerometer/Magnetometer calibration assistant 30 | class MagnetoCalibration { 31 | public: 32 | /// @brief Updates calibration with the sample (x, y, z) 33 | void sample(double x, double y, double z); 34 | 35 | /// @brief Outputs the calibration matrix corresponding to all previous samples 36 | /// 37 | /// This does not consume/destroy the calibration struct, allowing for the possibility of 38 | /// continuously updating the calibration. That said, there may be limitations on how many 39 | /// samples are useful to collect, due to the limitations of finite-storage numbers. 40 | /// 41 | /// @param BAinv 42 | void current_calibration(float BAinv[4][3]); 43 | 44 | private: 45 | // internal 10x10 matrix representing the 10x matrix multiplied by its transpose, 46 | // resulting in a 10x10 symmetric matrix 47 | double ata[100] = {0.0}; 48 | double norm_sum = 0.0; 49 | double sample_count = 0.0; 50 | }; 51 | 52 | #endif // __MAGENTO_1_4__ -------------------------------------------------------------------------------- /src/configuration/Configuration.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_CONFIGURATION_CONFIGURATION_H 25 | #define SLIMEVR_CONFIGURATION_CONFIGURATION_H 26 | 27 | #include 28 | 29 | #include "DeviceConfig.h" 30 | #include "logging/Logger.h" 31 | #include "../motionprocessing/GyroTemperatureCalibrator.h" 32 | 33 | namespace SlimeVR { 34 | namespace Configuration { 35 | class Configuration { 36 | public: 37 | void setup(); 38 | 39 | void save(); 40 | void reset(); 41 | 42 | void print(); 43 | 44 | int32_t getVersion() const; 45 | 46 | size_t getCalibrationCount() const; 47 | CalibrationConfig getCalibration(size_t sensorID) const; 48 | void setCalibration(size_t sensorID, const CalibrationConfig& config); 49 | 50 | bool loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config); 51 | bool saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config); 52 | 53 | private: 54 | void loadCalibrations(); 55 | bool runMigrations(int32_t version); 56 | 57 | bool m_Loaded = false; 58 | 59 | DeviceConfig m_Config{}; 60 | std::vector m_Calibrations; 61 | 62 | Logging::Logger m_Logger = Logging::Logger("Configuration"); 63 | }; 64 | } 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /src/sensors/mpu6050sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SENSORS_MPU6050SENSOR_H 25 | #define SENSORS_MPU6050SENSOR_H 26 | 27 | #include "sensor.h" 28 | #include 29 | #include "SensorFusionDMP.h" 30 | 31 | class MPU6050Sensor : public Sensor 32 | { 33 | public: 34 | MPU6050Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) 35 | : Sensor("MPU6050Sensor", type, id, address, rotation, sclPin, sdaPin){}; 36 | ~MPU6050Sensor(){}; 37 | void motionSetup() override final; 38 | void motionLoop() override final; 39 | void startCalibration(int calibrationType) override final; 40 | 41 | private: 42 | MPU6050 imu{}; 43 | Quaternion rawQuat{}; 44 | VectorInt16 rawAccel{}; 45 | // MPU dmp control/status vars 46 | bool dmpReady = false; // set true if DMP init was successful 47 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 48 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 49 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 50 | uint16_t fifoCount; // count of all bytes currently in FIFO 51 | uint8_t fifoBuffer[64]{}; // FIFO storage buffer 52 | 53 | SlimeVR::Sensors::SensorFusionDMP sfusion; 54 | 55 | #ifndef IMU_MPU6050_RUNTIME_CALIBRATION 56 | SlimeVR::Configuration::MPU6050CalibrationConfig m_Calibration = {}; 57 | #endif 58 | }; 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /src/network/packets.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_PACKETS_H_ 25 | #define SLIMEVR_PACKETS_H_ 26 | 27 | #define PACKET_HEARTBEAT 0 28 | // #define PACKET_ROTATION 1 // Deprecated 29 | // #define PACKET_GYRO 2 // Deprecated 30 | #define PACKET_HANDSHAKE 3 31 | #define PACKET_ACCEL 4 32 | // #define PACKET_MAG 5 // Deprecated 33 | // #define PACKET_RAW_CALIBRATION_DATA 6 // Deprecated 34 | // #define PACKET_CALIBRATION_FINISHED 7 // Deprecated 35 | #define PACKET_CONFIG 8 36 | // #define PACKET_RAW_MAGNETOMETER 9 // Deprecated 37 | #define PACKET_PING_PONG 10 38 | #define PACKET_SERIAL 11 39 | #define PACKET_BATTERY_LEVEL 12 40 | #define PACKET_TAP 13 41 | #define PACKET_ERROR 14 42 | #define PACKET_SENSOR_INFO 15 43 | // #define PACKET_ROTATION_2 16 // Deprecated 44 | #define PACKET_ROTATION_DATA 17 45 | #define PACKET_MAGNETOMETER_ACCURACY 18 46 | #define PACKET_SIGNAL_STRENGTH 19 47 | #define PACKET_TEMPERATURE 20 48 | // #define PACKET_USER_ACTION 21 // Joycon buttons only currently 49 | #define PACKET_FEATURE_FLAGS 22 50 | 51 | #define PACKET_BUNDLE 100 52 | 53 | #define PACKET_INSPECTION 105 // 0x69 54 | 55 | #define PACKET_RECEIVE_HEARTBEAT 1 56 | #define PACKET_RECEIVE_VIBRATE 2 57 | #define PACKET_RECEIVE_HANDSHAKE 3 58 | #define PACKET_RECEIVE_COMMAND 4 59 | 60 | #define PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA 1 61 | #define PACKET_INSPECTION_PACKETTYPE_FUSED_IMU_DATA 2 62 | #define PACKET_INSPECTION_PACKETTYPE_CORRECTION_DATA 3 63 | #define PACKET_INSPECTION_DATATYPE_INT 1 64 | #define PACKET_INSPECTION_DATATYPE_FLOAT 2 65 | 66 | #endif // SLIMEVR_PACKETS_H_ 67 | -------------------------------------------------------------------------------- /src/globals.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_GLOBALS_H_ 24 | #define SLIMEVR_GLOBALS_H_ 25 | 26 | #include 27 | #include "consts.h" 28 | #include "debug.h" 29 | #include "defines.h" 30 | #include "defines_bmi160.h" 31 | #include "defines_sensitivity.h" 32 | 33 | #ifndef SECOND_IMU 34 | #define SECOND_IMU IMU 35 | #endif 36 | 37 | #ifndef SECOND_IMU_ROTATION 38 | #define SECOND_IMU_ROTATION IMU_ROTATION 39 | #endif 40 | 41 | #ifndef BATTERY_MONITOR 42 | #define BATTERY_MONITOR BAT_INTERNAL 43 | #endif 44 | 45 | // If LED_PIN is not defined in "defines.h" take the default pin from "pins_arduino.h" framework. 46 | // If there is no pin defined for the board, use LED_PIN 255 and disable LED 47 | #if defined(LED_PIN) 48 | // LED_PIN is defined 49 | #if (LED_PIN < 0) || (LED_PIN >= LED_OFF) 50 | #define ENABLE_LEDS false 51 | #else 52 | #define ENABLE_LEDS true 53 | #endif 54 | #else 55 | // LED_PIN is not defined 56 | #if defined(LED_BUILTIN) && (LED_BUILTIN < LED_OFF) && (LED_BUILTIN >= 0) 57 | #define LED_PIN LED_BUILTIN 58 | #define ENABLE_LEDS true 59 | #else 60 | #define LED_PIN LED_OFF 61 | #define ENABLE_LEDS false 62 | #endif 63 | #endif 64 | 65 | #if !defined(LED_INVERTED) 66 | // default is inverted for SlimeVR / ESP-12E 67 | #define LED_INVERTED true 68 | #endif 69 | 70 | #if LED_INVERTED 71 | #define LED__ON LOW 72 | #define LED__OFF HIGH 73 | #else 74 | #define LED__ON HIGH 75 | #define LED__OFF LOW 76 | #endif 77 | 78 | #endif // SLIMEVR_GLOBALS_H_ 79 | -------------------------------------------------------------------------------- /src/defines_sensitivity.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef DEFINES_SENSITIVITY_H 25 | #define DEFINES_SENSITIVITY_H 26 | 27 | // ========================== 28 | // Sensitivity calibration 29 | // Only for: BMI160 30 | // ========================== 31 | 32 | // This type of calibration attempts to reduce "extra degrees measures", 33 | // useful if you do 360 spins 34 | 35 | // Trackers are identified by sensor id and MAC address of the device, 36 | // which is displayed in serial when you upload firmware 37 | 38 | // Number of spins determines calibration accuracy 39 | 40 | // 1. Put the axis you want to calibrate vertically 41 | // 2. Put the tracker into an orientation that you can repeat later 42 | // 3. Do a full reset, rotation must now be 0 0 0 43 | // 4. Spin the tracker clockwise <.spins> times around the axis 44 | // 5. Put the tracker back into the reset position 45 | // 6. Write down the resulting Y (yaw) value into the table below 46 | // Reflash, do the same rotation and check if the resulting angle is now 0 47 | 48 | #define SENSORID_PRIMARY 0 49 | #define SENSORID_AUX 1 50 | struct SensitivityOffsetXYZ { const char* mac; unsigned char sensorId; double spins; double x; double y; double z; }; 51 | const SensitivityOffsetXYZ sensitivityOffsets[] = { 52 | // example values 53 | { "A4:E5:7C:B6:00:01", SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 }, 54 | { "A4:E5:7C:B6:00:02", SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 }, 55 | { "A4:E5:7C:B6:00:03", SENSORID_PRIMARY, .spins = 10, .x = 11, .y = 2.2, .z = -1 }, 56 | { "A4:E5:7C:B6:00:04", SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 }, 57 | { "A4:E5:7C:B6:00:05", SENSORID_PRIMARY, .spins = 10, .x = -10.63, .y = -8.25, .z = -18.6 }, 58 | }; 59 | 60 | #endif -------------------------------------------------------------------------------- /src/sensors/SensorManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_SENSORMANAGER 25 | #define SLIMEVR_SENSORMANAGER 26 | 27 | #include "globals.h" 28 | #include "sensor.h" 29 | #include "EmptySensor.h" 30 | #include "logging/Logger.h" 31 | 32 | namespace SlimeVR 33 | { 34 | namespace Sensors 35 | { 36 | class SensorManager 37 | { 38 | public: 39 | SensorManager() 40 | : m_Logger(SlimeVR::Logging::Logger("SensorManager")) 41 | , m_Sensors(MAX_IMU_COUNT, nullptr) { 42 | for (auto & u : m_Sensors) { 43 | u = new EmptySensor(0); 44 | } 45 | } 46 | ~SensorManager() 47 | { 48 | for (auto u : m_Sensors) { 49 | if (u != nullptr) { 50 | delete u; 51 | } 52 | } 53 | } 54 | 55 | void setup(); 56 | void postSetup(); 57 | 58 | void update(); 59 | 60 | std::vector & getSensors() { return m_Sensors; }; 61 | 62 | private: 63 | SlimeVR::Logging::Logger m_Logger; 64 | 65 | std::vector m_Sensors; 66 | Sensor* buildSensor(uint8_t sensorID, uint8_t imuType, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, bool optional = false, int extraParam = 0); 67 | 68 | uint8_t activeSCL = 0; 69 | uint8_t activeSDA = 0; 70 | bool running = false; 71 | void swapI2C(uint8_t scl, uint8_t sda); 72 | 73 | uint32_t m_LastBundleSentAtMicros = micros(); 74 | }; 75 | } 76 | } 77 | 78 | #endif // SLIMEVR_SENSORFACTORY_H_ 79 | -------------------------------------------------------------------------------- /lib/ICM20948/License.md: -------------------------------------------------------------------------------- 1 | SparkFun License Information 2 | ============================ 3 | 4 | SparkFun uses two different licenses for our files — one for hardware and one for code. 5 | 6 | Hardware 7 | --------- 8 | 9 | **SparkFun hardware is released under [Creative Commons Share-alike 4.0 International](http://creativecommons.org/licenses/by-sa/4.0/).** 10 | 11 | Note: This is a human-readable summary of (and not a substitute for) the [license](http://creativecommons.org/licenses/by-sa/4.0/legalcode). 12 | 13 | You are free to: 14 | 15 | Share — copy and redistribute the material in any medium or format 16 | Adapt — remix, transform, and build upon the material 17 | for any purpose, even commercially. 18 | The licensor cannot revoke these freedoms as long as you follow the license terms. 19 | Under the following terms: 20 | 21 | Attribution — You must give appropriate credit, provide a link to the license, and indicate if changes were made. You may do so in any reasonable manner, but not in any way that suggests the licensor endorses you or your use. 22 | ShareAlike — If you remix, transform, or build upon the material, you must distribute your contributions under the same license as the original. 23 | No additional restrictions — You may not apply legal terms or technological measures that legally restrict others from doing anything the license permits. 24 | Notices: 25 | 26 | You do not have to comply with the license for elements of the material in the public domain or where your use is permitted by an applicable exception or limitation. 27 | No warranties are given. The license may not give you all of the permissions necessary for your intended use. For example, other rights such as publicity, privacy, or moral rights may limit how you use the material. 28 | 29 | 30 | Code 31 | -------- 32 | 33 | **SparkFun code, firmware, and software is released under the MIT License(http://opensource.org/licenses/MIT).** 34 | 35 | The MIT License (MIT) 36 | 37 | Copyright (c) 2016 SparkFun Electronics 38 | 39 | Permission is hereby granted, free of charge, to any person obtaining a copy 40 | of this software and associated documentation files (the "Software"), to deal 41 | in the Software without restriction, including without limitation the rights 42 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 43 | copies of the Software, and to permit persons to whom the Software is 44 | furnished to do so, subject to the following conditions: 45 | 46 | The above copyright notice and this permission notice shall be included in all 47 | copies or substantial portions of the Software. 48 | 49 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 52 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 53 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 54 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 55 | SOFTWARE. 56 | -------------------------------------------------------------------------------- /src/logging/Logger.h: -------------------------------------------------------------------------------- 1 | #ifndef LOGGING_LOGGER_H 2 | #define LOGGING_LOGGER_H 3 | 4 | #include "Level.h" 5 | #include "debug.h" 6 | #include 7 | 8 | namespace SlimeVR 9 | { 10 | namespace Logging 11 | { 12 | class Logger 13 | { 14 | public: 15 | Logger(const char *prefix) : m_Prefix(prefix), m_Tag(nullptr){}; 16 | Logger(const char *prefix, const char *tag) : m_Prefix(prefix), m_Tag(nullptr) 17 | { 18 | setTag(tag); 19 | }; 20 | 21 | ~Logger() 22 | { 23 | if (m_Tag != nullptr) 24 | { 25 | free(m_Tag); 26 | } 27 | } 28 | 29 | void setTag(const char *tag); 30 | 31 | void trace(const char *str, ...); 32 | void debug(const char *str, ...); 33 | void info(const char *str, ...); 34 | void warn(const char *str, ...); 35 | void error(const char *str, ...); 36 | void fatal(const char *str, ...); 37 | 38 | template 39 | inline void traceArray(const char *str, const T *array, size_t size) 40 | { 41 | logArray(TRACE, str, array, size); 42 | } 43 | 44 | template 45 | inline void debugArray(const char *str, const T *array, size_t size) 46 | { 47 | logArray(DEBUG, str, array, size); 48 | } 49 | 50 | template 51 | inline void infoArray(const char *str, const T *array, size_t size) 52 | { 53 | logArray(INFO, str, array, size); 54 | } 55 | 56 | template 57 | inline void warnArray(const char *str, const T *array, size_t size) 58 | { 59 | logArray(WARN, str, array, size); 60 | } 61 | 62 | template 63 | inline void errorArray(const char *str, const T *array, size_t size) 64 | { 65 | logArray(ERROR, str, array, size); 66 | } 67 | 68 | template 69 | inline void fatalArray(const char *str, const T *array, size_t size) 70 | { 71 | logArray(FATAL, str, array, size); 72 | } 73 | 74 | private: 75 | void log(Level level, const char *str, va_list args); 76 | 77 | template 78 | void logArray(Level level, const char *str, const T *array, size_t size) 79 | { 80 | if (level < LOG_LEVEL) 81 | { 82 | return; 83 | } 84 | 85 | char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2]; 86 | strcpy(buf, m_Prefix); 87 | if (m_Tag != nullptr) 88 | { 89 | strcat(buf, ":"); 90 | strcat(buf, m_Tag); 91 | } 92 | 93 | Serial.printf("[%-5s] [%s] %s", levelToString(level), buf, str); 94 | 95 | for (size_t i = 0; i < size; i++) 96 | { 97 | Serial.print(array[i]); 98 | } 99 | 100 | Serial.println(); 101 | } 102 | 103 | const char *const m_Prefix; 104 | char *m_Tag; 105 | }; 106 | } 107 | } 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /src/sensors/icm20948sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_ICM20948SENSOR_H_ 24 | #define SLIMEVR_ICM20948SENSOR_H_ 25 | 26 | #include 27 | #include "sensor.h" 28 | #include "SensorFusionDMP.h" 29 | 30 | class ICM20948Sensor : public Sensor 31 | { 32 | public: 33 | ICM20948Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) 34 | : Sensor("ICM20948Sensor", IMU_ICM20948, id, address, rotation, sclPin, sdaPin) {} 35 | ~ICM20948Sensor() override = default; 36 | void motionSetup() override final; 37 | void postSetup() override { 38 | this->lastData = millis(); 39 | } 40 | 41 | void motionLoop() override final; 42 | void sendData() override final; 43 | void startCalibration(int calibrationType) override final; 44 | 45 | private: 46 | void calculateAccelerationWithoutGravity(Quat *quaternion); 47 | unsigned long lastData = 0; 48 | unsigned long lastDataSent = 0; 49 | int bias_save_counter = 0; 50 | bool hasdata = false; 51 | // Performance test 52 | /* 53 | uint8_t cntbuf = 0; 54 | int32_t cntrounds = 0; 55 | unsigned long lastData2 = 0; 56 | */ 57 | 58 | #define DMPNUMBERTODOUBLECONVERTER 1073741824.0; 59 | 60 | ICM_20948_I2C imu; 61 | ICM_20948_Device_t pdev; 62 | icm_20948_DMP_data_t dmpData{}; 63 | icm_20948_DMP_data_t dmpDataTemp{}; 64 | 65 | SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration = {}; 66 | 67 | SlimeVR::Sensors::SensorFusionDMP sfusion; 68 | 69 | void saveCalibration(bool repeat); 70 | void loadCalibration(); 71 | void startCalibrationAutoSave(); 72 | void startDMP(); 73 | void connectSensor(); 74 | void startMotionLoop(); 75 | void checkSensorTimeout(); 76 | void readRotation(); 77 | void readFIFOToEnd(); 78 | 79 | #define OVERRIDEDMPSETUP true 80 | // TapDetector tapDetector; 81 | }; 82 | 83 | #endif // SLIMEVR_ICM20948SENSOR_H_ 84 | -------------------------------------------------------------------------------- /src/sensors/bno055sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #include "bno055sensor.h" 24 | #include "globals.h" 25 | #include "GlobalVars.h" 26 | 27 | void BNO055Sensor::motionSetup() { 28 | imu = Adafruit_BNO055(sensorId, addr); 29 | delay(3000); 30 | #if USE_6_AXIS 31 | if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_IMUPLUS)) 32 | #else 33 | if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_NDOF)) 34 | #endif 35 | { 36 | m_Logger.fatal("Can't connect to BNO055 at address 0x%02x", addr); 37 | ledManager.pattern(50, 50, 200); 38 | return; 39 | } 40 | 41 | delay(1000); 42 | imu.setExtCrystalUse(true); //Adafruit BNO055's use external crystal. Enable it, otherwise it does not work. 43 | imu.setAxisRemap(Adafruit_BNO055::REMAP_CONFIG_P0); 44 | imu.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P0); 45 | m_Logger.info("Connected to BNO055 at address 0x%02x", addr); 46 | 47 | working = true; 48 | configured = true; 49 | } 50 | 51 | void BNO055Sensor::motionLoop() { 52 | #if ENABLE_INSPECTION 53 | { 54 | Vector3 gyro = imu.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); 55 | Vector3 accel = imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); 56 | Vector3 mag = imu.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); 57 | 58 | networkConnection.sendInspectionRawIMUData(sensorId, UNPACK_VECTOR(gyro), 255, UNPACK_VECTOR(accel), 255, UNPACK_VECTOR(mag), 255); 59 | } 60 | #endif 61 | 62 | // TODO Optimize a bit with setting rawQuat directly 63 | Quat quat = imu.getQuat(); 64 | fusedRotation.set(quat.x, quat.y, quat.z, quat.w); 65 | fusedRotation *= sensorOffset; 66 | setFusedRotationReady(); 67 | 68 | #if SEND_ACCELERATION 69 | { 70 | acceleration = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); 71 | setAccelerationReady(); 72 | } 73 | #endif 74 | } 75 | 76 | void BNO055Sensor::startCalibration(int calibrationType) { 77 | 78 | } 79 | -------------------------------------------------------------------------------- /lib/ota/ota.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "ota.h" 25 | 26 | const unsigned long bootTime = millis(); 27 | bool enabled = true; 28 | 29 | void OTA::otaSetup(const char * const otaPassword) { 30 | if(otaPassword[0] == '\0') { 31 | enabled = false; 32 | return; // No password set up, disable OTA 33 | } 34 | ArduinoOTA.setPassword(otaPassword); 35 | 36 | ArduinoOTA.onStart([]() { 37 | String type; 38 | if (ArduinoOTA.getCommand() == U_FLASH) 39 | type = "sketch"; 40 | else // U_SPIFFS 41 | type = "filesystem"; 42 | 43 | // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end() 44 | Serial.println("Start updating " + type); 45 | }); 46 | ArduinoOTA.onEnd([]() { 47 | Serial.println("\nEnd"); 48 | }); 49 | ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) { 50 | Serial.printf("Progress: %u%%\n", (progress / (total / 100))); 51 | }); 52 | ArduinoOTA.onError([](ota_error_t error) { 53 | Serial.printf("Error[%u]: ", error); 54 | if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed"); 55 | else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed"); 56 | else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed"); 57 | else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed"); 58 | else if (error == OTA_END_ERROR) Serial.println("End Failed"); 59 | }); 60 | ArduinoOTA.begin(); 61 | 62 | Serial.println("[NOTICE] OTA updates allowed"); 63 | } 64 | 65 | void OTA::otaUpdate() { 66 | if(enabled) { 67 | if(bootTime + 60000 < millis()) { 68 | // Disable OTA 60 seconds after boot as protection measure 69 | enabled = false; 70 | Serial.println("[NOTICE] OTA updates disabled by timeout, this is not an error"); 71 | return; 72 | } 73 | ArduinoOTA.handle(); 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /src/defines_bmi160.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef BMI160_DEFINES_H 24 | #define BMI160_DEFINES_H 25 | 26 | // BMI160 magnetometer type, applies to both main and aux trackers, mixed types are not supported currently. 27 | // If only 1 out of 2 trackers has a mag, tracker without a mag should still function normally. 28 | // NOT USED if USE_6_AXIS == true 29 | // Pick one: 30 | #define BMI160_MAG_TYPE BMI160_MAG_TYPE_HMC 31 | // #define BMI160_MAG_TYPE BMI160_MAG_TYPE_QMC 32 | 33 | // Use VQF instead of mahony sensor fusion. 34 | // Features: rest bias estimation, magnetic distortion rejection. 35 | #define BMI160_USE_VQF true 36 | 37 | // Use BasicVQF instead of VQF (if BMI160_USE_VQF == true). 38 | // Disables the features above. 39 | #define BMI160_USE_BASIC_VQF false 40 | 41 | // Use temperature calibration. 42 | #define BMI160_USE_TEMPCAL true 43 | 44 | // How long to run gyro calibration for. 45 | // Disables this calibration step if value is 0. 46 | // Default: 5 47 | #define BMI160_CALIBRATION_GYRO_SECONDS 5 48 | 49 | // Calibration method options: 50 | // - Skip: disable this calibration step; 51 | // - Rotation: rotate the device in hand; 52 | // - 6 point: put the device in 6 unique orientations. 53 | // Default: ACCEL_CALIBRATION_METHOD_6POINT 54 | // #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_SKIP 55 | // #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_ROTATION 56 | #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_6POINT 57 | 58 | // How long to run magnetometer calibration for, if enabled and you have added a magnetometer. 59 | // Magnetometer not be used until you calibrate it. 60 | // Disables this calibration step if value is 0. 61 | // NOT USED if USE_6_AXIS == true 62 | // Default: 20 63 | #define BMI160_CALIBRATION_MAG_SECONDS 20 64 | 65 | // Send temperature to the server as AXXYY, 66 | // where XX is calibration progress from 0 to 60, and YY is temperature, 67 | // A is 1: not in calibration mode or 2: calibration in progress. 68 | #define BMI160_TEMPCAL_DEBUG false 69 | 70 | // Print debug info every second. 71 | #define BMI160_DEBUG false 72 | 73 | // Use sensitivity calibration. 74 | #define BMI160_USE_SENSCAL true 75 | 76 | #endif -------------------------------------------------------------------------------- /src/LEDManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_LEDMANAGER_H 24 | #define SLIMEVR_LEDMANAGER_H 25 | 26 | #include 27 | #include "globals.h" 28 | #include "logging/Logger.h" 29 | 30 | #define DEFAULT_LENGTH 300 31 | #define DEFAULT_GAP 500 32 | #define DEFAULT_INTERVAL 3000 33 | 34 | #define STANDBUY_LENGTH DEFAULT_LENGTH 35 | #define IMU_ERROR_LENGTH DEFAULT_LENGTH 36 | #define IMU_ERROR_INTERVAL 1000 37 | #define IMU_ERROR_COUNT 5 38 | #define LOW_BATTERY_LENGTH DEFAULT_LENGTH 39 | #define LOW_BATTERY_INTERVAL 300 40 | #define LOW_BATTERY_COUNT 1 41 | #define WIFI_CONNECTING_LENGTH DEFAULT_LENGTH 42 | #define WIFI_CONNECTING_INTERVAL 3000 43 | #define WIFI_CONNECTING_COUNT 3 44 | #define SERVER_CONNECTING_LENGTH DEFAULT_LENGTH 45 | #define SERVER_CONNECTING_INTERVAL 3000 46 | #define SERVER_CONNECTING_COUNT 2 47 | 48 | namespace SlimeVR 49 | { 50 | enum LEDStage 51 | { 52 | OFF, 53 | ON, 54 | GAP, 55 | INTERVAL 56 | }; 57 | 58 | class LEDManager 59 | { 60 | public: 61 | LEDManager(uint8_t pin) : m_Pin(pin) {} 62 | 63 | void setup(); 64 | 65 | /*! 66 | * @brief Turns the LED on 67 | */ 68 | void on(); 69 | 70 | /*! 71 | * @brief Turns the LED off 72 | */ 73 | void off(); 74 | 75 | /*! 76 | * @brief Blink the LED for [time]ms. *Can* cause lag 77 | * @param time Amount of ms to turn the LED on 78 | */ 79 | void blink(unsigned long time); 80 | 81 | /*! 82 | * @brief Show a pattern on the LED. *Can* cause lag 83 | * @param timeon Amount of ms to turn the LED on 84 | * @param timeoff Amount of ms to turn the LED off 85 | * @param times Amount of times to display the pattern 86 | */ 87 | void pattern(unsigned long timeon, unsigned long timeoff, int times); 88 | 89 | void update(); 90 | 91 | private: 92 | uint8_t m_CurrentCount = 0; 93 | unsigned long m_Timer = 0; 94 | LEDStage m_CurrentStage = OFF; 95 | unsigned long m_LastUpdate = millis(); 96 | 97 | uint8_t m_Pin; 98 | 99 | Logging::Logger m_Logger = Logging::Logger("LEDManager"); 100 | }; 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /src/network/featureflags.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2023 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_FEATURE_FLAGS_H_ 25 | #define SLIMEVR_FEATURE_FLAGS_H_ 26 | 27 | #include 28 | #include 29 | 30 | /** 31 | * Bit packed flags, enum values start with 0 and indicate which bit it is. 32 | * 33 | * Change the enums and `flagsEnabled` inside to extend. 34 | */ 35 | struct ServerFeatures { 36 | public: 37 | enum EServerFeatureFlags: uint32_t { 38 | // Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). 39 | PROTOCOL_BUNDLE_SUPPORT, 40 | 41 | // Add new flags here 42 | 43 | BITS_TOTAL, 44 | }; 45 | 46 | bool has(EServerFeatureFlags flag) { 47 | uint32_t bit = static_cast(flag); 48 | return m_Available && (m_Flags[bit / 8] & (1 << (bit % 8))); 49 | } 50 | 51 | /** 52 | * Whether the server supports the "feature flags" feature, 53 | * set to true when we've received flags packet from the server. 54 | */ 55 | bool isAvailable() { 56 | return m_Available; 57 | } 58 | 59 | static ServerFeatures from(uint8_t* received, uint32_t length) { 60 | ServerFeatures res; 61 | res.m_Available = true; 62 | memcpy(res.m_Flags, received, std::min(static_cast(sizeof(res.m_Flags)), length)); 63 | return res; 64 | } 65 | 66 | private: 67 | bool m_Available = false; 68 | 69 | uint8_t m_Flags[static_cast(EServerFeatureFlags::BITS_TOTAL) / 8 + 1]; 70 | }; 71 | 72 | class FirmwareFeatures { 73 | public: 74 | enum EFirmwareFeatureFlags: uint32_t { 75 | // EXAMPLE_FEATURE, 76 | B64_WIFI_SCANNING = 1, 77 | 78 | // Add new flags here 79 | 80 | BITS_TOTAL, 81 | }; 82 | 83 | // Flags to send 84 | static constexpr const std::initializer_list flagsEnabled = { 85 | // EXAMPLE_FEATURE, 86 | B64_WIFI_SCANNING, 87 | 88 | // Add enabled flags here 89 | }; 90 | 91 | static constexpr auto flags = []{ 92 | constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1; 93 | std::array packed{}; 94 | 95 | for (uint32_t bit : flagsEnabled) { 96 | packed[bit / 8] |= 1 << (bit % 8); 97 | } 98 | 99 | return packed; 100 | }(); 101 | }; 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /src/sensors/SensorFusionDMP.cpp: -------------------------------------------------------------------------------- 1 | #include "SensorFusionDMP.h" 2 | 3 | namespace SlimeVR 4 | { 5 | namespace Sensors 6 | { 7 | void SensorFusionDMP::updateAcc(sensor_real_t Axyz[3]) 8 | { 9 | std::copy(Axyz, Axyz+3, bAxyz); 10 | } 11 | 12 | void SensorFusionDMP::updateMag(sensor_real_t Mxyz[3]) 13 | { 14 | if (!magExist) { 15 | if (Mxyz[0] != 0.0f || Mxyz[1] != 0.0f || Mxyz[2] != 0.0f) { 16 | magExist = true; 17 | } else { 18 | return; 19 | } 20 | } 21 | 22 | getGravityVec(); 23 | dmpmag.update(qwxyz, bqwxyz, vecGravity, Mxyz); 24 | } 25 | 26 | void SensorFusionDMP::updateQuaternion(sensor_real_t nqwxyz[4]) 27 | { 28 | std::copy(nqwxyz, nqwxyz+4, bqwxyz); 29 | 30 | updated = true; 31 | gravityReady = false; 32 | linaccelReady = false; 33 | } 34 | 35 | void SensorFusionDMP::updateQuaternion(Quaternion const & nq) 36 | { 37 | bqwxyz[0] = nq.w; bqwxyz[1] = nq.x; bqwxyz[2] = nq.y; bqwxyz[3] = nq.z; 38 | 39 | updated = true; 40 | gravityReady = false; 41 | linaccelReady = false; 42 | } 43 | 44 | void SensorFusionDMP::updateQuaternion(Quat const & nq) 45 | { 46 | bqwxyz[0] = nq.w; bqwxyz[1] = nq.x; bqwxyz[2] = nq.y; bqwxyz[3] = nq.z; 47 | 48 | updated = true; 49 | gravityReady = false; 50 | linaccelReady = false; 51 | } 52 | 53 | bool SensorFusionDMP::isUpdated() 54 | { 55 | return updated; 56 | } 57 | 58 | void SensorFusionDMP::clearUpdated() 59 | { 60 | updated = false; 61 | } 62 | 63 | sensor_real_t const * SensorFusionDMP::getQuaternion() 64 | { 65 | if (!magExist) { 66 | // remap axis from DMP space to sensor space 67 | qwxyz[0] = bqwxyz[0]; 68 | qwxyz[1] = -bqwxyz[2]; 69 | qwxyz[2] = bqwxyz[1]; 70 | qwxyz[3] = bqwxyz[3]; 71 | } 72 | // dmpmag remaps axis during Mag update 73 | return qwxyz; 74 | } 75 | 76 | Quat SensorFusionDMP::getQuaternionQuat() 77 | { 78 | getQuaternion(); 79 | return Quat(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]); 80 | } 81 | 82 | sensor_real_t const * SensorFusionDMP::getGravityVec() 83 | { 84 | if (!gravityReady) { 85 | SensorFusion::calcGravityVec(bqwxyz, vecGravity); 86 | gravityReady = true; 87 | } 88 | return vecGravity; 89 | } 90 | 91 | sensor_real_t const * SensorFusionDMP::getLinearAcc() 92 | { 93 | if (!linaccelReady) { 94 | getGravityVec(); 95 | SensorFusion::calcLinearAcc(bAxyz, vecGravity, linAccel); 96 | linaccelReady = true; 97 | } 98 | return linAccel; 99 | } 100 | 101 | void SensorFusionDMP::getLinearAcc(sensor_real_t outLinAccel[3]) 102 | { 103 | getLinearAcc(); 104 | std::copy(linAccel, linAccel+3, outLinAccel); 105 | } 106 | 107 | Vector3 SensorFusionDMP::getLinearAccVec() 108 | { 109 | getLinearAcc(); 110 | return Vector3(linAccel[0], linAccel[1], linAccel[2]); 111 | } 112 | } 113 | } 114 | -------------------------------------------------------------------------------- /src/sensors/sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #include "sensor.h" 24 | #include "GlobalVars.h" 25 | #include 26 | #include "calibration.h" 27 | 28 | SensorStatus Sensor::getSensorState() { 29 | return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR; 30 | } 31 | 32 | void Sensor::setAccelerationReady() { 33 | newAcceleration = true; 34 | } 35 | 36 | void Sensor::setFusedRotationReady() { 37 | bool changed = OPTIMIZE_UPDATES ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) : true; 38 | if (ENABLE_INSPECTION || changed) { 39 | newFusedRotation = true; 40 | lastFusedRotationSent = fusedRotation; 41 | } 42 | } 43 | 44 | void Sensor::sendData() { 45 | if (newFusedRotation) { 46 | newFusedRotation = false; 47 | networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); 48 | 49 | #ifdef DEBUG_SENSOR 50 | m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); 51 | #endif 52 | 53 | #if SEND_ACCELERATION 54 | if (newAcceleration) { 55 | newAcceleration = false; 56 | networkConnection.sendSensorAcceleration(sensorId, acceleration); 57 | } 58 | #endif 59 | } 60 | } 61 | 62 | void Sensor::printTemperatureCalibrationUnsupported() { 63 | m_Logger.error("Temperature calibration not supported for IMU %s", getIMUNameByType(sensorType)); 64 | } 65 | void Sensor::printTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; 66 | void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; 67 | void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; 68 | void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; 69 | 70 | const char * getIMUNameByType(int imuType) { 71 | switch(imuType) { 72 | case IMU_MPU9250: 73 | return "MPU9250"; 74 | case IMU_MPU6500: 75 | return "MPU6500"; 76 | case IMU_BNO080: 77 | return "BNO080"; 78 | case IMU_BNO085: 79 | return "BNO085"; 80 | case IMU_BNO055: 81 | return "BNO055"; 82 | case IMU_MPU6050: 83 | return "MPU6050"; 84 | case IMU_BNO086: 85 | return "BNO086"; 86 | case IMU_BMI160: 87 | return "BMI160"; 88 | case IMU_ICM20948: 89 | return "ICM20948"; 90 | case IMU_ICM42688: 91 | return "ICM42688"; 92 | } 93 | return "Unknown"; 94 | } 95 | -------------------------------------------------------------------------------- /src/sensors/axisremap.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2023 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef AXISREMAP_H 24 | #define AXISREMAP_H 25 | 26 | // Number label for axis selections 27 | // 3 bits for each axis, 9 bits for one sensor, 18 bits at total 28 | // bit 1-0 29 | #define AXIS_REMAP_USE_X 0 30 | #define AXIS_REMAP_USE_Y 1 31 | #define AXIS_REMAP_USE_Z 2 32 | // bit 2 33 | #define AXIS_REMAP_INVERT 4 34 | 35 | #define AXIS_REMAP_USE_XN (AXIS_REMAP_USE_X | AXIS_REMAP_INVERT) 36 | #define AXIS_REMAP_USE_YN (AXIS_REMAP_USE_Y | AXIS_REMAP_INVERT) 37 | #define AXIS_REMAP_USE_ZN (AXIS_REMAP_USE_Z | AXIS_REMAP_INVERT) 38 | 39 | // Macros to extract axis selection from remap descriptor 40 | #define AXIS_REMAP_GET_X(desc) (desc & 0x07) 41 | #define AXIS_REMAP_GET_Y(desc) ((desc >> 3) & 0x07) 42 | #define AXIS_REMAP_GET_Z(desc) ((desc >> 6) & 0x07) 43 | #define AXIS_REMAP_GET_MAGX(desc) ((desc >> 9) & 0x07) 44 | #define AXIS_REMAP_GET_MAGY(desc) ((desc >> 12) & 0x07) 45 | #define AXIS_REMAP_GET_MAGZ(desc) ((desc >> 15) & 0x07) 46 | #define AXIS_REMAP_GET_ALL_IMU(desc) (desc & 0x1FF) 47 | #define AXIS_REMAP_GET_ALL_MAG(desc) ((desc >> 9) & 0x1FF) 48 | 49 | // Macro to build remap descriptor 50 | #define AXIS_REMAP_AXIS_X(x) (x & 0x07) 51 | #define AXIS_REMAP_AXIS_Y(y) ((y & 0x07) << 3) 52 | #define AXIS_REMAP_AXIS_Z(z) ((z & 0x07) << 6) 53 | #define AXIS_REMAP_AXIS_MAGX(x) ((x & 0x07) << 9) 54 | #define AXIS_REMAP_AXIS_MAGY(y) ((y & 0x07) << 12) 55 | #define AXIS_REMAP_AXIS_MAGZ(z) ((z & 0x07) << 15) 56 | 57 | #define AXIS_REMAP_BUILD(x, y, z, mx, my, mz) \ 58 | (AXIS_REMAP_AXIS_X(x) | AXIS_REMAP_AXIS_Y(y) | AXIS_REMAP_AXIS_Z(z) | \ 59 | AXIS_REMAP_AXIS_MAGX(mx) | AXIS_REMAP_AXIS_MAGY(my) | AXIS_REMAP_AXIS_MAGZ(mz)) 60 | 61 | #define AXIS_REMAP_DEFAULT AXIS_REMAP_BUILD(AXIS_REMAP_USE_X, AXIS_REMAP_USE_Y, AXIS_REMAP_USE_Z, \ 62 | AXIS_REMAP_USE_X, AXIS_REMAP_USE_Y, AXIS_REMAP_USE_Z) 63 | 64 | // Template functions for remapping 65 | template 66 | T inline remapOneAxis(int axisdesc, T x, T y, T z) { 67 | T result; 68 | switch (axisdesc & 0x3) { 69 | case AXIS_REMAP_USE_X: result = x; break; 70 | case AXIS_REMAP_USE_Y: result = y; break; 71 | case AXIS_REMAP_USE_Z: result = z; break; 72 | default: result = 0; 73 | } 74 | return (axisdesc & AXIS_REMAP_INVERT) ? -result : result; 75 | } 76 | 77 | template 78 | void inline remapAllAxis(int axisdesc, T *x, T *y, T *z) { 79 | T bx = *x, by = *y, bz = *z; 80 | *x = remapOneAxis(AXIS_REMAP_GET_X(axisdesc), bx, by, bz); 81 | *y = remapOneAxis(AXIS_REMAP_GET_Y(axisdesc), bx, by, bz); 82 | *z = remapOneAxis(AXIS_REMAP_GET_Z(axisdesc), bx, by, bz); 83 | } 84 | 85 | #endif // AXISREMAP_H 86 | -------------------------------------------------------------------------------- /test/i2cscan.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * i2c_port_address_scanner 3 | * Scans ports D0 to D7 on an ESP8266 and searches for I2C device. based on the original code 4 | * available on Arduino.cc and later improved by user Krodal and Nick Gammon (www.gammon.com.au/forum/?id=10896) 5 | * D8 throws exceptions thus it has been left out 6 | * 7 | */ 8 | #include 9 | #include 10 | 11 | #include "ota.h" 12 | #include "configuration/Configuration.h" 13 | #include "helper_3dmath.h" 14 | #include "udpclient.h" 15 | #include "credentials.h" 16 | 17 | SlimeVR::Configuration::DeviceConfig config; 18 | 19 | uint8_t portArray[] = {16, 5, 4, 0, 2, 14, 12, 13}; 20 | //String portMap[] = {"D0", "D1", "D2", "D3", "D4", "D5", "D6", "D7"}; //for Wemos 21 | String portMap[] = {"GPIO16", "GPIO5", "GPIO4", "GPIO0", "GPIO2", "GPIO14", "GPIO12", "GPIO13"}; 22 | 23 | 24 | void check_if_exist_I2C() { 25 | byte error, address; 26 | int nDevices; 27 | nDevices = 0; 28 | for (address = 1; address < 127; address++ ) { 29 | // The i2c_scanner uses the return value of 30 | // the Write.endTransmisstion to see if 31 | // a device did acknowledge to the address. 32 | Wire.beginTransmission(address); 33 | error = Wire.endTransmission(); 34 | 35 | if (error == 0){ 36 | Serial.print("I2C device found at address 0x"); 37 | if (address < 16) 38 | Serial.print("0"); 39 | Serial.print(address, HEX); 40 | Serial.println(" !"); 41 | 42 | nDevices++; 43 | } else if (error == 4) { 44 | Serial.print("Unknow error at address 0x"); 45 | if (address < 16) 46 | Serial.print("0"); 47 | Serial.println(address, HEX); 48 | } 49 | } //for loop 50 | if (nDevices == 0) 51 | Serial.println("No I2C devices found"); 52 | else 53 | Serial.println("Scan finished\n"); 54 | } 55 | 56 | void scanPorts() { 57 | for (uint8_t i = 0; i < sizeof(portArray); i++) { 58 | for (uint8_t j = 0; j < sizeof(portArray); j++) { 59 | if (i != j){ 60 | Serial.print("Scanning (SDA : SCL) - " + portMap[i] + " : " + portMap[j] + " - "); 61 | Wire.begin(portArray[i], portArray[j]); 62 | check_if_exist_I2C(); 63 | } 64 | } 65 | } 66 | } 67 | 68 | void I2C_recovery() { 69 | Serial.println("Starting I2C bus recovery"); 70 | delay(2000); 71 | int SDAPIN = 70; 72 | int CLKPIN = 71; 73 | //try i2c bus recovery at 100kHz = 5uS high, 5uS low 74 | pinMode(SDAPIN, OUTPUT);//keeping SDA high during recovery 75 | digitalWrite(SDAPIN, HIGH); 76 | pinMode(CLKPIN, OUTPUT); 77 | for (int i = 0; i < 10; i++) { //9nth cycle acts as NACK 78 | digitalWrite(CLKPIN, HIGH); 79 | delayMicroseconds(5); 80 | digitalWrite(CLKPIN, LOW); 81 | delayMicroseconds(5); 82 | } 83 | 84 | //a STOP signal (SDA from low to high while CLK is high) 85 | digitalWrite(SDAPIN, LOW); 86 | delayMicroseconds(5); 87 | digitalWrite(CLKPIN, HIGH); 88 | delayMicroseconds(2); 89 | digitalWrite(SDAPIN, HIGH); 90 | delayMicroseconds(2); 91 | //bus status is now : FREE 92 | 93 | Serial.println("bus recovery done, starting scan in 2 secs"); 94 | //return to power up mode 95 | pinMode(SDAPIN, INPUT); 96 | pinMode(CLKPIN, INPUT); 97 | delay(2000); 98 | //pins + begin advised in https://github.com/esp8266/Arduino/issues/452 99 | //Wire1.pins(SDAPIN, CLKPIN); //this changes default values for sda and clock as well 100 | Wire.begin(); 101 | //only pins: no signal on clk and sda 102 | //only begin: no signal on clk, no signal on sda 103 | } 104 | 105 | void loop() { 106 | otaUpdate(); 107 | } 108 | 109 | void setup() { 110 | Serial.begin(115200); 111 | while (!Serial); // Leonardo: wait for serial monitor 112 | Serial.println("\n\nI2C Scanner to scan for devices on each port pair D0 to D7"); 113 | scanPorts(); 114 | I2C_recovery(); 115 | scanPorts(); 116 | 117 | setUpWiFi(&config); 118 | otaSetup(otaPassword); 119 | } 120 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | 9 | ; ================================================ 10 | ; See docs for configuration options and examples: 11 | ; https://docs.slimevr.dev/firmware/configuring-project.html#1-configuring-platformioini 12 | ; ================================================ 13 | 14 | [env] 15 | lib_deps= 16 | https://github.com/SlimeVR/CmdParser.git 17 | https://github.com/SlimeVR/base64_arduino.git 18 | monitor_speed = 115200 19 | monitor_echo = yes 20 | monitor_filters = colorize 21 | ;monitor_rts = 0 22 | ;monitor_dtr = 0 23 | framework = arduino 24 | build_flags = 25 | !python scripts/get_git_commit.py 26 | ;If you want to set hardcoded WiFi SSID and password, uncomment and edit the lines below 27 | ;To uncomment, only remove ";" and leave the two spaces in front of the tags 28 | ; '" - quotes are necessary! 29 | ; -DWIFI_CREDS_SSID='"SSID"' 30 | ; -DWIFI_CREDS_PASSWD='"PASSWORD"' 31 | 32 | ;If you want to set a static IP address, uncomment and edit the lines below 33 | ; -DWIFI_USE_STATICIP 34 | ; -DWIFI_STATIC_IP=192,168,XXX,XXX 35 | ; -DWIFI_STATIC_GATEWAY=192,168,XXX,XXX 36 | ; -DWIFI_STATIC_SUBNET=255,255,255,0 37 | 38 | ; Uncomment below if your board are using 40MHz crystal instead of 26MHz for ESP8266 39 | ; -DF_CRYSTAL=40000000 40 | 41 | ; Enable -O2 GCC optimization 42 | -O2 43 | -std=gnu++17 44 | 45 | build_unflags = -Os -std=gnu++11 46 | 47 | ; If you want to enable OTA Updates, uncomment and set OTA password here and in credentials.h 48 | ; You can set upload_port to device's ip after it's set up for the first time 49 | ; Use the same password in SlimeVR Server to let it OTA Update the device 50 | ;upload_protocol = espota 51 | ;upload_port = 192.168.1.49 52 | ;upload_flags = 53 | ; --auth=SlimeVR-OTA 54 | 55 | ; Settings for different boards 56 | 57 | [env:esp12e] 58 | platform = espressif8266 @ 4.2.0 59 | board = esp12e 60 | ; Comment out this line below if you have any trouble uploading the firmware 61 | ; and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed 62 | upload_speed = 921600 63 | 64 | ; Uncomment below if you want to build for ESP-01 65 | ;[env:esp01_1m] 66 | ;platform = espressif8266 @ 4.2.0 67 | ;board = esp01_1m 68 | ;board_build.arduino.ldscript = "eagle.flash.1m64.ld" 69 | 70 | ; Uncomment below if you want to build for ESP8285 (ESP8266 with embedded Flash) 71 | ;[env:esp8285] 72 | ;platform = espressif8266 @ 4.2.0 73 | ;board = esp8285 74 | ;board_build.arduino.ldscript = "eagle.flash.1m64.ld" 75 | ;board_build.flash_mode = dout 76 | 77 | ; Uncomment below if you want to build for esp32 78 | ; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards 79 | ; [env:esp32] 80 | ; platform = espressif32 @ 6.1.0 81 | ; board = esp32dev 82 | ; Comment out this line below if you have any trouble uploading the firmware - and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed 83 | ;upload_speed = 921600 84 | 85 | ; If you want to use a ESP32C3, you can use this (experimental) 86 | ; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards 87 | ; There are 2 main Boardtypes: 88 | ; 1. Boards that use a USB 2 Serial Chipset ( esp32-c3-devkitm-1, ttgo-t-oi-plus ) 89 | ; 2. Boards that relay on the USB interface of the ESP32C3 ( lolin_c3_mini , dfrobot_beetle_esp32c3) 90 | ; On this board there are 2 type some of them need to have set the build flag (menuconfig) 91 | ; -DARDUINO_USB_MODE=1 92 | ; -DARDUINO_USB_CDC_ON_BOOT=1 93 | 94 | ;[env:esp32c3] 95 | ;platform = espressif32 @ 6.1.0 96 | ;build_flags = 97 | ; ${env.build_flags} 98 | ; -DESP32C3 99 | ;board = lolin_c3_mini 100 | -------------------------------------------------------------------------------- /src/batterymonitor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_BATTERYMONITOR_H_ 24 | #define SLIMEVR_BATTERYMONITOR_H_ 25 | 26 | #include 27 | #include "globals.h" 28 | #include 29 | #include 30 | #include "logging/Logger.h" 31 | 32 | #if ESP8266 33 | #define ADCResulution 1023.0 // ESP8266 has 12bit ADC 34 | #define ADCVoltageMax 1.0 // ESP8266 input is 1.0 V = 1023.0 35 | #endif 36 | #if ESP32 37 | #define ADCResulution 4095.0 // ESP32 has 12bit ADC 38 | #define ADCVoltageMax 3.3 // ESP32 input is 3.3 V = 4095.0 39 | #endif 40 | #ifndef ADCResulution 41 | #define ADCResulution 1023.0 42 | #endif 43 | #ifndef ADCVoltageMax 44 | #define ADCVoltageMax 1.0 45 | #endif 46 | 47 | #ifndef BATTERY_SHIELD_RESISTANCE 48 | #define BATTERY_SHIELD_RESISTANCE 180.0 49 | #endif 50 | #ifndef BATTERY_SHIELD_R1 51 | #define BATTERY_SHIELD_R1 100.0 52 | #endif 53 | #ifndef BATTERY_SHIELD_R2 54 | #define BATTERY_SHIELD_R2 220.0 55 | #endif 56 | 57 | #if BATTERY_MONITOR == BAT_EXTERNAL 58 | #ifndef PIN_BATTERY_LEVEL 59 | #error Internal ADC enabled without pin! Please select a pin. 60 | #endif 61 | // 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 62 | // 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 63 | // ESP32 Boards may have not the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can be changed. 64 | // Diagramm: 65 | // (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2] ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND) 66 | // SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0 67 | #define batteryADCMultiplier ADCVoltageMax / ADCResulution * (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) / BATTERY_SHIELD_R1 68 | #elif BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 69 | // Default recommended resistors are 9.1k and 5.1k 70 | #define batteryADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1 71 | #endif 72 | 73 | class BatteryMonitor 74 | { 75 | public: 76 | void Setup(); 77 | void Loop(); 78 | 79 | private: 80 | unsigned long last_battery_sample = 0; 81 | #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 82 | uint8_t address = 0; 83 | #endif 84 | #if BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 85 | uint16_t voltage_3_3 = 3000; 86 | #endif 87 | float voltage = -1; 88 | float level = -1; 89 | 90 | SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor"); 91 | }; 92 | 93 | #endif // SLIMEVR_BATTERYMONITOR_H_ 94 | -------------------------------------------------------------------------------- /src/configuration/CalibrationConfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H 25 | #define SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H 26 | 27 | #include 28 | 29 | namespace SlimeVR { 30 | namespace Configuration { 31 | struct BMI160CalibrationConfig { 32 | // accelerometer offsets and correction matrix 33 | float A_B[3]; 34 | float A_Ainv[3][3]; 35 | 36 | // magnetometer offsets and correction matrix 37 | float M_B[3]; 38 | float M_Ainv[3][3]; 39 | 40 | // raw offsets, determined from gyro at rest 41 | float G_off[3]; 42 | 43 | // calibration temperature for dynamic compensation 44 | float temperature; 45 | }; 46 | 47 | struct MPU6050CalibrationConfig { 48 | // accelerometer offsets and correction matrix 49 | float A_B[3]; 50 | 51 | // raw offsets, determined from gyro at rest 52 | float G_off[3]; 53 | }; 54 | 55 | struct MPU9250CalibrationConfig { 56 | // accelerometer offsets and correction matrix 57 | float A_B[3]; 58 | float A_Ainv[3][3]; 59 | 60 | // magnetometer offsets and correction matrix 61 | float M_B[3]; 62 | float M_Ainv[3][3]; 63 | 64 | // raw offsets, determined from gyro at rest 65 | float G_off[3]; 66 | }; 67 | 68 | struct ICM20948CalibrationConfig { 69 | // gyroscope bias 70 | int32_t G[3]; 71 | 72 | // accelerometer bias 73 | int32_t A[3]; 74 | 75 | // compass bias 76 | int32_t C[3]; 77 | }; 78 | 79 | struct ICM42688CalibrationConfig { 80 | // accelerometer offsets and correction matrix 81 | float A_B[3]; 82 | float A_Ainv[3][3]; 83 | 84 | // magnetometer offsets and correction matrix 85 | float M_B[3]; 86 | float M_Ainv[3][3]; 87 | 88 | // raw offsets, determined from gyro at rest 89 | float G_off[3]; 90 | }; 91 | 92 | enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, ICM42688 }; 93 | 94 | const char* calibrationConfigTypeToString(CalibrationConfigType type); 95 | 96 | struct CalibrationConfig { 97 | CalibrationConfigType type; 98 | 99 | union { 100 | BMI160CalibrationConfig bmi160; 101 | MPU6050CalibrationConfig mpu6050; 102 | MPU9250CalibrationConfig mpu9250; 103 | ICM20948CalibrationConfig icm20948; 104 | ICM42688CalibrationConfig icm42688; 105 | } data; 106 | }; 107 | } 108 | } 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /src/sensors/sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SLIMEVR_SENSOR_H_ 25 | #define SLIMEVR_SENSOR_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include "configuration/Configuration.h" 31 | #include "globals.h" 32 | #include "logging/Logger.h" 33 | #include "utils.h" 34 | 35 | #define DATA_TYPE_NORMAL 1 36 | #define DATA_TYPE_CORRECTION 2 37 | 38 | enum class SensorStatus : uint8_t { 39 | SENSOR_OFFLINE = 0, 40 | SENSOR_OK = 1, 41 | SENSOR_ERROR = 2 42 | }; 43 | 44 | class Sensor 45 | { 46 | public: 47 | Sensor(const char *sensorName, uint8_t type, uint8_t id, uint8_t address, float rotation, uint8_t sclpin=0, uint8_t sdapin=0) 48 | : addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName)), 49 | sclPin(sclpin), sdaPin(sdapin) 50 | { 51 | char buf[4]; 52 | sprintf(buf, "%u", id); 53 | m_Logger.setTag(buf); 54 | } 55 | 56 | virtual ~Sensor(){}; 57 | virtual void motionSetup(){}; 58 | virtual void postSetup(){}; 59 | virtual void motionLoop(){}; 60 | virtual void sendData(); 61 | virtual void setAccelerationReady(); 62 | virtual void setFusedRotationReady(); 63 | virtual void startCalibration(int calibrationType){}; 64 | virtual SensorStatus getSensorState(); 65 | virtual void printTemperatureCalibrationState(); 66 | virtual void printDebugTemperatureCalibrationState(); 67 | virtual void resetTemperatureCalibrationState(); 68 | virtual void saveTemperatureCalibration(); 69 | bool isWorking() { 70 | return working; 71 | }; 72 | bool isValid() { 73 | return sclPin != sdaPin; 74 | }; 75 | uint8_t getSensorId() { 76 | return sensorId; 77 | }; 78 | uint8_t getSensorType() { 79 | return sensorType; 80 | }; 81 | const Vector3& getAcceleration() { 82 | return acceleration; 83 | }; 84 | const Quat& getFusedRotation() { 85 | return fusedRotation; 86 | }; 87 | bool hasNewDataToSend() { 88 | return newFusedRotation || newAcceleration; 89 | }; 90 | 91 | bool hadData = false; 92 | protected: 93 | uint8_t addr = 0; 94 | uint8_t sensorId = 0; 95 | uint8_t sensorType = 0; 96 | bool configured = false; 97 | bool working = false; 98 | uint8_t calibrationAccuracy = 0; 99 | Quat sensorOffset; 100 | 101 | bool newFusedRotation = false; 102 | Quat fusedRotation{}; 103 | Quat lastFusedRotationSent{}; 104 | 105 | bool newAcceleration = false; 106 | Vector3 acceleration{}; 107 | 108 | SlimeVR::Logging::Logger m_Logger; 109 | 110 | public: 111 | uint8_t sclPin = 0; 112 | uint8_t sdaPin = 0; 113 | 114 | private: 115 | void printTemperatureCalibrationUnsupported(); 116 | }; 117 | 118 | const char * getIMUNameByType(int imuType); 119 | 120 | #endif // SLIMEVR_SENSOR_H_ 121 | -------------------------------------------------------------------------------- /src/debug.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_DEBUG_H_ 24 | #define SLIMEVR_DEBUG_H_ 25 | #include "consts.h" 26 | #include "logging/Level.h" 27 | 28 | #define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration 29 | #define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs 30 | #define BNO_USE_MAGNETOMETER_CORRECTION false // Set to true to enable magnetometer correction for BNO08x IMUs. Only works with USE_6_AXIS set to true. 31 | #define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently) 32 | #define LOAD_BIAS true // Loads the bias values from NVS on start 33 | #define SAVE_BIAS true // Periodically saves bias calibration data to NVS 34 | #define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only) 35 | #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 36 | #define SEND_ACCELERATION true // send linear acceleration to the server 37 | 38 | //Debug information 39 | 40 | #define LOG_LEVEL LOG_LEVEL_DEBUG 41 | 42 | #if LOG_LEVEL == LOG_LEVEL_TRACE 43 | #define DEBUG_SENSOR 44 | #define DEBUG_NETWORK 45 | #define DEBUG_CONFIGURATION 46 | #endif 47 | 48 | #define serialDebug false // Set to true to get Serial output for debugging 49 | #define serialBaudRate 115200 50 | #define LED_INTERVAL_STANDBY 10000 51 | #define PRINT_STATE_EVERY_MS 60000 52 | 53 | // Determines how often we sample and send data 54 | #define samplingRateInMillis 10 55 | 56 | // Sleeping options 57 | #define POWERSAVING_MODE POWER_SAVING_LEGACY // Minimum causes sporadic data pauses 58 | #if POWERSAVING_MODE >= POWER_SAVING_MINIMUM 59 | #define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000) 60 | #endif 61 | 62 | // Packet bundling/aggregation 63 | #define PACKET_BUNDLING PACKET_BUNDLING_BUFFERED 64 | // Extra tunable for PACKET_BUNDLING_BUFFERED (10000us = 10ms timeout, 100hz target) 65 | #define PACKET_BUNDLING_BUFFER_SIZE_MICROS 10000 66 | 67 | // Setup for the Magnetometer 68 | #define useFullCalibrationMatrix true 69 | 70 | // Battery configuration 71 | #define batterySampleRate 10000 72 | #define BATTERY_LOW_VOLTAGE_DEEP_SLEEP false 73 | #define BATTERY_LOW_POWER_VOLTAGE 3.3f // Voltage to raise error 74 | 75 | // Send updates over network only when changes are substantial 76 | // If "false" updates are sent at the sensor update rate (usually 100 TPS) 77 | // If "true" updates will be less frequent in the time of little motion 78 | // Experimental 79 | #define OPTIMIZE_UPDATES true 80 | 81 | #define I2C_SPEED 400000 82 | 83 | #define COMPLIANCE_MODE true 84 | #define USE_ATTENUATION COMPLIANCE_MODE && ESP8266 85 | #define ATTENUATION_N 10.0 / 4.0 86 | #define ATTENUATION_G 14.0 / 4.0 87 | #define ATTENUATION_B 40.0 / 4.0 88 | 89 | // Send inspection packets over the network to a profiler 90 | // Not recommended for production 91 | #define ENABLE_INSPECTION false 92 | 93 | #define FIRMWARE_BUILD_NUMBER 17 94 | #define FIRMWARE_VERSION "0.4.0" 95 | 96 | #endif // SLIMEVR_DEBUG_H_ 97 | -------------------------------------------------------------------------------- /ci/build.py: -------------------------------------------------------------------------------- 1 | import json 2 | import os 3 | import sys 4 | import shutil 5 | from enum import Enum 6 | from textwrap import dedent 7 | from typing import List 8 | import configparser 9 | 10 | COLOR_ESC = '\033[' 11 | COLOR_RESET = f'{COLOR_ESC}0m' 12 | COLOR_GREEN = f'{COLOR_ESC}32m' 13 | COLOR_RED = f'{COLOR_ESC}31m' 14 | COLOR_CYAN = f'{COLOR_ESC}36m' 15 | COLOR_GRAY = f'{COLOR_ESC}30;1m' 16 | 17 | 18 | class DeviceConfiguration: 19 | def __init__(self, platform: str, board: str, platformio_board: str) -> None: 20 | self.platform = platform 21 | self.board = board 22 | self.platformio_board = platformio_board 23 | 24 | def filename(self) -> str: 25 | return f"{self.platformio_board}.bin" 26 | 27 | def __str__(self) -> str: 28 | return f"{self.platform}@{self.board}" 29 | 30 | 31 | def get_matrix() -> List[DeviceConfiguration]: 32 | matrix: List[DeviceConfiguration] = [] 33 | 34 | config = configparser.ConfigParser() 35 | config.read("./platformio-tools.ini") 36 | for section in config.sections(): 37 | if section == "env": 38 | continue 39 | 40 | board = section.split(":")[1] 41 | platform = config[section]["platform"] 42 | platformio_board = config[section]["board"] 43 | 44 | matrix.append(DeviceConfiguration( 45 | platform, 46 | board, 47 | platformio_board)) 48 | 49 | return matrix 50 | 51 | 52 | def prepare() -> None: 53 | print(f"🡢 {COLOR_CYAN}Preparation{COLOR_RESET}") 54 | 55 | print(f" 🡢 {COLOR_GRAY}Backing up platformio.ini{COLOR_RESET}") 56 | shutil.copy("./platformio.ini", "platformio.ini.bak") 57 | 58 | print( 59 | f" 🡢 {COLOR_GRAY}Switching platformio.ini to platformio-tools.ini{COLOR_RESET}") 60 | shutil.copy("./platformio-tools.ini", "platformio.ini") 61 | 62 | if os.path.exists("./build"): 63 | print(f" 🡢 {COLOR_GRAY}Removing existing build folder...{COLOR_RESET}") 64 | shutil.rmtree("./build") 65 | 66 | print(f" 🡢 {COLOR_GRAY}Creating build folder...{COLOR_RESET}") 67 | os.mkdir("./build") 68 | 69 | print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}") 70 | 71 | 72 | def cleanup() -> None: 73 | print(f"🡢 {COLOR_CYAN}Cleanup{COLOR_RESET}") 74 | 75 | print(f" 🡢 {COLOR_GRAY}Restoring platformio.ini...{COLOR_RESET}") 76 | shutil.copy("platformio.ini.bak", "platformio.ini") 77 | 78 | print(f" 🡢 {COLOR_GRAY}Removing platformio.ini.bak...{COLOR_RESET}") 79 | os.remove("platformio.ini.bak") 80 | 81 | print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}") 82 | 83 | 84 | def build() -> int: 85 | print(f"🡢 {COLOR_CYAN}Build{COLOR_RESET}") 86 | 87 | failed_builds: List[str] = [] 88 | code = 0 89 | 90 | matrix = get_matrix() 91 | 92 | for device in matrix: 93 | print(f" 🡢 {COLOR_CYAN}Building for {device.platform}{COLOR_RESET}") 94 | 95 | status = build_for_device(device) 96 | 97 | if not status: 98 | failed_builds.append(device.platformio_board) 99 | 100 | if len(failed_builds) > 0: 101 | print(f" 🡢 {COLOR_RED}Failed!{COLOR_RESET}") 102 | 103 | for failed_build in failed_builds: 104 | print(f" 🡢 {COLOR_RED}{failed_build}{COLOR_RESET}") 105 | 106 | code = 1 107 | else: 108 | print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}") 109 | 110 | return code 111 | 112 | 113 | def build_for_device(device: DeviceConfiguration) -> bool: 114 | success = True 115 | 116 | print(f"::group::Build {device}") 117 | 118 | code = os.system(f"platformio run -e {device.board}") 119 | 120 | if code == 0: 121 | shutil.copy(f".pio/build/{device.board}/firmware.bin", 122 | f"build/{device.filename()}") 123 | 124 | print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}") 125 | else: 126 | success = False 127 | 128 | print(f" 🡢 {COLOR_RED}Failed!{COLOR_RESET}") 129 | 130 | print("::endgroup::") 131 | 132 | return success 133 | 134 | 135 | def main() -> None: 136 | prepare() 137 | code = build() 138 | cleanup() 139 | 140 | sys.exit(code) 141 | 142 | 143 | if __name__ == "__main__": 144 | main() 145 | -------------------------------------------------------------------------------- /lib/magneto/mymathlib_matrix.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __MYMATHLIB_MATRIX_H__ 3 | 4 | /* Note: This code is from http://www.mymathlib.com/matrices/ and appears to have no license attached. 5 | * I doubt that anyone will pursue us for the use of this library with a copyright date of 2004 6 | * that's been distributed along with magneto for awhile, and the intent of the site seems to be to 7 | * provide code for people to use, but it's something to be aware of. 8 | */ 9 | namespace mymathlib::matrix { 10 | void Multiply_Self_Transpose(double*, double*, int, int); 11 | void Get_Submatrix(double*, int, int, double*, int, int, int); 12 | int Choleski_LU_Decomposition(double*, int); 13 | int Choleski_LU_Inverse(double*, int); 14 | void Multiply_Matrices(double*, double*, int, int, double*, int); 15 | void Identity_Matrix(double*, int); 16 | 17 | int Hessenberg_Form_Elementary(double*, double*, int); 18 | void Hessenberg_Elementary_Transform(double*, double*, int[], int); 19 | 20 | void Copy_Vector(double*, double*, int); 21 | 22 | int QR_Hessenberg_Matrix(double*, double*, double[], double[], int, int); 23 | void One_Real_Eigenvalue(double[], double[], double[], int, double); 24 | void Two_Eigenvalues(double*, double*, double[], double[], int, int, double); 25 | void Update_Row(double*, double, double, int, int); 26 | void Update_Column(double*, double, double, int, int); 27 | void Update_Transformation(double*, double, double, int, int); 28 | void Double_QR_Iteration(double*, double*, int, int, int, double*, int); 29 | void Product_and_Sum_of_Shifts(double*, int, int, double*, double*, double*, int); 30 | int Two_Consecutive_Small_Subdiagonal(double*, int, int, int, double, double); 31 | void Double_QR_Step(double*, int, int, int, double, double, double*, int); 32 | void BackSubstitution(double*, double[], double[], int); 33 | void BackSubstitute_Real_Vector(double*, double[], double[], int, double, int); 34 | void BackSubstitute_Complex_Vector(double*, double[], double[], int, double, int); 35 | void Calculate_Eigenvectors(double*, double*, double[], double[], int); 36 | void Complex_Division(double, double, double, double, double*, double*); 37 | 38 | void Transpose_Square_Matrix(double*, int); 39 | 40 | int Lower_Triangular_Solve(double* L, double B[], double x[], int n); 41 | int Lower_Triangular_Inverse(double* L, int n); 42 | int Upper_Triangular_Solve(double* U, double B[], double x[], int n); 43 | 44 | void Interchange_Rows(double* A, int row1, int row2, int ncols); 45 | void Interchange_Columns(double* A, int col1, int col2, int nrows, int ncols); 46 | void Identity_Matrix(double* A, int n); 47 | void Copy_Vector(double* d, double* s, int n); 48 | 49 | void Hessenberg_Elementary_Transform(double* H, double* S, int perm[], int n); 50 | 51 | void One_Real_Eigenvalue(double Hrow[], double eigen_real[], double eigen_imag[], int row, double shift); 52 | void Two_Eigenvalues(double* H, double* S, double eigen_real[], double eigen_imag[], int n, int k, double t); 53 | void Update_Row(double* Hrow, double cos, double sin, int n, int k); 54 | void Update_Column(double* H, double cos, double sin, int n, int k); 55 | void Update_Transformation(double* S, double cos, double sin, int n, int k); 56 | void Double_QR_Iteration(double* H, double* S, int row, int min_row, int n, double* shift, int iteration); 57 | void Product_and_Sum_of_Shifts(double* H, int n, int max_row, double* shift, double* trace, double* det, int iteration); 58 | int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, int max_row, int n, double trace, double det); 59 | void Double_QR_Step(double* H, int min_row, int max_row, int min_col, double trace, double det, double* S, int n); 60 | void Complex_Division(double x, double y, double u, double v, double* a, double* b); 61 | void BackSubstitution(double* H, double eigen_real[], double eigen_imag[], int n); 62 | void BackSubstitute_Real_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n); 63 | void BackSubstitute_Complex_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n); 64 | void Calculate_Eigenvectors(double* H, double* S, double eigen_real[], double eigen_imag[], int n); 65 | } 66 | 67 | #endif -------------------------------------------------------------------------------- /src/sensors/mpu9250sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #ifndef SENSORS_MPU9250SENSOR_H 25 | #define SENSORS_MPU9250SENSOR_H 26 | 27 | #include "sensor.h" 28 | #include "logging/Logger.h" 29 | 30 | #include 31 | 32 | #define MPU9250_DEFAULT_ODR_HZ 8000.0f 33 | #define MPU9250_SAMPLE_DIV 39 34 | constexpr float MPU9250_ODR_TS = ( 1.0f / MPU9250_DEFAULT_ODR_HZ) * (1+MPU9250_SAMPLE_DIV); 35 | 36 | #define MPU_USE_DMPMAG 1 37 | 38 | #if MPU_USE_DMPMAG 39 | #include "SensorFusionDMP.h" 40 | #else 41 | #include "SensorFusion.h" 42 | #endif 43 | 44 | class MPU9250Sensor : public Sensor 45 | { 46 | public: 47 | MPU9250Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) 48 | : Sensor("MPU9250Sensor", IMU_MPU9250, id, address, rotation, sclPin, sdaPin) 49 | #if !MPU_USE_DMPMAG 50 | , sfusion(MPU9250_ODR_TS) 51 | #endif 52 | {}; 53 | ~MPU9250Sensor(){}; 54 | void motionSetup() override final; 55 | void motionLoop() override final; 56 | void startCalibration(int calibrationType) override final; 57 | void getMPUScaled(); 58 | 59 | private: 60 | MPU9250 imu{}; 61 | bool dmpReady = false; // set true if DMP init was successful 62 | // TODO: actually check interrupt status 63 | // uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 64 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 65 | 66 | #if MPU_USE_DMPMAG 67 | SlimeVR::Sensors::SensorFusionDMP sfusion; 68 | #else 69 | SlimeVR::Sensors::SensorFusion sfusion; 70 | #endif 71 | 72 | // raw data and scaled as vector 73 | float Axyz[3]{}; 74 | float Gxyz[3]{}; 75 | float Mxyz[3]{}; 76 | VectorInt16 rawAccel{}; 77 | Quat correction{0, 0, 0, 0}; 78 | 79 | SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration = {}; 80 | 81 | // outputs to respective member variables 82 | void parseAccelData(int16_t data[3]); 83 | void parseGyroData(int16_t data[3]); 84 | void parseMagData(int16_t data[3]); 85 | 86 | // 6 bytes for gyro, 6 bytes for accel, 7 bytes for magnetometer 87 | static constexpr uint16_t sensor_data_len = 19; 88 | 89 | struct fifo_sample { 90 | int16_t accel[3]; 91 | int16_t gyro[3]; 92 | int16_t mag[3]; 93 | uint8_t mag_status; 94 | }; 95 | 96 | // acts as a memory space for getNextSample. upon success, can read from the sample member 97 | // TODO: this may be overcomplicated, we may be able to just use fifo_sample and i misunderstood strict aliasing rules. 98 | union fifo_sample_raw { 99 | uint8_t raw[sensor_data_len]; 100 | struct fifo_sample sample; 101 | }; 102 | 103 | // returns true if sample was read, outputs number of waiting samples in remaining_count if not null. 104 | bool getNextSample(union fifo_sample_raw *buffer, uint16_t *remaining_count); 105 | static void swapFifoData(union fifo_sample_raw* sample); 106 | }; 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /src/consts.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_CONSTS_H_ 24 | #define SLIMEVR_CONSTS_H_ 25 | 26 | // List of constants used in other places 27 | #define IMU_UNKNOWN 0 28 | #define IMU_MPU9250 1 29 | #define IMU_MPU6500 2 30 | #define IMU_BNO080 3 31 | #define IMU_BNO085 4 32 | #define IMU_BNO055 5 33 | #define IMU_MPU6050 6 34 | #define IMU_BNO086 7 35 | #define IMU_BMI160 8 36 | #define IMU_ICM20948 9 37 | #define IMU_ICM42688 10 38 | #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware 39 | 40 | #define BOARD_UNKNOWN 0 41 | #define BOARD_SLIMEVR_LEGACY 1 42 | #define BOARD_SLIMEVR_DEV 2 43 | #define BOARD_NODEMCU 3 44 | #define BOARD_CUSTOM 4 45 | #define BOARD_WROOM32 5 46 | #define BOARD_WEMOSD1MINI 6 47 | #define BOARD_TTGO_TBASE 7 48 | #define BOARD_ESP01 8 49 | #define BOARD_SLIMEVR 9 50 | #define BOARD_LOLIN_C3_MINI 10 51 | #define BOARD_BEETLE32C3 11 52 | #define BOARD_ES32C3DEVKITM1 12 53 | #define BOARD_OWOTRACK 13 // Only used by owoTrack mobile app 54 | #define BOARD_WRANGLER 14 // Only used by wrangler app 55 | #define BOARD_MOCOPI 15 // Used by mocopi/moslime 56 | #define BOARD_WEMOSWROOM02 16 57 | #define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware 58 | 59 | #define BAT_EXTERNAL 1 60 | #define BAT_INTERNAL 2 61 | #define BAT_MCP3021 3 62 | #define BAT_INTERNAL_MCP3021 4 63 | 64 | #define LED_OFF 255 65 | 66 | #define POWER_SAVING_LEGACY 0 // No sleeping, but PS enabled 67 | #define POWER_SAVING_NONE 1 // No sleeping, no PS => for connection issues 68 | #define POWER_SAVING_MINIMUM 2 // Sleeping and PS => default 69 | #define POWER_SAVING_MODERATE 3 // Sleeping and better PS => might miss broadcasts, use at own risk 70 | #define POWER_SAVING_MAXIMUM 4 // Actual CPU sleeping, currently has issues with disconnecting 71 | 72 | // Send rotation/acceleration data as separate frames. 73 | // PPS: 1470 @ 5+1, 1960 @ 5+3 74 | #define PACKET_BUNDLING_DISABLED 0 75 | // Less packets. Pack data per sensor and send asap. 76 | // Compared to PACKET_BUNDLING_DISABLED, reduces PPS by ~54% for 5+1, by ~63% for 5+3 setups. 77 | // PPS: 680 @ 5+1, 740 @ 5+3 78 | #define PACKET_BUNDLING_LOWLATENCY 1 79 | // Even less packets, if more than 1 sensor - wait for data from all sensors or until timeout, then send. 80 | // Compared to PACKET_BUNDLING_LOWLATENCY, reduces PPS by ~5% for 5+1, by ~15% for 5+3 setups. 81 | // PPS: 650 @ 5+1, 650 @ 5+3 82 | #define PACKET_BUNDLING_BUFFERED 2 83 | 84 | #define DEG_0 0.f 85 | #define DEG_90 -PI / 2 86 | #define DEG_180 PI 87 | #define DEG_270 PI / 2 88 | 89 | #define CONST_EARTH_GRAVITY 9.80665 90 | 91 | #define ACCEL_CALIBRATION_METHOD_SKIP 0 92 | #define ACCEL_CALIBRATION_METHOD_ROTATION 1 93 | #define ACCEL_CALIBRATION_METHOD_6POINT 2 94 | 95 | #define BMI160_MAG_TYPE_HMC 1 96 | #define BMI160_MAG_TYPE_QMC 2 97 | 98 | #define MCU_UKNOWN 0 99 | #define MCU_ESP8266 1 100 | #define MCU_ESP32 2 101 | #define MCU_OWOTRACK_ANDROID 3 // Only used by owoTrack mobile app 102 | #define MCU_WRANGLER 4 // Only used by wrangler app 103 | #define MCU_OWOTRACK_IOS 5 // Only used by owoTrack mobile app 104 | #define MCU_ESP32_C3 6 105 | #define MCU_MOCOPI 7 // Used by mocopi/moslime 106 | #define MCU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware 107 | 108 | #ifdef ESP8266 109 | #define HARDWARE_MCU MCU_ESP8266 110 | #elif defined(ESP32) 111 | #define HARDWARE_MCU MCU_ESP32 112 | #else 113 | #define HARDWARE_MCU MCU_UKNOWN 114 | #endif 115 | 116 | #define CURRENT_CONFIGURATION_VERSION 1 117 | 118 | #endif // SLIMEVR_CONSTS_H_ 119 | -------------------------------------------------------------------------------- /src/motionprocessing/OnlinePolyfit.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #include 24 | 25 | #ifndef ONLINE_POLYFIT_H 26 | #define ONLINE_POLYFIT_H 27 | 28 | template 29 | class OnlineVectorPolyfit { 30 | public: 31 | constexpr static int32_t numDimensions = dimensions; 32 | constexpr static int32_t numCoefficients = degree + 1; 33 | constexpr static double forgettingFactor = std::exp(-1.0/forgettingFactorNumSamples); 34 | 35 | OnlineVectorPolyfit() { 36 | reset(); 37 | } 38 | 39 | void reset() { 40 | std::fill(Rb[0], Rb[0] + rows * cols, 0.0); 41 | std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f); 42 | } 43 | 44 | // Recursive least squares update using QR decomposition by Givens transformations 45 | void update(double xValue, const double yValues[numDimensions]) { 46 | double xin[cols]; 47 | xin[0] = 1; 48 | for (int32_t i = 1; i < cols - numDimensions; i++) { 49 | xin[i] = xin[i - 1] * xValue; 50 | } 51 | for (int32_t i = 0; i < numDimensions; i++) { 52 | xin[cols - numDimensions + i] = yValues[i]; 53 | } 54 | 55 | // degree = 3, dimensions = 3, yValues = [x, y, z] 56 | // B I I I Ix Iy Iz R R R R bx by bz 57 | // . B I I Ix Iy Iz === . R R R bx by bz 58 | // . . B I Ix Iy Iz === . . R R bx by bz 59 | // . . . B Ix Iy Iz . . . R bx by bz 60 | 61 | // https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf 62 | 63 | for (int32_t y = 0; y < rows; y++) { 64 | double c = 1, s = 0; 65 | if (xin[y] != 0.0) { 66 | Rb[y][y] *= forgettingFactor; 67 | const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]); 68 | c = Rb[y][y] * (1.0 / norm); 69 | s = xin[y] * (1.0 / norm); 70 | Rb[y][y] = norm; 71 | } 72 | for (int32_t x = y + 1; x < cols; x++) { 73 | Rb[y][x] *= forgettingFactor; 74 | const double xout = (c * xin[x] - s * Rb[y][x]); 75 | Rb[y][x] = (s * xin[x] + c * Rb[y][x]); 76 | xin[x] = xout; 77 | } 78 | } 79 | } 80 | 81 | // Back solves upper triangular system 82 | // Returns float[numDimensions][numCoefficients] coefficients from lowest to highest power 83 | const auto computeCoefficients() { 84 | // https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution 85 | for (int32_t d = 0; d < numDimensions; d++) { 86 | for (int32_t y = rows - 1; y >= 0; y--) { 87 | const int32_t bColumn = cols - numDimensions + d; 88 | coeffs[d][y] = Rb[y][bColumn]; 89 | if (Rb[y][y] == 0.0) continue; 90 | for (int32_t x = y + 1; x < rows; x++) { 91 | coeffs[d][y] -= coeffs[d][x] * Rb[y][x]; 92 | } 93 | coeffs[d][y] /= Rb[y][y]; 94 | } 95 | } 96 | return coeffs; 97 | } 98 | 99 | float predict(int32_t d, float x) { 100 | if (d >= numDimensions) return 0.0; 101 | // https://en.wikipedia.org/wiki/Horner%27s_method 102 | float y = coeffs[d][numCoefficients - 1]; 103 | for (int32_t i = numCoefficients - 2; i >= 0; i--) { 104 | y = y * x + coeffs[d][i]; 105 | } 106 | return y; 107 | } 108 | 109 | std::pair tangentAt(float x) { 110 | float intercept = coeffs[0]; 111 | float slope = coeffs[1]; 112 | for (uint32_t i = 2; i < degree + 1; i++) { 113 | intercept -= coeffs[i] * (i - 1) * pow(x, i); 114 | slope += coeffs[i] * i * pow(x, i - 1); 115 | } 116 | return std::make_pair(slope, intercept); 117 | } 118 | private: 119 | constexpr static int32_t rows = numCoefficients; 120 | constexpr static int32_t cols = numCoefficients + 3; 121 | double Rb[rows][cols]; 122 | float coeffs[numDimensions][rows]; 123 | }; 124 | 125 | #endif -------------------------------------------------------------------------------- /src/sensors/SensorFusion.h: -------------------------------------------------------------------------------- 1 | #ifndef SLIMEVR_SENSORFUSION_H 2 | #define SLIMEVR_SENSORFUSION_H 3 | 4 | #include "globals.h" 5 | #include "sensor.h" 6 | 7 | #define SENSOR_DOUBLE_PRECISION 0 8 | 9 | #define SENSOR_FUSION_TYPE SENSOR_FUSION_VQF 10 | 11 | #define SENSOR_FUSION_MAHONY 1 12 | #define SENSOR_FUSION_MADGWICK 2 13 | #define SENSOR_FUSION_BASICVQF 3 14 | #define SENSOR_FUSION_VQF 4 15 | 16 | #if SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY 17 | #define SENSOR_FUSION_TYPE_STRING "mahony" 18 | #elif SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK 19 | #define SENSOR_FUSION_TYPE_STRING "madgwick" 20 | #elif SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF 21 | #define SENSOR_FUSION_TYPE_STRING "bvqf" 22 | #elif SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF 23 | #define SENSOR_FUSION_TYPE_STRING "vqf" 24 | #endif 25 | 26 | #define SENSOR_USE_MAHONY (SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY) 27 | #define SENSOR_USE_MADGWICK (SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK) 28 | #define SENSOR_USE_BASICVQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF) 29 | #define SENSOR_USE_VQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF) 30 | 31 | 32 | #include "../motionprocessing/types.h" 33 | 34 | #include "mahony.h" 35 | #include "madgwick.h" 36 | #include 37 | #include 38 | 39 | namespace SlimeVR 40 | { 41 | namespace Sensors 42 | { 43 | #if SENSOR_USE_VQF 44 | struct SensorVQFParams: VQFParams { 45 | SensorVQFParams() : VQFParams() { 46 | #ifndef VQF_NO_MOTION_BIAS_ESTIMATION 47 | motionBiasEstEnabled = false; 48 | #endif 49 | tauAcc = 2.0f; 50 | restMinT = 2.0f; 51 | restThGyr = 0.6f; // 400 norm 52 | restThAcc = 0.06f; // 100 norm 53 | } 54 | }; 55 | #endif 56 | 57 | class SensorFusion 58 | { 59 | public: 60 | SensorFusion(sensor_real_t gyrTs, sensor_real_t accTs=-1.0, sensor_real_t magTs=-1.0) 61 | : gyrTs(gyrTs), 62 | accTs( (accTs<0) ? gyrTs : accTs ), 63 | magTs( (magTs<0) ? gyrTs : magTs ) 64 | #if SENSOR_USE_MAHONY 65 | #elif SENSOR_USE_MADGWICK 66 | #elif SENSOR_USE_BASICVQF 67 | , basicvqf(gyrTs, ((accTs<0) ? gyrTs : accTs), 68 | ((magTs<0) ? gyrTs : magTs)) 69 | #elif SENSOR_USE_VQF 70 | , vqf(vqfParams, gyrTs, ((accTs<0) ? gyrTs : accTs), 71 | ((magTs<0) ? gyrTs : magTs)) 72 | #endif 73 | {} 74 | 75 | void update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f); 76 | void update9D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f); 77 | void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat=-1.0f); 78 | void updateMag(sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f); 79 | void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f); 80 | 81 | bool isUpdated(); 82 | void clearUpdated(); 83 | sensor_real_t const * getQuaternion(); 84 | Quat getQuaternionQuat(); 85 | sensor_real_t const * getGravityVec(); 86 | sensor_real_t const * getLinearAcc(); 87 | void getLinearAcc(sensor_real_t outLinAccel[3]); 88 | Vector3 getLinearAccVec(); 89 | 90 | static void calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3]); 91 | static void calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3]); 92 | 93 | protected: 94 | sensor_real_t gyrTs; 95 | sensor_real_t accTs; 96 | sensor_real_t magTs; 97 | 98 | #if SENSOR_USE_MAHONY 99 | Mahony mahony; 100 | #elif SENSOR_USE_MADGWICK 101 | Madgwick madgwick; 102 | #elif SENSOR_USE_BASICVQF 103 | BasicVQF basicvqf; 104 | #elif SENSOR_USE_VQF 105 | SensorVQFParams vqfParams {}; 106 | VQF vqf; 107 | #endif 108 | 109 | // A also used for linear acceleration extraction 110 | sensor_real_t bAxyz[3]{0.0f, 0.0f, 0.0f}; 111 | 112 | #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK 113 | // Buffer M here to keep the behavior of BMI160 114 | sensor_real_t bMxyz[3]{0.0f, 0.0f, 0.0f}; 115 | bool accelUpdated = false; 116 | #endif 117 | 118 | bool magExist = false; 119 | sensor_real_t qwxyz[4]{1.0f, 0.0f, 0.0f, 0.0f}; 120 | bool updated = false; 121 | 122 | bool gravityReady = false; 123 | sensor_real_t vecGravity[3]{0.0f, 0.0f, 0.0f}; 124 | bool linaccelReady = false; 125 | sensor_real_t linAccel[3]{0.0f, 0.0f, 0.0f}; 126 | #if ESP32 127 | sensor_real_t linAccel_guard; // Temporary patch for some weird ESP32 bug 128 | #endif 129 | }; 130 | } 131 | } 132 | 133 | #endif // SLIMEVR_SENSORFUSION_H 134 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "Wire.h" 25 | #include "ota.h" 26 | #include "GlobalVars.h" 27 | #include "globals.h" 28 | #include "credentials.h" 29 | #include 30 | #include "serial/serialcommands.h" 31 | #include "batterymonitor.h" 32 | #include "logging/Logger.h" 33 | 34 | Timer<> globalTimer; 35 | SlimeVR::Logging::Logger logger("SlimeVR"); 36 | SlimeVR::Sensors::SensorManager sensorManager; 37 | SlimeVR::LEDManager ledManager(LED_PIN); 38 | SlimeVR::Status::StatusManager statusManager; 39 | SlimeVR::Configuration::Configuration configuration; 40 | SlimeVR::Network::Manager networkManager; 41 | SlimeVR::Network::Connection networkConnection; 42 | 43 | int sensorToCalibrate = -1; 44 | bool blinking = false; 45 | unsigned long blinkStart = 0; 46 | unsigned long loopTime = 0; 47 | unsigned long lastStatePrint = 0; 48 | bool secondImuActive = false; 49 | BatteryMonitor battery; 50 | 51 | void setup() 52 | { 53 | Serial.begin(serialBaudRate); 54 | globalTimer = timer_create_default(); 55 | 56 | #ifdef ESP32C3 57 | // Wait for the Computer to be able to connect. 58 | delay(2000); 59 | #endif 60 | 61 | Serial.println(); 62 | Serial.println(); 63 | Serial.println(); 64 | 65 | logger.info("SlimeVR v" FIRMWARE_VERSION " starting up..."); 66 | 67 | statusManager.setStatus(SlimeVR::Status::LOADING, true); 68 | 69 | ledManager.setup(); 70 | configuration.setup(); 71 | 72 | SerialCommands::setUp(); 73 | 74 | #if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 || IMU == IMU_BMI160|| IMU == IMU_ICM42688 75 | I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down 76 | // Fixes I2C issues for certain IMUs. Only has been tested on IMUs above. Testing advised when adding other IMUs. 77 | #endif 78 | // join I2C bus 79 | 80 | #if ESP32 81 | // 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.) 82 | Wire.end(); 83 | #endif 84 | 85 | // using `static_cast` here seems to be better, because there are 2 similar function signatures 86 | Wire.begin(static_cast(PIN_IMU_SDA), static_cast(PIN_IMU_SCL)); 87 | 88 | #ifdef ESP8266 89 | Wire.setClockStretchLimit(150000L); // Default stretch limit 150mS 90 | #endif 91 | #ifdef ESP32 // Counterpart on ESP32 to ClockStretchLimit 92 | Wire.setTimeOut(150); 93 | #endif 94 | Wire.setClock(I2C_SPEED); 95 | 96 | // Wait for IMU to boot 97 | delay(500); 98 | 99 | sensorManager.setup(); 100 | 101 | networkManager.setup(); 102 | OTA::otaSetup(otaPassword); 103 | battery.Setup(); 104 | 105 | statusManager.setStatus(SlimeVR::Status::LOADING, false); 106 | 107 | sensorManager.postSetup(); 108 | 109 | loopTime = micros(); 110 | } 111 | 112 | void loop() 113 | { 114 | globalTimer.tick(); 115 | SerialCommands::update(); 116 | OTA::otaUpdate(); 117 | networkManager.update(); 118 | sensorManager.update(); 119 | battery.Loop(); 120 | ledManager.update(); 121 | #ifdef TARGET_LOOPTIME_MICROS 122 | long elapsed = (micros() - loopTime); 123 | if (elapsed < TARGET_LOOPTIME_MICROS) 124 | { 125 | long sleepus = TARGET_LOOPTIME_MICROS - elapsed - 100;//µs to sleep 126 | long sleepms = sleepus / 1000;//ms to sleep 127 | if(sleepms > 0) // if >= 1 ms 128 | { 129 | delay(sleepms); // sleep ms = save power 130 | sleepus -= sleepms * 1000; 131 | } 132 | if (sleepus > 100) 133 | { 134 | delayMicroseconds(sleepus); 135 | } 136 | } 137 | loopTime = micros(); 138 | #endif 139 | #if defined(PRINT_STATE_EVERY_MS) && PRINT_STATE_EVERY_MS > 0 140 | unsigned long now = millis(); 141 | if(lastStatePrint + PRINT_STATE_EVERY_MS < now) { 142 | lastStatePrint = now; 143 | SerialCommands::printState(); 144 | } 145 | #endif 146 | } 147 | -------------------------------------------------------------------------------- /src/network/connection.h: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2023 SlimeVR Contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #ifndef SLIMEVR_NETWORK_CONNECTION_H_ 24 | #define SLIMEVR_NETWORK_CONNECTION_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include "globals.h" 30 | #include "quat.h" 31 | #include "sensors/sensor.h" 32 | #include "wifihandler.h" 33 | #include "featureflags.h" 34 | 35 | namespace SlimeVR { 36 | namespace Network { 37 | 38 | class Connection { 39 | public: 40 | void searchForServer(); 41 | void update(); 42 | void reset(); 43 | bool isConnected() const { return m_Connected; } 44 | 45 | // PACKET_ACCEL 4 46 | void sendSensorAcceleration(uint8_t sensorId, Vector3 vector); 47 | 48 | // PACKET_BATTERY_LEVEL 12 49 | void sendBatteryLevel(float batteryVoltage, float batteryPercentage); 50 | 51 | // PACKET_TAP 13 52 | void sendSensorTap(uint8_t sensorId, uint8_t value); 53 | 54 | // PACKET_ERROR 14 55 | void sendSensorError(uint8_t sensorId, uint8_t error); 56 | 57 | // PACKET_ROTATION_DATA 17 58 | void sendRotationData( 59 | uint8_t sensorId, 60 | Quat* const quaternion, 61 | uint8_t dataType, 62 | uint8_t accuracyInfo 63 | ); 64 | 65 | // PACKET_MAGNETOMETER_ACCURACY 18 66 | void sendMagnetometerAccuracy(uint8_t sensorId, float accuracyInfo); 67 | 68 | // PACKET_SIGNAL_STRENGTH 19 69 | void sendSignalStrength(uint8_t signalStrength); 70 | 71 | // PACKET_TEMPERATURE 20 72 | void sendTemperature(uint8_t sensorId, float temperature); 73 | 74 | // PACKET_FEATURE_FLAGS 22 75 | void sendFeatureFlags(); 76 | 77 | #if ENABLE_INSPECTION 78 | void sendInspectionRawIMUData( 79 | uint8_t sensorId, 80 | int16_t rX, 81 | int16_t rY, 82 | int16_t rZ, 83 | uint8_t rA, 84 | int16_t aX, 85 | int16_t aY, 86 | int16_t aZ, 87 | uint8_t aA, 88 | int16_t mX, 89 | int16_t mY, 90 | int16_t mZ, 91 | uint8_t mA 92 | ); 93 | void sendInspectionRawIMUData( 94 | uint8_t sensorId, 95 | float rX, 96 | float rY, 97 | float rZ, 98 | uint8_t rA, 99 | float aX, 100 | float aY, 101 | float aZ, 102 | uint8_t aA, 103 | float mX, 104 | float mY, 105 | float mZ, 106 | uint8_t mA 107 | ); 108 | #endif 109 | 110 | const ServerFeatures& getServerFeatureFlags() { 111 | return m_ServerFeatures; 112 | } 113 | 114 | bool beginBundle(); 115 | bool endBundle(); 116 | 117 | private: 118 | void updateSensorState(std::vector & sensors); 119 | void maybeRequestFeatureFlags(); 120 | 121 | bool beginPacket(); 122 | bool endPacket(); 123 | 124 | size_t write(const uint8_t *buffer, size_t size); 125 | size_t write(uint8_t byte); 126 | 127 | bool sendPacketType(uint8_t type); 128 | bool sendPacketNumber(); 129 | bool sendFloat(float f); 130 | bool sendByte(uint8_t c); 131 | bool sendShort(uint16_t i); 132 | bool sendInt(uint32_t i); 133 | bool sendLong(uint64_t l); 134 | bool sendBytes(const uint8_t* c, size_t length); 135 | bool sendShortString(const char* str); 136 | bool sendLongString(const char* str); 137 | 138 | int getWriteError(); 139 | 140 | void returnLastPacket(int len); 141 | 142 | // PACKET_HEARTBEAT 0 143 | void sendHeartbeat(); 144 | 145 | // PACKET_HANDSHAKE 3 146 | void sendTrackerDiscovery(); 147 | 148 | // PACKET_SENSOR_INFO 15 149 | void sendSensorInfo(Sensor* sensor); 150 | 151 | bool m_Connected = false; 152 | SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection"); 153 | 154 | WiFiUDP m_UDP; 155 | unsigned char m_Packet[128]; // buffer for incoming packets 156 | uint64_t m_PacketNumber = 0; 157 | 158 | int m_ServerPort = 6969; 159 | IPAddress m_ServerHost = IPAddress(255, 255, 255, 255); 160 | unsigned long m_LastConnectionAttemptTimestamp; 161 | unsigned long m_LastPacketTimestamp; 162 | 163 | SensorStatus m_AckedSensorState[MAX_IMU_COUNT] = {SensorStatus::SENSOR_OFFLINE}; 164 | unsigned long m_LastSensorInfoPacketTimestamp = 0; 165 | 166 | uint8_t m_FeatureFlagsRequestAttempts = 0; 167 | unsigned long m_FeatureFlagsRequestTimestamp = millis(); 168 | ServerFeatures m_ServerFeatures{}; 169 | 170 | bool m_IsBundle = false; 171 | uint16_t m_BundlePacketPosition = 0; 172 | uint16_t m_BundlePacketInnerCount = 0; 173 | 174 | unsigned char m_Buf[8]; 175 | }; 176 | 177 | } // namespace Network 178 | } // namespace SlimeVR 179 | 180 | #endif // SLIMEVR_NETWORK_CONNECTION_H_ 181 | -------------------------------------------------------------------------------- /src/batterymonitor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2021 Eiren Rain & SlimeVR contributors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | #include "batterymonitor.h" 24 | #include "GlobalVars.h" 25 | 26 | #if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021) 27 | ADC_MODE(ADC_VCC); 28 | #endif 29 | 30 | void BatteryMonitor::Setup() 31 | { 32 | #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 33 | for (uint8_t i = 0x48; i < 0x4F; i++) 34 | { 35 | if (I2CSCAN::hasDevOnBus(i)) 36 | { 37 | address = i; 38 | break; 39 | } 40 | } 41 | if (address == 0) 42 | { 43 | m_Logger.error("MCP3021 not found on I2C bus"); 44 | } 45 | #endif 46 | } 47 | 48 | void BatteryMonitor::Loop() 49 | { 50 | #if BATTERY_MONITOR == BAT_EXTERNAL || BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 51 | auto now_ms = millis(); 52 | if (now_ms - last_battery_sample >= batterySampleRate) 53 | { 54 | last_battery_sample = now_ms; 55 | voltage = -1; 56 | #if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021) 57 | // Find out what your max measurement is (voltage_3_3). 58 | // Take the max measurement and check if it was less than 50mV 59 | // if yes output 5.0V 60 | // if no output 3.3V - dropvoltage + 0.1V 61 | auto ESPmV = ESP.getVcc(); 62 | if (ESPmV > voltage_3_3) 63 | { 64 | voltage_3_3 = ESPmV; 65 | } 66 | else 67 | { 68 | //Calculate drop in mV 69 | ESPmV = voltage_3_3 - ESPmV; 70 | if (ESPmV < 50) 71 | { 72 | voltage = 5.0F; 73 | } 74 | else 75 | { 76 | voltage = 3.3F - ((float)ESPmV / 1000.0F) + 0.1F; //we assume 100mV drop on the linear converter 77 | } 78 | } 79 | #endif 80 | #if BATTERY_MONITOR == BAT_EXTERNAL 81 | voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * batteryADCMultiplier; 82 | #endif 83 | #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 84 | if (address > 0) 85 | { 86 | Wire.beginTransmission(address); 87 | Wire.requestFrom(address, (uint8_t)2); 88 | auto MSB = Wire.read(); 89 | auto LSB = Wire.read(); 90 | auto status = Wire.endTransmission(); 91 | if (status == 0) 92 | { 93 | float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2)); 94 | v *= batteryADCMultiplier; 95 | voltage = (voltage > 0) ? min(voltage, v) : v; 96 | } 97 | } 98 | #endif 99 | if (voltage > 0) //valid measurement 100 | { 101 | // Estimate battery level, 3.2V is 0%, 4.17V is 100% (1.0) 102 | if (voltage > 3.975f) 103 | level = (voltage - 2.920f) * 0.8f; 104 | else if (voltage > 3.678f) 105 | level = (voltage - 3.300f) * 1.25f; 106 | else if (voltage > 3.489f) 107 | level = (voltage - 3.400f) * 1.7f; 108 | else if (voltage > 3.360f) 109 | level = (voltage - 3.300f) * 0.8f; 110 | else 111 | level = (voltage - 3.200f) * 0.3f; 112 | 113 | level = (level - 0.05f) / 0.95f; // Cut off the last 5% (3.36V) 114 | 115 | if (level > 1) 116 | level = 1; 117 | else if (level < 0) 118 | level = 0; 119 | networkConnection.sendBatteryLevel(voltage, level); 120 | #ifdef BATTERY_LOW_POWER_VOLTAGE 121 | if (voltage < BATTERY_LOW_POWER_VOLTAGE) 122 | { 123 | #if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP 124 | ESP.deepSleep(0); 125 | #else 126 | statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true); 127 | #endif 128 | } else { 129 | statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false); 130 | } 131 | #endif 132 | } 133 | } 134 | #endif 135 | } 136 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | Thanks for helping with SlimeVR! This document explains what to know when 4 | contributing. 5 | 6 | We are most active on [Discord](https://discord.gg/SlimeVR), come chat with us if you 7 | have any questions or want to hang out! 😎 8 | 9 | ## Setting up the development environment 10 | 11 | We use PlatformIO as our build tooling and rely on Git for version control. There are 12 | instructions to set all of this up [here](https://docs.slimevr.dev/firmware/setup-and-install.html). 13 | 14 | ## Using third-party code 15 | 16 | Often you will want to use or reference third-party code. You should not just 17 | copy-paste code from random libraries! There are several reasons for this: 18 | 19 | - It is not clear when/where the code originated from. 20 | - The original code's license needs to be kept intact. 21 | - It removes the ability to update our code when the original updates. 22 | - It makes it harder for us to maintain the code, because we don't know what has changed 23 | from the original! 24 | 25 | Instead, we will want to do the following: 26 | 1. Check that the dependency has a permissive license. 27 | 1. (Optional) Fork the dependency if it needs any modification. 28 | 1. Add the dependency via PlatformIO. 29 | 30 | ### Check the license 31 | 32 | The SlimeVR firmware is under the MIT License and also is intended to be permissively 33 | licensed. This means that we will not accept contributions that include code from any 34 | viral software licenses. If you do not know what that means, here is a breakdown of some 35 | common software licenses: 36 | 37 | > Fine to use: 38 | > * MIT 39 | > * Apache 2.0 40 | > * BSD 41 | > 42 | > Will not be accepted: 43 | > * GPLv3 or v2 44 | > * LGPL 45 | > * No license 46 | 47 | This includes the license of any code that your dependency depends on! If you are ever 48 | unsure, feel free to ask us in our Discord. 49 | 50 | ### (Optional) Fork the dependency 51 | 52 | When you want to use the depdendency, you should ask yourself: "Can I simply call the 53 | functions/classes in the dependency from *my* code, or do I actually need to modify the 54 | source code of the original files"? If the answer is "I need to modify the original 55 | files", then you will need to fork the dependency so that your modifications are clear. 56 | 57 | > **Note** 58 | > If you have never forked on GitHub before, a fork is basically a copy of a library that 59 | > maintains the original's history. This means that any changes you commit in your fork 60 | > will show up as changes against the original. We can even update these changes as the 61 | > original changes! 62 | 63 | To create a fork in GitHub, simply go to the original library and click this button: 64 | ![](https://docs.github.com/assets/cb-28613/images/help/repository/fork_button.png) 65 | Now clone your repo to your computer. From this point forward, any changes that you 66 | make while working on your fork will have their Git history preserved and linked to the 67 | original when you commit those changes. 68 | 69 | We will likely ask you to transfer ownership of the fork to the SlimeVR organization if 70 | your PR is going to be merged. 71 | 72 | 73 | ### Adding the dependency 74 | 75 | PlatformIO provides us a helpful way to do dependency management. 76 | Here we provide the basic workflow you can follow, but you can refer to the 77 | [PlatformIO documentation on dependecies][dep docs] for more details. 78 | 79 | [dep docs]: https://docs.platformio.org/en/latest/librarymanager/dependencies.html 80 | 81 | PlatformIO gives us a helpful way to do dependency management, which you can read an 82 | overview of [here](https://docs.platformio.org/en/latest/librarymanager/dependencies.html). 83 | But to save you time, we have outlined the basic workflow for you. 84 | 85 | There is a field called `lib_deps` in the `platformio.ini` file that describes the list 86 | of dependencies in the code. These are third-party libraries located outside the main 87 | firmware git repo. You want to add your dependency to this list. The full list of 88 | options for this field is outlined [here], but for simplicity, there are a few common 89 | ways of giving dependencies: 90 | 91 | [here]: https://docs.platformio.org/en/latest/core/userguide/pkg/cmd_install.html#cmd-pkg-install-specifications 92 | 93 | ```ini 94 | [env] 95 | lib_deps= 96 | bblanchon/ArduinoJson@6.9.12 97 | https://github.com/Some/Dependency.git#v1.2.3 98 | symlink://C:/path/to/the/library 99 | ``` 100 | 101 | In order, these dependencies do the following: 102 | 1. Adds a dependency on version `6.9.12` of the 103 | [ArduinoJson](https://registry.platformio.org/libraries/bblanchon/ArduinoJson) 104 | library by `bblanchon`. This library was uploaded to the PlatformIO 105 | [package registry](https://registry.platformio.org/search) which contains many popular 106 | libraries intended for use with PlatformIO. 107 | 1. Adds a dependency to a Git repo located at `https://github.com/Some/Dependency` and 108 | uses the commit tagged as `v1.2.3`. This could also have been a branch name, or a 109 | commit hash. 110 | 1. Links to a folder locally on your computer with the path `C:/path/to/the/library`. 111 | This is useful when testing your code locally where you want to make changes in your 112 | fork (which is checked out at that path), but you don't want to have to constantly 113 | commit and push to your fork just to get the changes usable by the firmware. 114 | 115 | > **Note** 116 | > If you have forked the dependency instead of using it as-is, we recommend using the 117 | > `symlink://` approach while developing locally, and then using the Git approach when 118 | > submitting your code for a pull request. This is because while developing locally, 119 | > you will want to test out frequent changes and won't want to have to push constantly 120 | > to Git and have PlatformIO constantly retrieve those changes. However, for anyone 121 | > else to be able to use your code, it needs to be in a Git repository rather than a symlink. 122 | > 123 | > Once you are done doing local testing and are ready to submit a PR, you can push 124 | > whatever changes you made to the forked dependency to its repository, and switch the 125 | > firmware's `lib_deps` back to the Git url. 126 | > 127 | > Dont forget that your fork will need to be pushed and publicly viewable on GitHub for 128 | > us to be able to use and review it! 129 | -------------------------------------------------------------------------------- /lib/ICM20948/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ICM_20948_I2C KEYWORD1 10 | ICM_20948_SPI KEYWORD1 11 | ICM_20948_Status_e KEYWORD1 12 | ICM_20948_InternalSensorID_bm KEYWORD1 13 | icm_20948_DMP_data_t KEYWORD1 14 | 15 | ####################################### 16 | # Methods and Functions (KEYWORD2) 17 | ####################################### 18 | 19 | ICM_20948 KEYWORD2 20 | enableDebugging KEYWORD2 21 | disableDebugging KEYWORD2 22 | debugPrintStatus KEYWORD2 23 | debugPrint KEYWORD2 24 | debugPrintln KEYWORD2 25 | doDebugPrint KEYWORD2 26 | debugPrintf KEYWORD2 27 | getAGMT KEYWORD2 28 | magX KEYWORD2 29 | magY KEYWORD2 30 | magZ KEYWORD2 31 | accX KEYWORD2 32 | accY KEYWORD2 33 | accZ KEYWORD2 34 | gyrX KEYWORD2 35 | gyrY KEYWORD2 36 | gyrZ KEYWORD2 37 | temp KEYWORD2 38 | statusString KEYWORD2 39 | setBank KEYWORD2 40 | swReset KEYWORD2 41 | sleep KEYWORD2 42 | lowPower KEYWORD2 43 | setClockSource KEYWORD2 44 | checkID KEYWORD2 45 | dataReady KEYWORD2 46 | getWhoAmI KEYWORD2 47 | isConnected KEYWORD2 48 | setSampleMode KEYWORD2 49 | setFullScale KEYWORD2 50 | setDLPFcfg KEYWORD2 51 | enableDLPF KEYWORD2 52 | setSampleRate KEYWORD2 53 | clearInterrupts 54 | cfgIntActiveLow KEYWORD2 55 | cfgIntOpenDrain KEYWORD2 56 | cfgIntLatch KEYWORD2 57 | cfgIntAnyReadToClear KEYWORD2 58 | cfgFsyncActiveLow KEYWORD2 59 | cfgFsyncIntMode KEYWORD2 60 | intEnableI2C KEYWORD2 61 | intEnableDMP KEYWORD2 62 | intEnablePLL KEYWORD2 63 | intEnableWOM KEYWORD2 64 | intEnableWOF KEYWORD2 65 | intEnableRawDataReady KEYWORD2 66 | intEnableOverflowFIFO KEYWORD2 67 | intEnableWatermarkFIFO KEYWORD2 68 | WOMThreshold KEYWORD2 69 | i2cMasterPassthrough KEYWORD2 70 | i2cMasterEnable KEYWORD2 71 | i2cControllerConfigurePeripheral KEYWORD2 72 | i2cControllerPeriph4Transaction KEYWORD2 73 | i2cMasterSingleW KEYWORD2 74 | i2cMasterSingleR KEYWORD2 75 | startupDefault KEYWORD2 76 | read KEYWORD2 77 | write KEYWORD2 78 | startupMagnetometer KEYWORD2 79 | magWhoIAm KEYWORD2 80 | readMag KEYWORD2 81 | writeMag KEYWORD2 82 | resetMag KEYWORD2 83 | enableFIFO KEYWORD2 84 | resetFIFO KEYWORD2 85 | setFIFOmode KEYWORD2 86 | getFIFOcount KEYWORD2 87 | readFIFO KEYWORD2 88 | enableDMP KEYWORD2 89 | resetDMP KEYWORD2 90 | loadDMPFirmware KEYWORD2 91 | setDMPstartAddress KEYWORD2 92 | enableDMPSensor KEYWORD2 93 | enableDMPSensorInt KEYWORD2 94 | writeDMPmems KEYWORD2 95 | readDMPmems KEYWORD2 96 | setDMPODRrate KEYWORD2 97 | readDMPdataFromFIFO KEYWORD2 98 | setGyroSF KEYWORD2 99 | initializeDMP KEYWORD2 100 | begin KEYWORD2 101 | 102 | ####################################### 103 | # Constants (LITERAL1) 104 | ####################################### 105 | 106 | _ICM_20948_H_ LITERAL1 107 | ICM_20948_ARD_UNUSED_PIN LITERAL1 108 | ICM_20948_SPI_DEFAULT_FREQ LITERAL1 109 | ICM_20948_SPI_DEFAULT_ORDER LITERAL1 110 | ICM_20948_SPI_DEFAULT_MODE LITERAL1 111 | ICM_20948_I2C_ADDR_AD0 LITERAL1 112 | ICM_20948_I2C_ADDR_AD1 LITERAL1 113 | ICM_20948_WHOAMI LITERAL1 114 | MAG_AK09916_I2C_ADDR LITERAL1 115 | MAG_AK09916_WHO_AM_I LITERAL1 116 | MAG_REG_WHO_AM_I LITERAL1 117 | ICM_20948_Stat_Ok LITERAL1 118 | ICM_20948_Stat_Err LITERAL1 119 | ICM_20948_Stat_NotImpl LITERAL1 120 | ICM_20948_Stat_ParamErr LITERAL1 121 | ICM_20948_Stat_WrongID LITERAL1 122 | ICM_20948_Stat_InvalSensor LITERAL1 123 | ICM_20948_Stat_NoData LITERAL1 124 | ICM_20948_Stat_SensorNotSupported LITERAL1 125 | ICM_20948_Stat_DMPNotSupported LITERAL1 126 | ICM_20948_Stat_DMPVerifyFail LITERAL1 127 | ICM_20948_Stat_FIFONoDataAvail LITERAL1 128 | ICM_20948_Stat_FIFOIncompleteData LITERAL1 129 | ICM_20948_Stat_FIFOMoreDataAvail LITERAL1 130 | ICM_20948_Stat_UnrecognisedDMPHeader LITERAL1 131 | ICM_20948_Stat_UnrecognisedDMPHeader2 LITERAL1 132 | ICM_20948_Stat_InvalDMPRegister LITERAL1 133 | ICM_20948_Stat_NUM LITERAL1 134 | ICM_20948_Stat_Unknown LITERAL1 135 | ICM_20948_Internal_Acc LITERAL1 136 | ICM_20948_Internal_Gyr LITERAL1 137 | ICM_20948_Internal_Mag LITERAL1 138 | ICM_20948_Internal_Tmp LITERAL1 139 | ICM_20948_Internal_Mst LITERAL1 140 | DMP_ODR_Reg_Accel LITERAL1 141 | DMP_ODR_Reg_Gyro LITERAL1 142 | DMP_ODR_Reg_Cpass LITERAL1 143 | DMP_ODR_Reg_ALS LITERAL1 144 | DMP_ODR_Reg_Quat6 LITERAL1 145 | DMP_ODR_Reg_Quat9 LITERAL1 146 | DMP_ODR_Reg_PQuat6 LITERAL1 147 | DMP_ODR_Reg_Geomag LITERAL1 148 | DMP_ODR_Reg_Pressure LITERAL1 149 | DMP_ODR_Reg_Gyro_Calibr LITERAL1 150 | DMP_ODR_Reg_Cpass_Calibr LITERAL1 151 | INV_ICM20948_SENSOR_ACCELEROMETER LITERAL1 152 | INV_ICM20948_SENSOR_GYROSCOPE LITERAL1 153 | INV_ICM20948_SENSOR_RAW_ACCELEROMETER LITERAL1 154 | INV_ICM20948_SENSOR_RAW_GYROSCOPE LITERAL1 155 | INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED LITERAL1 156 | INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED LITERAL1 157 | INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON LITERAL1 158 | INV_ICM20948_SENSOR_STEP_DETECTOR LITERAL1 159 | INV_ICM20948_SENSOR_STEP_COUNTER LITERAL1 160 | INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR LITERAL1 161 | INV_ICM20948_SENSOR_ROTATION_VECTOR LITERAL1 162 | INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR LITERAL1 163 | INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD LITERAL1 164 | INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION LITERAL1 165 | INV_ICM20948_SENSOR_FLIP_PICKUP LITERAL1 166 | INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR LITERAL1 167 | INV_ICM20948_SENSOR_GRAVITY LITERAL1 168 | INV_ICM20948_SENSOR_LINEAR_ACCELERATION LITERAL1 169 | INV_ICM20948_SENSOR_ORIENTATION LITERAL1 170 | INV_ICM20948_SENSOR_B2S LITERAL1 171 | DMP_header_bitmap_Header2 LITERAL1 172 | DMP_header_bitmap_Step_Detector LITERAL1 173 | DMP_header_bitmap_Compass_Calibr LITERAL1 174 | DMP_header_bitmap_Gyro_Calibr LITERAL1 175 | DMP_header_bitmap_Pressure LITERAL1 176 | DMP_header_bitmap_Geomag LITERAL1 177 | DMP_header_bitmap_PQuat6 LITERAL1 178 | DMP_header_bitmap_Quat9 LITERAL1 179 | DMP_header_bitmap_Quat6 LITERAL1 180 | DMP_header_bitmap_ALS LITERAL1 181 | DMP_header_bitmap_Compass LITERAL1 182 | DMP_header_bitmap_Gyro LITERAL1 183 | DMP_header_bitmap_Accel LITERAL1 184 | DMP_header2_bitmap_Secondary_On_Off LITERAL1 185 | DMP_header2_bitmap_Activity_Recog LITERAL1 186 | DMP_header2_bitmap_Pickup LITERAL1 187 | DMP_header2_bitmap_Fsync LITERAL1 188 | DMP_header2_bitmap_Compass_Accuracy LITERAL1 189 | DMP_header2_bitmap_Gyro_Accuracy LITERAL1 190 | DMP_header2_bitmap_Accel_Accuracy LITERAL1 191 | DMP_Data_ready_Gyro LITERAL1 192 | DMP_Data_ready_Accel LITERAL1 193 | DMP_Data_ready_Secondary_Compass LITERAL1 194 | -------------------------------------------------------------------------------- /src/LEDManager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SlimeVR Code is placed under the MIT license 3 | Copyright (c) 2022 TheDevMinerTV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "LEDManager.h" 25 | #include "GlobalVars.h" 26 | #include "status/Status.h" 27 | 28 | namespace SlimeVR 29 | { 30 | void LEDManager::setup() 31 | { 32 | #if ENABLE_LEDS 33 | pinMode(m_Pin, OUTPUT); 34 | #endif 35 | 36 | // Do the initial pull of the state 37 | update(); 38 | } 39 | 40 | void LEDManager::on() 41 | { 42 | #if ENABLE_LEDS 43 | digitalWrite(m_Pin, LED__ON); 44 | #endif 45 | } 46 | 47 | void LEDManager::off() 48 | { 49 | #if ENABLE_LEDS 50 | digitalWrite(m_Pin, LED__OFF); 51 | #endif 52 | } 53 | 54 | void LEDManager::blink(unsigned long time) 55 | { 56 | on(); 57 | delay(time); 58 | off(); 59 | } 60 | 61 | void LEDManager::pattern(unsigned long timeon, unsigned long timeoff, int times) 62 | { 63 | for (int i = 0; i < times; i++) 64 | { 65 | blink(timeon); 66 | delay(timeoff); 67 | } 68 | } 69 | 70 | void LEDManager::update() 71 | { 72 | unsigned long time = millis(); 73 | unsigned long diff = time - m_LastUpdate; 74 | 75 | // Don't tick the LEDManager *too* often 76 | if (diff < 10) 77 | { 78 | return; 79 | } 80 | 81 | m_LastUpdate = time; 82 | 83 | unsigned int length = 0; 84 | unsigned int count = 0; 85 | 86 | if (statusManager.hasStatus(Status::LOW_BATTERY)) 87 | { 88 | count = LOW_BATTERY_COUNT; 89 | switch (m_CurrentStage) 90 | { 91 | case ON: 92 | case OFF: 93 | length = LOW_BATTERY_LENGTH; 94 | break; 95 | case GAP: 96 | length = DEFAULT_GAP; 97 | break; 98 | case INTERVAL: 99 | length = LOW_BATTERY_INTERVAL; 100 | break; 101 | } 102 | } 103 | else if (statusManager.hasStatus(Status::IMU_ERROR)) 104 | { 105 | count = IMU_ERROR_COUNT; 106 | switch (m_CurrentStage) 107 | { 108 | case ON: 109 | case OFF: 110 | length = IMU_ERROR_LENGTH; 111 | break; 112 | case GAP: 113 | length = DEFAULT_GAP; 114 | break; 115 | case INTERVAL: 116 | length = IMU_ERROR_INTERVAL; 117 | break; 118 | } 119 | } 120 | else if (statusManager.hasStatus(Status::WIFI_CONNECTING)) 121 | { 122 | count = WIFI_CONNECTING_COUNT; 123 | switch (m_CurrentStage) 124 | { 125 | case ON: 126 | case OFF: 127 | length = WIFI_CONNECTING_LENGTH; 128 | break; 129 | case GAP: 130 | length = DEFAULT_GAP; 131 | break; 132 | case INTERVAL: 133 | length = WIFI_CONNECTING_INTERVAL; 134 | break; 135 | } 136 | } 137 | else if (statusManager.hasStatus(Status::SERVER_CONNECTING)) 138 | { 139 | count = SERVER_CONNECTING_COUNT; 140 | switch (m_CurrentStage) 141 | { 142 | case ON: 143 | case OFF: 144 | length = SERVER_CONNECTING_LENGTH; 145 | break; 146 | case GAP: 147 | length = DEFAULT_GAP; 148 | break; 149 | case INTERVAL: 150 | length = SERVER_CONNECTING_INTERVAL; 151 | break; 152 | } 153 | } 154 | else 155 | { 156 | #if defined(LED_INTERVAL_STANDBY) && LED_INTERVAL_STANDBY > 0 157 | count = 1; 158 | switch (m_CurrentStage) 159 | { 160 | case ON: 161 | case OFF: 162 | length = STANDBUY_LENGTH; 163 | break; 164 | case GAP: 165 | length = DEFAULT_GAP; 166 | break; 167 | case INTERVAL: 168 | length = LED_INTERVAL_STANDBY; 169 | break; 170 | } 171 | #else 172 | return; 173 | #endif 174 | } 175 | 176 | if (m_CurrentStage == OFF || m_Timer + diff >= length) 177 | { 178 | m_Timer = 0; 179 | // Advance stage 180 | switch (m_CurrentStage) 181 | { 182 | case OFF: 183 | on(); 184 | m_CurrentStage = ON; 185 | m_CurrentCount = 0; 186 | break; 187 | case ON: 188 | off(); 189 | m_CurrentCount++; 190 | if (m_CurrentCount >= count) 191 | { 192 | m_CurrentCount = 0; 193 | m_CurrentStage = INTERVAL; 194 | } 195 | else 196 | { 197 | m_CurrentStage = GAP; 198 | } 199 | break; 200 | case GAP: 201 | case INTERVAL: 202 | on(); 203 | m_CurrentStage = ON; 204 | break; 205 | } 206 | } 207 | else 208 | { 209 | m_Timer += diff; 210 | } 211 | } 212 | } 213 | --------------------------------------------------------------------------------